diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 10cdc57ec1..8d0dca96ad 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -92,6 +92,7 @@ #include #include "gz/sim/EntityComponentManager.hh" +#include "gz/sim/Link.hh" #include "gz/sim/Model.hh" #include "gz/sim/System.hh" #include "gz/sim/Util.hh" @@ -342,6 +343,14 @@ class gz::sim::systems::PhysicsPrivate /// \param[in] _world The world to disable it for. public: void DisableContactSurfaceCustomization(const Entity &_world); + /// \brief Update the AxisAlignedBox for link entities. + /// \param[in] _ecm The entity component manager. + public: void UpdateLinksBoundingBoxes(EntityComponentManager &_ecm); + + /// \brief Update the AxisAlignedBox for model entities. + /// \param[in] _ecm The entity component manager. + public: void UpdateModelsBoundingBoxes(EntityComponentManager &_ecm); + /// \brief Cache the top-level model for each entity. /// The key is an entity and the value is its top level model. public: std::unordered_map topLevelModelMap; @@ -554,12 +563,18 @@ class gz::sim::systems::PhysicsPrivate ////////////////////////////////////////////////// - // Bounding box + // Model Bounding box /// \brief Feature list for model bounding box. - public: struct BoundingBoxFeatureList : physics::FeatureList< + public: struct ModelBoundingBoxFeatureList : physics::FeatureList< MinimumFeatureList, physics::GetModelBoundingBox>{}; + ////////////////////////////////////////////////// + // Link Bounding box + /// \brief Feature list for model bounding box. + public: struct LinkBoundingBoxFeatureList : physics::FeatureList< + MinimumFeatureList, + physics::GetLinkBoundingBox>{}; ////////////////////////////////////////////////// // Joint velocity command @@ -679,7 +694,7 @@ class gz::sim::systems::PhysicsPrivate physics::Model, MinimumFeatureList, JointFeatureList, - BoundingBoxFeatureList, + ModelBoundingBoxFeatureList, NestedModelFeatureList, ConstructSdfLinkFeatureList, ConstructSdfJointFeatureList>; @@ -696,7 +711,8 @@ class gz::sim::systems::PhysicsPrivate CollisionFeatureList, HeightmapFeatureList, LinkForceFeatureList, - MeshFeatureList>; + MeshFeatureList, + LinkBoundingBoxFeatureList>; /// \brief A map between link entity ids in the ECM to Link Entities in /// gz-physics. @@ -2836,46 +2852,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm) }); // Populate bounding box info - // Only compute bounding box if component exists to avoid unnecessary - // computations - _ecm.Each( - [&](const Entity &_entity, const components::Model *, - components::AxisAlignedBox *_bbox) - { - if (!this->entityModelMap.HasEntity(_entity)) - { - gzwarn << "Failed to find model [" << _entity << "]." << std::endl; - return true; - } - - auto bbModel = - this->entityModelMap.EntityCast(_entity); - - if (!bbModel) - { - static bool informed{false}; - if (!informed) - { - gzdbg << "Attempting to get a bounding box, but the physics " - << "engine doesn't support feature " - << "[GetModelBoundingBox]. Bounding box won't be populated." - << std::endl; - informed = true; - } - - // Break Each call since no AxisAlignedBox'es can be processed - return false; - } - - math::AxisAlignedBox bbox = - math::eigen3::convert(bbModel->GetAxisAlignedBoundingBox()); - auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ? - ComponentState::PeriodicChange : - ComponentState::NoChange; - _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state); - - return true; - }); + this->UpdateLinksBoundingBoxes(_ecm); + this->UpdateModelsBoundingBoxes(_ecm); } // NOLINT readability/fn_size // TODO (azeey) Reduce size of function and remove the NOLINT above @@ -4354,6 +4332,107 @@ void PhysicsPrivate::DisableContactSurfaceCustomization(const Entity &_world) << _world << "]" << std::endl; } +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateLinksBoundingBoxes(EntityComponentManager &_ecm) +{ + // Only compute bounding box if component exists to avoid unnecessary + // computation for links. + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + components::AxisAlignedBox *_bbox) + { + auto linkPhys = this->entityLinkMap.Get(_entity); + if (!linkPhys) + { + gzwarn << "Failed to find link [" << _entity << "]." << std::endl; + return true; + } + + auto bbLink = this->entityLinkMap.EntityCast< + LinkBoundingBoxFeatureList>(_entity); + + // Bounding box expressed in the link frame + math::AxisAlignedBox bbox; + + if (bbLink) + { + bbox = math::eigen3::convert( + bbLink->GetAxisAlignedBoundingBox(linkPhys->GetFrameID())); + } + else + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to get a bounding box, but the physics " + << "engine doesn't support feature " + << "[GetLinkBoundingBox]. Link bounding boxes will be " + << "computed from their collision shapes based on their " + << "geometry properties in SDF." << std::endl; + informed = true; + } + + // Fallback to SDF API to get the link AABB from its collision shapes. + // If the link has no collision shapes, the AABB will be invalid. + bbox = gz::sim::Link(_entity).ComputeAxisAlignedBox(_ecm).value_or( + math::AxisAlignedBox()); + } + + auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state); + + return true; + }); +} + +////////////////////////////////////////////////// +void PhysicsPrivate::UpdateModelsBoundingBoxes(EntityComponentManager &_ecm) +{ + // Only compute bounding box if component exists to avoid unnecessary + // computation for models. + _ecm.Each( + [&](const Entity &_entity, const components::Model *, + components::AxisAlignedBox *_bbox) + { + if (!this->entityModelMap.HasEntity(_entity)) + { + gzwarn << "Failed to find model [" << _entity << "]." << std::endl; + return true; + } + + auto bbModel = this->entityModelMap.EntityCast< + ModelBoundingBoxFeatureList>(_entity); + + if (!bbModel) + { + static bool informed{false}; + if (!informed) + { + gzdbg << "Attempting to get a bounding box, but the physics " + << "engine doesn't support feature " + << "[GetModelBoundingBox]. Bounding box won't be populated." + << std::endl; + informed = true; + } + + // Break Each call since no AxisAlignedBox'es can be processed + return false; + } + + // Bounding box expressed in the world frame + math::AxisAlignedBox bbox = + math::eigen3::convert(bbModel->GetAxisAlignedBoundingBox()); + auto state = _bbox->SetData(bbox, this->axisAlignedBoxEql) ? + ComponentState::PeriodicChange : + ComponentState::NoChange; + _ecm.SetChanged(_entity, components::AxisAlignedBox::typeId, state); + + return true; + }); +} + GZ_ADD_PLUGIN(Physics, System, Physics::ISystemConfigure, diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index f487e1b289..f6e5f63f9b 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -1493,7 +1493,8 @@ TEST_F(PhysicsSystemFixtureWithDart6_10, JointEffortLimitsCommandComponent) } ///////////////////////////////////////////////// -TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) +TEST_F(PhysicsSystemFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(GetModelBoundingBox)) { ServerConfig serverConfig; @@ -1561,6 +1562,138 @@ TEST_F(PhysicsSystemFixture, GZ_UTILS_TEST_DISABLED_ON_WIN32(GetBoundingBox)) bbox.begin()->second); } +///////////////////////////////////////////////// +TEST_F(PhysicsSystemFixture, + GZ_UTILS_TEST_DISABLED_ON_WIN32(GetLinkBoundingBox)) +{ + const auto sdfFile = std::string(PROJECT_BINARY_PATH) + + "/test/worlds/bounding_boxes.sdf"; + + sdf::Root root; + root.Load(sdfFile); + const sdf::World *world = root.WorldByIndex(0); + auto gravity = world->Gravity(); + + // The server update period + auto dt = 1ms; + + ServerConfig serverConfig; + serverConfig.SetSdfFile(sdfFile); + Server server(serverConfig); + server.SetUpdatePeriod(dt); + + // Create a map of link scoped name to its world axis aligned box + std::map worldLinkBbox; + + // Create a map of link scoped name to its initial world axis aligned box + std::map initialWorldLinkBbox; + + test::Relay testSystem; + + testSystem.OnPreUpdate( + [&](const UpdateInfo &_info, + EntityComponentManager &_ecm) + { + _ecm.Each( + [&](const Entity &_entity, const components::Link *, + const components::Name *_name)->bool + { + auto link = Link(_entity); + auto model = link.ParentModel(_ecm); + EXPECT_TRUE(model.has_value()); + + if (_name->Data() == "link" && !model->Static(_ecm)) + { + // The component should be null in the first iteration only + if (_info.iterations <= 1) + { + EXPECT_FALSE(link.AxisAlignedBox(_ecm).has_value()); + link.EnableBoundingBoxChecks(_ecm); + + // Store initial world bounding boxes for post-simulation checks + initialWorldLinkBbox[scopedName(_entity, _ecm, "/", false)] = + link.WorldAxisAlignedBox(_ecm).value(); + + return true; + } + auto linkAabb = link.AxisAlignedBox(_ecm); + auto worldAabb = link.WorldAxisAlignedBox(_ecm); + + EXPECT_TRUE(linkAabb.has_value()); + EXPECT_TRUE(worldAabb.has_value()); + + // Links are not initialized at the world origin + // and their models are free-falling. + EXPECT_NE(linkAabb, worldAabb); + } + return true; + }); + }); + + testSystem.OnPostUpdate( + [&](const UpdateInfo &, + const EntityComponentManager &_ecm) + { + // store links that have axis aligned box computed + _ecm.Each( + [&](const Entity & _entity, const components::Link *, + const components::AxisAlignedBox *)->bool + { + auto link = Link(_entity); + auto aabbSdf = link.ComputeAxisAlignedBox(_ecm).value(); + auto aabbPhysics = link.AxisAlignedBox(_ecm).value(); + + // The bounding box computed by the physics engine should be + // close-enough to the one computed from the SDF collisions. + EXPECT_TRUE(aabbPhysics.Min().Equal(aabbSdf.Min(), 5e-3)); + EXPECT_TRUE(aabbPhysics.Max().Equal(aabbSdf.Max(), 5e-3)); + + // Store final world bounding boxes for post-simulation checks + worldLinkBbox[scopedName(_entity, _ecm, "/", false)] = + link.WorldAxisAlignedBox(_ecm).value(); + + return true; + }); + }); + + server.AddSystem(testSystem.systemPtr); + size_t iters = 100; + const double tf = std::chrono::duration(dt).count() * iters; + server.Run(true, iters, false); + EXPECT_EQ(2u, worldLinkBbox.size()); + + // Check that final world bound boxes are close to the initial ones + // plus the expected free-fall displacement. + for (const auto &[name, bbox] : worldLinkBbox) + { + auto expectedBbox = initialWorldLinkBbox[name] + + 0.5 * gravity * tf * tf; + + // 5mm tolerance is used since actual dt is variable + EXPECT_TRUE(expectedBbox.Min().Equal(bbox.Min(), 5e-3)); + EXPECT_TRUE(expectedBbox.Max().Equal(bbox.Max(), 5e-3)); + } + + // Simulate until t=1.5s which is enough for models to reach the ground. + server.Run(true, 1500 - iters, false); + EXPECT_EQ(2u, worldLinkBbox.size()); + + // Collisions link starts at (2, 2, 2) and it is trivial to obtain its + // bounding box'es min and max points. + auto collisionsBbox = worldLinkBbox["bounding_boxes/collisions/link"]; + EXPECT_EQ(math::Vector3d(2 - 1.5, 2 - 1.5, 0), collisionsBbox.Min()); + EXPECT_EQ(math::Vector3d(2 + 1.5, 2 + 1.5, 1), collisionsBbox.Max()); + + // Duck link starts at (0, 0, 3) and its bounding box can be qualitatively + // checked since the displacement is along the z-axis only. + auto duckBbox = worldLinkBbox["bounding_boxes/duck/link"]; + EXPECT_NEAR(duckBbox.Min().Z(), 0, 1e-3); + EXPECT_NEAR(duckBbox.Max().Z(), duckBbox.ZLength(), 1e-3); + EXPECT_LT(duckBbox.Min().X(), 0); + EXPECT_GT(duckBbox.Max().X(), 0); + EXPECT_LT(duckBbox.Min().Y(), 0); + EXPECT_GT(duckBbox.Max().Y(), 0); +} ///////////////////////////////////////////////// // This tests whether nested models can be loaded correctly diff --git a/test/worlds/CMakeLists.txt b/test/worlds/CMakeLists.txt index a522517795..b043af34bf 100644 --- a/test/worlds/CMakeLists.txt +++ b/test/worlds/CMakeLists.txt @@ -6,3 +6,4 @@ configure_file (environmental_data.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/envi configure_file (environmental_sensor.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/environmental_sensor.sdf) configure_file (hydrodynamics.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/hydrodynamics.sdf) +configure_file (bounding_boxes.sdf.in ${PROJECT_BINARY_DIR}/test/worlds/bounding_boxes.sdf) diff --git a/test/worlds/bounding_boxes.sdf.in b/test/worlds/bounding_boxes.sdf.in new file mode 100644 index 0000000000..8d6a4f325c --- /dev/null +++ b/test/worlds/bounding_boxes.sdf.in @@ -0,0 +1,145 @@ + + + + + 0 + + + + + + + true + 0.8 0.8 0.8 1 + 0.5 0.5 0.5 1 + 0.5 0.5 -1 + + + + true + + + 0 0 -0.05 0 0 0 + + + 0 0 1 + 100 100 + + + 20 20 0.1 + + + + + 0 0 -0.05 0 0 0 + + + 0 0 1 + 100 100 + + + 20 20 0.1 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + 0 0 3 0 0 0 + + + + 3.92 + 0 + 0 + 3.92 + 0 + 3.92 + + 39 + + + 0 0 -0.4 1.57 0 0 + + + 0.5 0.5 0.5 + file://@CMAKE_SOURCE_DIR@/test/media/duck_collider.dae + + + + + 0 0 -0.4 1.57 0 0 + + + 0.5 0.5 0.5 + file://@CMAKE_SOURCE_DIR@/test/media/duck.dae + + + + + + + + 2 2 2 0 0 0 + + + 0 1 0 0 0 0 + + + 0.5 + + + + + 0 1 0 0 0 0 + + + 0.5 + + + + + 1 -1 0 0 0 0 + + + 1 1 1 + + + + + 1 -1 0 0 0 0 + + + 1 1 1 + + + + + -1 0 0 0 0 0 + + + 1 1 1 + + + + + -1 0 0 0 0 0 + + + 1 1 1 + + + + + + + +