Add support to get link AABB from its collisions

include/gz/sim/Link.hh

@@ -28,6 +28,7 @@
 #include <gz/math/Pose3.hh>
 #include <gz/math/Quaternion.hh>
 #include <gz/math/Vector3.hh>
+#include <gz/math/AxisAlignedBox.hh>
 
 #include <gz/sim/config.hh>
 #include <gz/sim/EntityComponentManager.hh>
@@ -356,6 +357,46 @@ namespace gz
                                   const math::Vector3d &_torque,
                                   const math::Vector3d &_offset) const;
 
+      /// \brief By default, Gazebo will not report bounding box for a link, so
+      /// functions like `AxisAlignedBox` and `WorldAxisAlignedBox` will return
+      /// nullopt. This function can be used to enable bounding box checks and
+      /// it also initializes the bounding box based on the link's collision
+      /// shapes.
+      /// \param[in] _ecm Mutable reference to the ECM.
+      /// \param[in] _enable True to enable checks, false to disable. Defaults
+      /// to true.
+      public: void EnableBoundingBoxChecks(
+          EntityComponentManager &_ecm,
+          bool _enable = true) const;
+
+      /// \brief Get the link's axis-aligned bounding box in the link frame.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Link's AxisAlignedBox in the link frame or nullopt if the
+      /// link entity does not have a components::AxisAlignedBox component.
+      /// \sa EnableBoundingBoxChecks
+      public: std::optional<math::AxisAlignedBox> AxisAlignedBox(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the link's axis-aligned bounding box in the world frame.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Link's AxisAlignedBox in the world frame or nullopt if the
+      /// link entity does not have a components::AxisAlignedBox component.
+      /// \sa AxisAlignedBox
+      public: std::optional<math::AxisAlignedBox> WorldAxisAlignedBox(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Compute the axis-aligned bounding box of the link.
+      /// It generates an axis-aligned bounding box for a link based on the
+      /// collision shapes attached to the it. The link bounding box is
+      /// generated by merging all the bounding boxes of the collision
+      /// geometry shapes attached to the link.
+      /// \param[in] _ecm Entity-component manager.
+      /// \return Link's axis-aligned bounding box in the link frame or
+      /// nullopt if the link does not have collisions. It returns an
+      /// invalid bounding box if all of the link collisions are empty.
+      public: std::optional<math::AxisAlignedBox> ComputeAxisAlignedBox(
+        const EntityComponentManager &_ecm) const;
+
       /// \brief Pointer to private data.
       private: std::unique_ptr<LinkPrivate> dataPtr;
     };

include/gz/sim/Util.hh

@@ -25,6 +25,7 @@
 #include <vector>
 
 #include <gz/common/Mesh.hh>
+#include <gz/math/AxisAlignedBox.hh>
 #include <gz/math/Pose3.hh>
 #include <sdf/Mesh.hh>
 
@@ -327,6 +328,20 @@ namespace gz
     GZ_SIM_VISIBLE const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
         const common::Mesh &_mesh);
 
+    /// \brief Transform an axis-aligned bounding box by a pose.
+    /// \param[in] _aabb Axis-aligned bounding box to transform.
+    /// \param[in] _pose Pose to transform the bounding box by.
+    /// \return The axis-aligned bounding box in the pose target frame.
+    GZ_SIM_VISIBLE math::AxisAlignedBox transformAxisAlignedBox(
+      const math::AxisAlignedBox & _aabb,
+      const math::Pose3d & _pose);
+
+    /// \brief Compute the axis-aligned bounding box of a mesh.
+    /// \param _sdfMesh Mesh SDF DOM.
+    /// \return The AABB of the mesh in its local frame.
+    GZ_SIM_VISIBLE math::AxisAlignedBox meshAxisAlignedBox(
+      const sdf::Mesh _sdfMesh);
+
     /// \brief Environment variable holding resource paths.
     const std::string kResourcePathEnv{"GZ_SIM_RESOURCE_PATH"};
 

python/src/gz/sim/Link.cc

