Add support to get link AABB from its collisions

Signed-off-by: Gabriel Pacheco <gabriel.fpacheco@gmail.com>

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,27 @@ namespace gz
                                   const math::Vector3d &_torque,
                                   const math::Vector3d &_offset) const;
 
+      /// \brief Get 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
+      /// geometries.
+      /// \param _ecm
+      /// \return Link's axis-aligned bounding box in the link frame or
+      /// nullopt if the link does not have collisions. It may return an
+      /// empty bounding box if all of the link collisions are empty.
+      public: std::optional<math::AxisAlignedBox> AxisAlignedBox(
+          const EntityComponentManager &_ecm) const;
+
+      /// \brief Get the link's axis-aligned bounding box in the world frame
+      /// \param _ecm Entity-component manager.
+      /// \return Link's axis-aligned bounding box in the world frame or
+      /// nullopt if the link does not have collisions. It may return an
+      /// empty bounding box if all of the link collisions are empty.
+      /// \sa AxisAlignedBox
+      public: std::optional<math::AxisAlignedBox> WorldAxisAlignedBox(
+          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>
 
@@ -323,6 +324,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"};
 

src/Link.cc

@@ -513,3 +513,57 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm,
       msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset);
   }
 }
+
+//////////////////////////////////////////////////
+std::optional<math::AxisAlignedBox> Link::AxisAlignedBox(
+  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;
+}
+
+//////////////////////////////////////////////////
+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()
+  );
+}

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

@@ -1032,3 +1032,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));
+}