Mesh: handle unset custom inetial calc

Warn and return default inertial values if custom inertia calculator is unset. Signed-off-by: Steve Peters <[email protected]>
gazebosim · Sep 27, 2023 · d6d8e7e · d6d8e7e
1 parent c550e3d
commit d6d8e7e
Show file tree

Hide file tree

Showing 2 changed files with 17 additions and 0 deletions.
diff --git a/src/Geometry.cc b/src/Geometry.cc
@@ -312,6 +312,7 @@ void Geometry::SetPolylineShape(const std::vector<Polyline> &_polylines)
   this->dataPtr->polylines = _polylines;
 }
 
+/////////////////////////////////////////////////
 std::optional<gz::math::Inertiald> Geometry::CalculateInertial(
   sdf::Errors &_errors, const ParserConfig &_config,
   double _density, sdf::ElementPtr _autoInertiaParams)

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