@@ -191,6 +191,27 @@ void defineSimLink(py::object module)
       "Add a wrench expressed in world coordinates and applied to "
       "the link at an offset from the link's origin. This wrench is applied "
       "for one simulation step.")
+  .def("enable_bounding_box_checks", &gz::sim::Link::EnableBoundingBoxChecks,
+      py::arg("ecm"),
+      py::arg("enable") = true,
+      "By default, Gazebo will not report bounding box for a link, so "
+      "functions like `axis_aligned_box` and `world_axis_aligned_box` will "
+      "return nullopt. This function can be used to enable bounding box checks "
+      "and it also initializes the bounding box based on the link's collision "
+      "shapes.")
+  .def("axis_aligned_box", &gz::sim::Link::AxisAlignedBox,
+      py::arg("ecm"),
+      "Get the Link's axis-aligned box represented in the link frame.")
+  .def("world_axis_aligned_box", &gz::sim::Link::WorldAxisAlignedBox,
+      py::arg("ecm"),
+      "Get the Link's axis-aligned box represented in the world frame.")
+  .def("compute_axis_aligned_box", &gz::sim::Link::ComputeAxisAlignedBox,
+      py::arg("ecm"),
+      "Compute the Link's axis-aligned box represented in the link frame. "
+      "It generates an axis-aligned bounding box for a link based on the "
+      "collision shapes attached to the it. The link bounding box is "
+      "generated by merging all the bounding boxes of the collision "
+      "geometry shapes attached to the link.")
   .def("__copy__",
       [](const gz::sim::Link &self)
       {

python/test/link_TEST.py

@@ -18,7 +18,7 @@
 
 from gz_test_deps.common import set_verbosity
 from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity
-from gz_test_deps.math import Inertiald, Matrix3d, Vector3d, Pose3d
+from gz_test_deps.math import AxisAlignedBox, Inertiald, Matrix3d, Vector3d, Pose3d
 
 class TestModel(unittest.TestCase):
     post_iterations = 0
@@ -88,7 +88,27 @@ def on_pre_udpate_cb(_info, _ecm):
             self.assertEqual(0, link.world_kinetic_energy(_ecm))
             link.enable_velocity_checks(_ecm, False)
             link.enable_acceleration_checks(_ecm, False)
-
+            # Compute Axis Aligned Box Test
+            # Offset of 0.5 meters along z-axis
+            self.assertEqual(
+                AxisAlignedBox(Vector3d(-0.5, -0.5, 0), Vector3d(0.5, 0.5, 1)),
+                link.compute_axis_aligned_box(_ecm)
+            )
+            # Axis Aligned Box Test with disabled bounding box checks
+            link.enable_bounding_box_checks(_ecm, False)
+            self.assertEqual(None, link.axis_aligned_box(_ecm))
+            self.assertEqual(None, link.world_axis_aligned_box(_ecm))
+            # Axis Aligned Box Test with enabled bounding box checks
+            link.enable_bounding_box_checks(_ecm, True)
+            self.assertEqual(
+                AxisAlignedBox(Vector3d(-0.5, -0.5, 0), Vector3d(0.5, 0.5, 1)),
+                link.axis_aligned_box(_ecm)
+            )
+            # Same as above since the link is at the world origin
+            self.assertEqual(
+                AxisAlignedBox(Vector3d(-0.5, -0.5, 0), Vector3d(0.5, 0.5, 1)),
+                link.world_axis_aligned_box(_ecm)
+            )
 
         def on_udpate_cb(_info, _ecm):
             self.iterations += 1

src/Link.cc

@@ -24,6 +24,7 @@
 #include "gz/sim/components/AngularAcceleration.hh"
 #include "gz/sim/components/AngularVelocity.hh"
 #include "gz/sim/components/AngularVelocityCmd.hh"
+#include "gz/sim/components/AxisAlignedBox.hh"
 #include "gz/sim/components/CanonicalLink.hh"
 #include "gz/sim/components/Collision.hh"
 #include "gz/sim/components/ExternalWorldWrenchCmd.hh"
@@ -534,3 +535,81 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
       msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset);
   }
 }
