From 40c03244652bbde08da30ac282fbd9bd4fce26a0 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Tue, 18 Feb 2025 13:24:58 -0300 Subject: [PATCH] Add support to get link AABB from its collisions Signed-off-by: Gabriel Pacheco --- include/gz/sim/Link.hh | 22 ++++ include/gz/sim/Util.hh | 15 +++ src/Link.cc | 54 ++++++++++ src/Util.cc | 28 +++++ src/Util_TEST.cc | 80 ++++++++++++++ test/integration/link.cc | 220 +++++++++++++++++++++++++++++++++++++++ 6 files changed, 419 insertions(+) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 24a85d0463..9ea5d95f63 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -28,6 +28,7 @@ #include #include #include +#include #include #include @@ -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 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 WorldAxisAlignedBox( + const EntityComponentManager &_ecm) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/include/gz/sim/Util.hh b/include/gz/sim/Util.hh index de0585e5c0..3bb06a537c 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -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"}; diff --git a/src/Link.cc b/src/Link.cc index 715e541766..f0bd5dc842 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -513,3 +513,57 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, msgs::Convert(linkWrenchComp->Data().torque()) + torqueWithOffset); } } + +////////////////////////////////////////////////// +std::optional 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(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(entity).value() + ); + } + + return linkAabb; +} + +////////////////////////////////////////////////// +std::optional 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() + ); +} diff --git a/src/Util.cc b/src/Util.cc index 4d9a459ed4..edaf0c7d38 100644 --- a/src/Util.cc +++ b/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()) + ); +} + } } } diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 9ec0e9be1a..6fa994060f 100644 --- a/src/Util_TEST.cc +++ b/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(std::rand()), + static_cast(std::rand()), + static_cast(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)); +} diff --git a/test/integration/link.cc b/test/integration/link.cc index c706c1135a..161993763b 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -48,6 +48,13 @@ #include #include +#include +#include +#include +#include +#include +#include + #include "../helpers/EnvTestFixture.hh" using namespace gz; @@ -698,3 +705,216 @@ TEST_F(LinkIntegrationTest, LinkAddWorldForce) EXPECT_EQ(math::Vector3d::Zero, math::Vector3d( wrenchMsg.torque().x(), wrenchMsg.torque().y(), wrenchMsg.torque().z())); } + +///////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) +{ + // linkA + // - collisionAA (box) + // - collisionAB (sphere) + // + // linkB + // - collisionBA (ellipsoid) + // - collisionBB (mesh) + // + // linkC + + gz::sim::EntityComponentManager ecm; + gz::sim::EventManager evm; + gz::sim::SdfEntityCreator creator(ecm, evm); + + sdf::Collision collision; + sdf::Geometry geom; + sdf::ElementPtr sdf(new sdf::Element()); + + // Link A + auto eLinkA = ecm.CreateEntity(); + ecm.CreateComponent(eLinkA, gz::sim::components::Link()); + ecm.CreateComponent(eLinkA, gz::sim::components::Name("linkA")); + + // Create box collision AA - Child of Link A + sdf::Box box; + box.SetSize(gz::math::Vector3d(2, 2, 2)); + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(2.0, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkA); + + // Create sphere collision AB - Child of Link A + sdf::Sphere sphere; + sphere.SetRadius(2.0); + geom.SetType(sdf::GeometryType::SPHERE); + geom.SetSphereShape(sphere); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(-3.0, 2.0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkA); + + // LinkA - bounding box from collisions + gz::sim::Link linkA(eLinkA); + auto aabb = linkA.AxisAlignedBox(ecm); + + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(-5.0, aabb->Min().X()); + EXPECT_EQ(-1.0, aabb->Min().Y()); + EXPECT_EQ(-2.0, aabb->Min().Z()); + EXPECT_EQ(3.0, aabb->Max().X()); + EXPECT_EQ(4.0, aabb->Max().Y()); + EXPECT_EQ(2.0, aabb->Max().Z()); + + // Link B + auto eLinkB = ecm.CreateEntity(); + ecm.CreateComponent(eLinkB, gz::sim::components::Link()); + ecm.CreateComponent(eLinkB, gz::sim::components::Name("linkB")); + + // Create ellipsoid collision BA - Child of Link B + sdf::Ellipsoid ellipsoid; + ellipsoid.SetRadii(gz::math::Vector3d(0.5, 0.6, 0.1)); + geom.SetType(sdf::GeometryType::ELLIPSOID); + geom.SetEllipsoidShape(ellipsoid); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(1, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkB); + + // Create mesh collision BB - Child of Link B + sdf::Mesh mesh; + mesh.SetUri("name://unit_box"); + geom.SetType(sdf::GeometryType::MESH); + geom.SetMeshShape(mesh); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(-1, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkB); + + // LinkB - bounding box from collisions + gz::sim::Link linkB(eLinkB); + aabb = linkB.AxisAlignedBox(ecm); + + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(-1.5, aabb->Min().X()); + EXPECT_EQ(-0.6, aabb->Min().Y()); + EXPECT_EQ(-0.5, aabb->Min().Z()); + EXPECT_EQ(1.5, aabb->Max().X()); + EXPECT_EQ(0.6, aabb->Max().Y()); + EXPECT_EQ(0.5, aabb->Max().Z()); + + // Link C + auto eLinkC = ecm.CreateEntity(); + ecm.CreateComponent(eLinkC, gz::sim::components::Link()); + ecm.CreateComponent(eLinkC, gz::sim::components::Name("linkC")); + + // LinkC - No collisions + gz::sim::Link linkC(eLinkC); + EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + + // Add invalid mesh collision to LinkC + sdf::Mesh meshInvalid; + meshInvalid.SetUri("invalid_uri"); + meshInvalid.SetFilePath("invalid_filepath"); + geom.SetType(sdf::GeometryType::MESH); + geom.SetMeshShape(meshInvalid); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkC); + + // LinkC - Invalid mesh is the only collision present, bounding + // box should be invalid (aab.Min > aab.Max) and a warning + // should be printed + linkC = gz::sim::Link(eLinkC); + EXPECT_EQ(math::AxisAlignedBox(), linkC.AxisAlignedBox(ecm)); + + // Add valid mesh collision to LinkC + mesh.SetUri("name://unit_box"); + geom.SetMeshShape(mesh); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLinkC); + + // LinkC - Invalid mesh will be skiped with warning, bounding + // box will be defined for the valid mesh only + linkC = gz::sim::Link(eLinkC); + + aabb = linkC.AxisAlignedBox(ecm); + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(-0.5, aabb->Min().X()); + EXPECT_EQ(-0.5, aabb->Min().Y()); + EXPECT_EQ(-0.5, aabb->Min().Z()); + EXPECT_EQ(0.5, aabb->Max().X()); + EXPECT_EQ(0.5, aabb->Max().Y()); + EXPECT_EQ(0.5, aabb->Max().Z()); + + // Reparenting invalid mesh collision from LinkC to LinkB + // should have the same effect as above but without a warning + auto eCollisions = ecm.ChildrenByComponents( + eLinkC, gz::sim::components::Collision()); + creator.SetParent(eCollisions[0], eLinkB); + + linkC = gz::sim::Link(eLinkC); + aabb = linkC.AxisAlignedBox(ecm); + + EXPECT_TRUE(aabb.has_value()); + EXPECT_EQ(-0.5, aabb->Min().X()); + EXPECT_EQ(-0.5, aabb->Min().Y()); + EXPECT_EQ(-0.5, aabb->Min().Z()); + EXPECT_EQ(0.5, aabb->Max().X()); + EXPECT_EQ(0.5, aabb->Max().Y()); + EXPECT_EQ(0.5, aabb->Max().Z()); +} + +///////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, AxisAlignedBoxInWorld) +{ + // model + // - link + // - collision (mesh) + + gz::sim::EntityComponentManager ecm; + gz::sim::EventManager evm; + gz::sim::SdfEntityCreator creator(ecm, evm); + + sdf::Collision collision; + sdf::Geometry geom; + sdf::ElementPtr sdf(new sdf::Element()); + + // Model with translation only pose wrt world + auto modelPoseInWorld = gz::math::Pose3d(1023, -932, 378, 0, 0, 0); + auto eModel = ecm.CreateEntity(); + ecm.CreateComponent(eModel, gz::sim::components::Model()); + ecm.CreateComponent(eModel, gz::sim::components::Name("model")); + ecm.CreateComponent(eModel, gz::sim::components::Pose(modelPoseInWorld)); + + // Link with translation only pose wrt model + auto linkPoseInModel = gz::math::Pose3d(0.1, -0.2, 0.3, 0, 0, 0); + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, gz::sim::components::Link()); + ecm.CreateComponent(eLink, gz::sim::components::Name("link")); + ecm.CreateComponent(eLink, gz::sim::components::Pose(linkPoseInModel)); + creator.SetParent(eLink, eModel); + + // Create mesh collision - Child of Link + sdf::Mesh mesh; + mesh.SetUri("name://unit_box"); + geom.SetType(sdf::GeometryType::MESH); + geom.SetMeshShape(mesh); + collision.Load(sdf); + collision.SetGeom(geom); + collision.SetRawPose(gz::math::Pose3d(0, 0, 0, 0, 0, 0)); + creator.SetParent(creator.CreateEntities(&collision), eLink); + + // Model - bounding box from link collision + gz::sim::Link link(eLink); + auto aabbInWorld = link.WorldAxisAlignedBox(ecm); + + EXPECT_TRUE(aabbInWorld.has_value()); + EXPECT_EQ(1023 + 0.1 - 0.5, aabbInWorld->Min().X()); + EXPECT_EQ(1023 + 0.1 + 0.5, aabbInWorld->Max().X()); + EXPECT_EQ(-932 - 0.2 - 0.5, aabbInWorld->Min().Y()); + EXPECT_EQ(-932 - 0.2 + 0.5, aabbInWorld->Max().Y()); + EXPECT_EQ(378 + 0.3 - 0.5, aabbInWorld->Min().Z()); + EXPECT_EQ(378 + 0.3 + 0.5, aabbInWorld->Max().Z()); +}