From 61b2e1a33e42f6a6f763ad71747434e7d4d3d8d7 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Tue, 18 Feb 2025 13:24:58 -0300 Subject: [PATCH 1/7] 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 18573c72e7..c614304d85 100644 --- a/include/gz/sim/Util.hh +++ b/include/gz/sim/Util.hh @@ -25,6 +25,7 @@ #include #include +#include #include #include @@ -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"}; diff --git a/src/Link.cc b/src/Link.cc index b9535cd17b..afec21dea2 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -534,3 +534,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 414e262c33..6d427820e1 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -1042,3 +1042,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()); +} From dca7e4a12d74e7205aa1dce45c6a5d8e8f27c6a0 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Wed, 19 Feb 2025 20:03:43 -0300 Subject: [PATCH 2/7] Add python bindings to new methods and corresponding tests Signed-off-by: Gabriel Pacheco --- python/src/gz/sim/Link.cc | 6 ++++++ python/test/link_TEST.py | 14 +++++++++++++- 2 files changed, 19 insertions(+), 1 deletion(-) diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index f6c425fcb4..c88d1b954e 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -191,6 +191,12 @@ 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("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("__copy__", [](const gz::sim::Link &self) { diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index d28c48d9e6..cf5858c1e5 100755 --- a/python/test/link_TEST.py +++ b/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,6 +88,18 @@ 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) + # 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.axis_aligned_box(_ecm) + ) + # World Axis Aligned Box Test + # Same as above since the link is at the 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): From 4aa7981e725e3f087c9a442de2ccb47bc26aea82 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Thu, 20 Feb 2025 13:02:08 -0300 Subject: [PATCH 3/7] Fix and improve new methods API documentation Signed-off-by: Gabriel Pacheco --- include/gz/sim/Link.hh | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 9ea5d95f63..c809843fb5 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -362,18 +362,18 @@ namespace gz /// collision shapes attached to the it. The link bounding box is /// generated by merging all the bounding boxes of the collision /// geometries. - /// \param _ecm + /// \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 may return an - /// empty bounding box if all of the link collisions are empty. + /// 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 AxisAlignedBox( const EntityComponentManager &_ecm) const; /// \brief Get the link's axis-aligned bounding box in the world frame - /// \param _ecm Entity-component manager. + /// \param[in] _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. + /// nullopt if the link does not have collisions. It returns an + /// invalid bounding box if all of the link collisions are empty. /// \sa AxisAlignedBox public: std::optional WorldAxisAlignedBox( const EntityComponentManager &_ecm) const; From 2c02ec1c9140df5d52f16072b9d7761604bfb357 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Thu, 20 Feb 2025 13:02:29 -0300 Subject: [PATCH 4/7] Include missing used headers Signed-off-by: Gabriel Pacheco --- src/Util_TEST.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/Util_TEST.cc b/src/Util_TEST.cc index 6d427820e1..c88cc6f439 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include From 2885b7ae8d9d8bc6e54a9efa6665a7de2ec907ab Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Mon, 10 Mar 2025 13:10:50 -0300 Subject: [PATCH 5/7] Update axis-aligned box computation to better fit overall architecture * Following https://github.com/gazebosim/gz-sim/issues/2353#issuecomment-2691376533 * This allows to use the ECM as cache if bounding box doesn't have to be recomputed and is more suitable so that other systems can determine the AABB if necessary (e.g. physics system if checks are enabled) Signed-off-by: Gabriel Pacheco --- include/gz/sim/Link.hh | 41 ++++++++++++++++------- python/src/gz/sim/Link.cc | 8 +++++ python/test/link_TEST.py | 1 + src/Link.cc | 69 ++++++++++++++++++++++++++------------- test/integration/link.cc | 31 ++++++++++++++++-- 5 files changed, 114 insertions(+), 36 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index c809843fb5..5d74840421 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -357,27 +357,44 @@ 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. + /// \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 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. + /// \return Link's AxisAlignedBox in the link frame or nullopt + /// \sa EnableBoundingBoxChecks public: std::optional AxisAlignedBox( const EntityComponentManager &_ecm) const; - /// \brief Get the link's axis-aligned bounding box in the world frame + /// \brief Get the link's axis-aligned bounding box in the world frame. /// \param[in] _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 returns an - /// invalid bounding box if all of the link collisions are empty. + /// \return Link's AxisAlignedBox in the world frame or nullopt /// \sa AxisAlignedBox public: std::optional 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. + private: std::optional ComputeAxisAlignedBox( + const EntityComponentManager &_ecm) const; + /// \brief Pointer to private data. private: std::unique_ptr dataPtr; }; diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index c88d1b954e..151e8afdae 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -191,6 +191,14 @@ 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.") diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index cf5858c1e5..eefe962900 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -89,6 +89,7 @@ def on_pre_udpate_cb(_info, _ecm): link.enable_velocity_checks(_ecm, False) link.enable_acceleration_checks(_ecm, False) # Axis Aligned Box Test + link.enable_bounding_box_checks(_ecm, True) # Offset of 0.5 meters along z-axis self.assertEqual( AxisAlignedBox(Vector3d(-0.5, -0.5, 0), Vector3d(0.5, 0.5, 1)), diff --git a/src/Link.cc b/src/Link.cc index afec21dea2..a40dc2bec9 100644 --- a/src/Link.cc +++ b/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" @@ -535,12 +536,54 @@ void Link::AddWorldWrench(EntityComponentManager &_ecm, } } +////////////////////////////////////////////////// +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 Link::AxisAlignedBox( - const gz::sim::EntityComponentManager & ecm) const + const gz::sim::EntityComponentManager & _ecm) const +{ + return _ecm.ComponentData(this->dataPtr->id); +} + +////////////////////////////////////////////////// +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() + ); +} + +////////////////////////////////////////////////// +std::optional Link::ComputeAxisAlignedBox( + const gz::sim::EntityComponentManager & _ecm) const { math::AxisAlignedBox linkAabb; - auto collisions = this->Collisions(ecm); + auto collisions = this->Collisions(_ecm); if (collisions.empty()) { @@ -549,7 +592,7 @@ std::optional Link::AxisAlignedBox( for (auto & entity : collisions) { - auto collision = ecm.ComponentData(entity); + auto collision = _ecm.ComponentData(entity); auto geom = collision.value().Geom(); auto geomAabb = geom->AxisAlignedBox(&sim::meshAxisAlignedBox); @@ -564,27 +607,9 @@ std::optional Link::AxisAlignedBox( // Merge geometry AABB (expressed in link frame) into link AABB linkAabb += sim::transformAxisAlignedBox( geomAabb.value(), - ecm.ComponentData(entity).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/test/integration/link.cc b/test/integration/link.cc index 161993763b..8fcc8f23d0 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -754,6 +755,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkA - bounding box from collisions gz::sim::Link linkA(eLinkA); + linkA.EnableBoundingBoxChecks(ecm); auto aabb = linkA.AxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); @@ -764,6 +766,18 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) EXPECT_EQ(4.0, aabb->Max().Y()); EXPECT_EQ(2.0, aabb->Max().Z()); + // If ECM is updated with new link AABB, the ECM value is returned + math::AxisAlignedBox newAabb(-math::Vector3d::One, math::Vector3d::One); + ecm.SetComponentData(eLinkA, newAabb); + EXPECT_EQ(newAabb, linkA.AxisAlignedBox(ecm)); + + // If the bounding box checks are disabled and then enabled again, the + // bounding box should be recalculated from the collisions + linkA.EnableBoundingBoxChecks(ecm, false); + EXPECT_EQ(std::nullopt, linkA.AxisAlignedBox(ecm)); + linkA.EnableBoundingBoxChecks(ecm); + EXPECT_EQ(aabb, linkA.AxisAlignedBox(ecm)); + // Link B auto eLinkB = ecm.CreateEntity(); ecm.CreateComponent(eLinkB, gz::sim::components::Link()); @@ -791,6 +805,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkB - bounding box from collisions gz::sim::Link linkB(eLinkB); + linkB.EnableBoundingBoxChecks(ecm); aabb = linkB.AxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); @@ -806,9 +821,11 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) ecm.CreateComponent(eLinkC, gz::sim::components::Link()); ecm.CreateComponent(eLinkC, gz::sim::components::Name("linkC")); - // LinkC - No collisions + // LinkC - No collisions (AABB should be invalid if checks are enabled) gz::sim::Link linkC(eLinkC); EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + linkC.EnableBoundingBoxChecks(ecm); + EXPECT_EQ(math::AxisAlignedBox(), linkC.AxisAlignedBox(ecm)); // Add invalid mesh collision to LinkC sdf::Mesh meshInvalid; @@ -825,6 +842,9 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // box should be invalid (aab.Min > aab.Max) and a warning // should be printed linkC = gz::sim::Link(eLinkC); + linkC.EnableBoundingBoxChecks(ecm, false); + EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + linkC.EnableBoundingBoxChecks(ecm); EXPECT_EQ(math::AxisAlignedBox(), linkC.AxisAlignedBox(ecm)); // Add valid mesh collision to LinkC @@ -838,8 +858,11 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkC - Invalid mesh will be skiped with warning, bounding // box will be defined for the valid mesh only linkC = gz::sim::Link(eLinkC); - + linkC.EnableBoundingBoxChecks(ecm, false); + EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + linkC.EnableBoundingBoxChecks(ecm); aabb = linkC.AxisAlignedBox(ecm); + EXPECT_TRUE(aabb.has_value()); EXPECT_EQ(-0.5, aabb->Min().X()); EXPECT_EQ(-0.5, aabb->Min().Y()); @@ -855,6 +878,9 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) creator.SetParent(eCollisions[0], eLinkB); linkC = gz::sim::Link(eLinkC); + linkC.EnableBoundingBoxChecks(ecm, false); + EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + linkC.EnableBoundingBoxChecks(ecm); aabb = linkC.AxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); @@ -908,6 +934,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInWorld) // Model - bounding box from link collision gz::sim::Link link(eLink); + link.EnableBoundingBoxChecks(ecm); auto aabbInWorld = link.WorldAxisAlignedBox(ecm); EXPECT_TRUE(aabbInWorld.has_value()); From 0c79183ef296a031bb9a4170eb2b9b3e91312220 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Tue, 11 Mar 2025 14:42:49 -0300 Subject: [PATCH 6/7] Make link AABB computation from collision shapes public This could be helpful if users want to explicetly compute the AABB from the link SDF collision shapes. For instance, the Physics system can fallback and use this method in case the selected engine does not support AABB calculation. Signed-off-by: Gabriel Pacheco --- include/gz/sim/Link.hh | 8 +++++--- test/integration/link.cc | 28 +++++++++++++++------------- 2 files changed, 20 insertions(+), 16 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 5d74840421..387bb86c43 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -371,14 +371,16 @@ namespace gz /// \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 + /// \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 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 + /// \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 WorldAxisAlignedBox( const EntityComponentManager &_ecm) const; @@ -392,7 +394,7 @@ namespace gz /// \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. - private: std::optional ComputeAxisAlignedBox( + public: std::optional ComputeAxisAlignedBox( const EntityComponentManager &_ecm) const; /// \brief Pointer to private data. diff --git a/test/integration/link.cc b/test/integration/link.cc index 8fcc8f23d0..82c3b6a934 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -755,8 +755,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkA - bounding box from collisions gz::sim::Link linkA(eLinkA); - linkA.EnableBoundingBoxChecks(ecm); - auto aabb = linkA.AxisAlignedBox(ecm); + auto aabb = linkA.ComputeAxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); EXPECT_EQ(-5.0, aabb->Min().X()); @@ -766,12 +765,20 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) EXPECT_EQ(4.0, aabb->Max().Y()); EXPECT_EQ(2.0, aabb->Max().Z()); + // By default, the bounding box checks are disabled. + // Once enabled, the result must match the computed AABB. + EXPECT_EQ(std::nullopt, linkA.AxisAlignedBox(ecm)); + linkA.EnableBoundingBoxChecks(ecm); + EXPECT_EQ(aabb, linkA.AxisAlignedBox(ecm)); + // If ECM is updated with new link AABB, the ECM value is returned + // instead of recomputing the AABB from the collisions math::AxisAlignedBox newAabb(-math::Vector3d::One, math::Vector3d::One); ecm.SetComponentData(eLinkA, newAabb); + EXPECT_NE(newAabb, aabb.value()); EXPECT_EQ(newAabb, linkA.AxisAlignedBox(ecm)); - // If the bounding box checks are disabled and then enabled again, the + // If the bounding box checks are disabled and then re-enabled, the // bounding box should be recalculated from the collisions linkA.EnableBoundingBoxChecks(ecm, false); EXPECT_EQ(std::nullopt, linkA.AxisAlignedBox(ecm)); @@ -805,8 +812,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkB - bounding box from collisions gz::sim::Link linkB(eLinkB); - linkB.EnableBoundingBoxChecks(ecm); - aabb = linkB.AxisAlignedBox(ecm); + aabb = linkB.ComputeAxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); EXPECT_EQ(-1.5, aabb->Min().X()); @@ -824,6 +830,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkC - No collisions (AABB should be invalid if checks are enabled) gz::sim::Link linkC(eLinkC); EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + EXPECT_EQ(std::nullopt, linkC.ComputeAxisAlignedBox(ecm)); linkC.EnableBoundingBoxChecks(ecm); EXPECT_EQ(math::AxisAlignedBox(), linkC.AxisAlignedBox(ecm)); @@ -844,6 +851,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) linkC = gz::sim::Link(eLinkC); linkC.EnableBoundingBoxChecks(ecm, false); EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); + EXPECT_EQ(math::AxisAlignedBox(), linkC.ComputeAxisAlignedBox(ecm)); linkC.EnableBoundingBoxChecks(ecm); EXPECT_EQ(math::AxisAlignedBox(), linkC.AxisAlignedBox(ecm)); @@ -858,10 +866,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) // LinkC - Invalid mesh will be skiped with warning, bounding // box will be defined for the valid mesh only linkC = gz::sim::Link(eLinkC); - linkC.EnableBoundingBoxChecks(ecm, false); - EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); - linkC.EnableBoundingBoxChecks(ecm); - aabb = linkC.AxisAlignedBox(ecm); + aabb = linkC.ComputeAxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); EXPECT_EQ(-0.5, aabb->Min().X()); @@ -878,10 +883,7 @@ TEST_F(LinkIntegrationTest, AxisAlignedBoxInLink) creator.SetParent(eCollisions[0], eLinkB); linkC = gz::sim::Link(eLinkC); - linkC.EnableBoundingBoxChecks(ecm, false); - EXPECT_EQ(std::nullopt, linkC.AxisAlignedBox(ecm)); - linkC.EnableBoundingBoxChecks(ecm); - aabb = linkC.AxisAlignedBox(ecm); + aabb = linkC.ComputeAxisAlignedBox(ecm); EXPECT_TRUE(aabb.has_value()); EXPECT_EQ(-0.5, aabb->Min().X()); From 2168cff89ef171553b26d21b8694eaa284b3a3d6 Mon Sep 17 00:00:00 2001 From: Gabriel Pacheco Date: Tue, 11 Mar 2025 15:30:08 -0300 Subject: [PATCH 7/7] Add python bindings to new public method Signed-off-by: Gabriel Pacheco --- python/src/gz/sim/Link.cc | 7 +++++++ python/test/link_TEST.py | 17 ++++++++++++----- 2 files changed, 19 insertions(+), 5 deletions(-) diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 151e8afdae..297d37e0ab 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -205,6 +205,13 @@ void defineSimLink(py::object module) .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) { diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index eefe962900..c90b26e49c 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -88,21 +88,28 @@ 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) - # Axis Aligned Box Test - link.enable_bounding_box_checks(_ecm, True) + # 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) ) - # World Axis Aligned Box Test - # Same as above since the link is at the origin + # 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