+
+//////////////////////////////////////////////////
+void Link::EnableBoundingBoxChecks(
+  gz::sim::EntityComponentManager & _ecm,
+  bool _enable) const
+{
+  math::AxisAlignedBox linkAabb;
+  if (_enable)
+  {
+    // Compute link's AABB from its collision shapes for proper initialization
+    linkAabb = this->ComputeAxisAlignedBox(_ecm).value_or(
+      math::AxisAlignedBox());
+  }
+
+  enableComponent(_ecm, this->dataPtr->id, _enable,
+    components::AxisAlignedBox(linkAabb));
+}
+
+//////////////////////////////////////////////////
+std::optional<math::AxisAlignedBox> Link::AxisAlignedBox(
+  const gz::sim::EntityComponentManager & _ecm) const
+{
+  return _ecm.ComponentData<components::AxisAlignedBox>(this->dataPtr->id);
+}
+
+//////////////////////////////////////////////////
+std::optional<math::AxisAlignedBox> Link::WorldAxisAlignedBox(
+  const gz::sim::EntityComponentManager & _ecm) const
+{
+  auto linkAabb = this->AxisAlignedBox(_ecm);
+
+  if (!linkAabb.has_value())
+  {
+    return std::nullopt;
+  }
+
+  // Return the link AABB in the world frame
+  return sim::transformAxisAlignedBox(
+    linkAabb.value(),
+    this->WorldPose(_ecm).value()
+  );
+}
+
+//////////////////////////////////////////////////
+std::optional<math::AxisAlignedBox> Link::ComputeAxisAlignedBox(
+  const gz::sim::EntityComponentManager & _ecm) const
+{
+  math::AxisAlignedBox linkAabb;
+  auto collisions = this->Collisions(_ecm);
+
+  if (collisions.empty())
+  {
+    return std::nullopt;
+  }
+
+  for (auto & entity : collisions)
+  {
+    auto collision = _ecm.ComponentData<components::CollisionElement>(entity);
+    auto geom = collision.value().Geom();
+    auto geomAabb = geom->AxisAlignedBox(&sim::meshAxisAlignedBox);
+
+    if (!geomAabb.has_value() || geomAabb == math::AxisAlignedBox())
+    {
+      gzwarn << "Failed to get bounding box for collision entity ["
+             << entity << "]. It will be ignored in the computation "
+             << "of the link bounding box." << std::endl;
+      continue;
+    }
+
+    // Merge geometry AABB (expressed in link frame) into link AABB
+    linkAabb += sim::transformAxisAlignedBox(
+      geomAabb.value(),
+      _ecm.ComponentData<components::Pose>(entity).value()
+    );
+  }
+
+  return linkAabb;
+}

src/Util.cc

@@ -989,6 +989,34 @@ const common::Mesh *optimizeMesh(const sdf::Mesh &_meshSdf,
   return optimizedMesh;
 }
 
+math::AxisAlignedBox meshAxisAlignedBox(sdf::Mesh _sdfMesh)
+{
+  auto mesh = loadMesh(_sdfMesh);
+  if (!mesh)
+  {
+    gzwarn << "Mesh could not be loaded. Invalidating its bounding box."
+           << std::endl;
+
+    return math::AxisAlignedBox();
+  }
+
+  // Get the mesh's bounding box
+  math::Vector3d meshCenter, meshMin, meshMax;
+  mesh->AABB(meshCenter, meshMin, meshMax);
+
+  return math::AxisAlignedBox(meshMin, meshMax);
+}
+
+math::AxisAlignedBox transformAxisAlignedBox(
+  const math::AxisAlignedBox & _aabb,
+  const math::Pose3d & _pose)
+{
+  return math::AxisAlignedBox(
+    _pose.CoordPositionAdd(_aabb.Min()),
+    _pose.CoordPositionAdd(_aabb.Max())
+  );
+}
+
 }
 }
 }

src/Util_TEST.cc

@@ -15,6 +15,10 @@
  *
 */
 
+#include <string>
+#include <cstdlib>
+#include <ctime>
+
 #include <gtest/gtest.h>
 #include <gz/common/Console.hh>
 #include <sdf/Actor.hh>
