Compute link bounding box on physics system

Signed-off-by: Gabriel Pacheco <[email protected]>

Author: gabrielfpacheco

src/systems/physics/Physics.cc

@@ -343,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<Entity, Entity> topLevelModelMap;
@@ -555,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
@@ -680,7 +694,7 @@ class gz::sim::systems::PhysicsPrivate
             physics::Model,
             MinimumFeatureList,
             JointFeatureList,
-            BoundingBoxFeatureList,
+            ModelBoundingBoxFeatureList,
             NestedModelFeatureList,
             ConstructSdfLinkFeatureList,
             ConstructSdfJointFeatureList>;
@@ -697,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.
@@ -2853,46 +2868,8 @@ void PhysicsPrivate::UpdatePhysics(EntityComponentManager &_ecm)
       });
 
   // Populate bounding box info
-  // Only compute bounding box if component exists to avoid unnecessary
-  // computations
-  _ecm.Each<components::Model, components::AxisAlignedBox>(
-      [&](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<BoundingBoxFeatureList>(_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
 
@@ -4371,6 +4348,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<components::Link, components::AxisAlignedBox>(
+    [&](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<components::Model, components::AxisAlignedBox>(
+    [&](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,

src/systems/physics/Physics.hh

@@ -51,6 +51,7 @@
 
 #include <gz/sim/config.hh>
 #include <gz/sim/System.hh>
+#include <gz/sim/Link.hh>
 
 namespace gz
 {

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<std::string, math::AxisAlignedBox> worldLinkBbox;
+
+  // Create a map of link scoped name to its initial world axis aligned box
+  std::map<std::string, math::AxisAlignedBox> initialWorldLinkBbox;
+
+  test::Relay testSystem;
+
+  testSystem.OnPreUpdate(
+    [&](const UpdateInfo &_info,
+    EntityComponentManager &_ecm)
+    {
+      _ecm.Each<components::Link, components::Name>(
+        [&](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<components::Link, components::AxisAlignedBox>(
+        [&](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<double>(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

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)