handle bad inertial values in collision

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Feb 11, 2025 · 2b0cbb2 · 2b0cbb2
1 parent 75867c0
commit 2b0cbb2
Show file tree

Hide file tree

Showing 3 changed files with 53 additions and 19 deletions.
diff --git a/src/Collision.cc b/src/Collision.cc
@@ -324,7 +324,10 @@ void Collision::CalculateInertial(
   {
     _errors.push_back({ErrorCode::LINK_INERTIA_INVALID,
         "Inertia Calculated for collision: " +
-        this->dataPtr->name + " is invalid."});
+        this->dataPtr->name + " is invalid, using default inertial values."});
+    _inertial = gz::math::Inertiald(
+        gz::math::MassMatrix3d(1, gz::math::Vector3d::One,
+        gz::math::Vector3d::Zero), gz::math::Pose3d::Zero);
   }
   else
   {

diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc
@@ -195,6 +195,49 @@ TEST(DOMcollision, SetSurface)
   EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2);
 }
 
+/////////////////////////////////////////////////
+TEST(DOMCollision, IncorrectMeshCollisionCalculateInertial)
+{
+  sdf::Collision collision;
+
+  sdf::ElementPtr sdf(new sdf::Element());
+  collision.Load(sdf);
+
+  const sdf::ParserConfig sdfParserConfig;
+  sdf::Geometry geom;
+  sdf::Mesh mesh;
+  geom.SetType(sdf::GeometryType::MESH);
+  geom.SetMeshShape(mesh);
+  collision.SetGeom(geom);
+
+  sdf::ParserConfig config;
+  sdf::Errors errors;
+  sdf::CustomInertiaCalcProperties inertiaCalcProps;
+
+  // Custom inertia calculator that returns null inertia
+  auto customMeshInertiaCalculator = [](
+    sdf::Errors &,
+    const sdf::CustomInertiaCalcProperties &)
+      -> std::optional<gz::math::Inertiald>
+  {
+    return std::nullopt;
+  };
+  config.RegisterCustomInertiaCalc(customMeshInertiaCalculator);
+
+  gz::math::Inertiald collisionInertial;
+  collision.CalculateInertial(errors, collisionInertial, config);
+  ASSERT_FALSE(errors.empty());
+
+  // Expect mesh class to handle null inertia and return
+  // default inertial values
+  gz::math::Inertiald defaultInertial;
+  defaultInertial.SetMassMatrix(
+    gz::math::MassMatrix3d(1.0,
+      gz::math::Vector3d::One,
+      gz::math::Vector3d::Zero));
+  ASSERT_EQ(collisionInertial, defaultInertial);
+}
+
 /////////////////////////////////////////////////
 TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial)
 {

diff --git a/src/Mesh.cc b/src/Mesh.cc
@@ -376,10 +376,6 @@ std::optional<gz::math::Inertiald> Mesh::CalculateInertial(sdf::Errors &_errors,
 {
   const auto &customCalculator = _config.CustomInertiaCalc();
 
-  auto defaultInertial = gz::math::Inertiald(
-      gz::math::MassMatrix3d(1, gz::math::Vector3d::One,
-                             gz::math::Vector3d::Zero),
-                             gz::math::Pose3d::Zero);
   if (!customCalculator)
   {
     Error err(
@@ -389,25 +385,17 @@ std::optional<gz::math::Inertiald> Mesh::CalculateInertial(sdf::Errors &_errors,
         "inertial values.");
     enforceConfigurablePolicyCondition(
           _config.WarningsPolicy(), err, _errors);
-    return defaultInertial;
+
+    using namespace gz::math;
+    return Inertiald(
+        MassMatrix3d(1, Vector3d::One, Vector3d::Zero),
+        Pose3d::Zero);
   }
 
   sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties(
     _density, *this, _autoInertiaParams);
 
-  std::optional<gz::math::Inertiald> inertial =
-      customCalculator(_errors, calcInterface);
-  if (!inertial)
-  {
-    Error err(
-        sdf::ErrorCode::WARNING,
-        "Custom moment of inertia calculator for meshes produced invalid inertia, "
-        "using default inertial values.");
-    enforceConfigurablePolicyCondition(
-          _config.WarningsPolicy(), err, _errors);
-    return defaultInertial;
-  }
-  return inertial;
+  return customCalculator(_errors, calcInterface);
 }
 
 /////////////////////////////////////////////////