@@ -1042,3 +1046,83 @@ TEST_F(UtilTest, LoadMesh)
   EXPECT_NE(nullptr, optimizedMesh);
   EXPECT_EQ(16u, optimizedMesh->SubMeshCount());
 }
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, MeshAxisAlignedBoundingBox)
+{
+  sdf::Mesh meshSdf;
+  math::AxisAlignedBox emptyBox, aab;
+  EXPECT_EQ(emptyBox, meshAxisAlignedBox(meshSdf));
+
+  meshSdf.SetUri("invalid_uri");
+  meshSdf.SetFilePath("invalid_filepath");
+  EXPECT_EQ(emptyBox, meshAxisAlignedBox(meshSdf));
+
+  meshSdf.SetUri("name://unit_box");
+  aab = meshAxisAlignedBox(meshSdf);
+  EXPECT_NE(emptyBox, aab);
+  EXPECT_EQ(aab.Size(), math::Vector3d::One);
+  EXPECT_EQ(aab.Min(), math::Vector3d(-0.5, -0.5, -0.5));
+  EXPECT_EQ(aab.Max(), math::Vector3d(0.5, 0.5, 0.5));
+  EXPECT_EQ(aab.Center(), math::Vector3d::Zero);
+
+  meshSdf.SetUri("duck.dae");
+  std::string filePath = common::joinPaths(std::string(PROJECT_SOURCE_PATH),
+    "test", "media", "duck.dae");
+  meshSdf.SetFilePath(filePath);
+  aab = meshAxisAlignedBox(meshSdf);
+  EXPECT_NE(emptyBox, aab);
+
+  // Expected values obtained from the mesh file using Blender:
+  auto minExp = math::Vector3d(-69.2985, 9.92937, -61.3282) * 1e-2;
+  auto maxExp = math::Vector3d(96.1799, 163.9700, 53.9252) * 1e-2;
+
+  EXPECT_EQ(minExp, aab.Min());
+  EXPECT_EQ(maxExp, aab.Max());
+  EXPECT_EQ((minExp + maxExp) / 2, aab.Center());
+  EXPECT_EQ(maxExp - minExp, aab.Size());
+}
+
+/////////////////////////////////////////////////
+TEST_F(UtilTest, TransformAxisAlignedBoxFrame)
+{
+  // Creates a pseudo-random vector with values between -max and max
+  std::srand(std::time(nullptr));
+  auto randomVector3d = [](int max = 1) -> math::Vector3d
+  {
+    return (
+      math::Vector3d(
+        static_cast<double>(std::rand()),
+        static_cast<double>(std::rand()),
+        static_cast<double>(std::rand())
+      ) / RAND_MAX - 0.5 * math::Vector3d::One
+    ) * 2 * max;
+  };
+
+  math::AxisAlignedBox aab(randomVector3d(3), randomVector3d(2));
+
+  // Trivial case: identity transform should not change the box
+  math::Pose3d transform = math::Pose3d::Zero;
+  math::AxisAlignedBox aabExp = aab;
+  EXPECT_EQ(aabExp, transformAxisAlignedBox(aab, transform));
+
+  // Pure translation offset
+  transform.Pos() = randomVector3d(10);
+  aabExp = aab + transform.Pos();
+  EXPECT_EQ(aabExp, transformAxisAlignedBox(aab, transform));
+
+  // Pure rotation of 90 degrees around the z-axis
+  // x -> y, y -> -x, z -> z
+  transform.Reset();
+  transform.Rot() = math::Quaterniond(0, 0, GZ_PI_2);
+  aabExp = math::AxisAlignedBox(
+    math::Vector3d(-aab.Max().Y(), aab.Min().X(), aab.Min().Z()),
+    math::Vector3d(-aab.Min().Y(), aab.Max().X(), aab.Max().Z())
+  );
+  EXPECT_EQ(aabExp, transformAxisAlignedBox(aab, transform));
+
+  // Full transform: translation and rotation
+  transform.Pos() = randomVector3d(150);
+  aabExp = aabExp + transform.Pos();
+  EXPECT_EQ(aabExp, transformAxisAlignedBox(aab, transform));
+}