diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index 24a85d0463..387bb86c43 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,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 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 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 ComputeAxisAlignedBox( + 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/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index f6c425fcb4..297d37e0ab 100644 --- a/python/src/gz/sim/Link.cc +++ b/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) { diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index d28c48d9e6..c90b26e49c 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,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 diff --git a/src/Link.cc b/src/Link.cc index b9535cd17b..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" @@ -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 Link::AxisAlignedBox( + 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); + + 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; +} 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..c88cc6f439 100644 --- a/src/Util_TEST.cc +++ b/src/Util_TEST.cc @@ -15,6 +15,10 @@ * */ +#include +#include +#include + #include #include #include @@ -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(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..82c3b6a934 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -48,6 +49,13 @@ #include #include +#include +#include +#include +#include +#include +#include + #include "../helpers/EnvTestFixture.hh" using namespace gz; @@ -698,3 +706,244 @@ 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.ComputeAxisAlignedBox(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()); + + // 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 re-enabled, 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()); + 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.ComputeAxisAlignedBox(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 (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)); + + // 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); + 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)); + + // 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.ComputeAxisAlignedBox(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.ComputeAxisAlignedBox(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); + link.EnableBoundingBoxChecks(ecm); + 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()); +}