Add policy for handling CalculateInertial failures (#1543)

If CalculateInertial fails to find valid inertial values,
report an error by default, but allow users to choose
a policy that uses default inertial values with a warning
instead of a hard failure.

The policy is used in Collision::CalculateInertial and tested.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
Signed-off-by: Ian Chen <ichen@openrobotics.org>
Co-authored-by: Ian Chen <ichen@openrobotics.org>

include/sdf/ParserConfig.hh

@@ -69,6 +69,21 @@ enum class ConfigureResolveAutoInertials
   SAVE_CALCULATION_IN_ELEMENT,
 };
 
+/// \enum CalculateInertialFailurePolicyType
+/// \brief Configuration options of how CalculateInertial() failures should
+/// be handled.
+enum class CalculateInertialFailurePolicyType
+{
+  /// \brief If this value is used, failures of Geometry::CalculateInertial()
+  /// will result in a LINK_INERTIA_INVALID error with no inertial values
+  /// written.
+  ERR,
+
+  /// \brief If this value is used, failures of Geometry::CalculateInertial()
+  /// will result in default inertial values used and a WARNING.
+  WARN_AND_USE_DEFAULT_INERTIAL,
+};
+
 // Forward declare private data class.
 class ParserConfigPrivate;
 
@@ -192,6 +207,19 @@ class SDFORMAT_VISIBLE ParserConfig
   public: void SetCalculateInertialConfiguration(
     ConfigureResolveAutoInertials _configuration);
 
+  /// \brief Get the current policy for handling failures of the
+  /// CalculateInertial() function. By default an error is reported.
+  /// \return Current set value of the CalculateInertialFailurePolicyType enum
+  public: CalculateInertialFailurePolicyType
+    CalculateInertialFailurePolicy() const;
+
+  /// \brief Set the policy for handling failures of the CalculateInertial()
+  /// function
+  /// \param[in] _policy The policy to set for handling failures of the
+  /// CalculateInertial() function
+  public: void SetCalculateInertialFailurePolicy(
+    CalculateInertialFailurePolicyType _policy);
+
   /// \brief Registers a custom model parser.
   /// \param[in] _modelParser Callback as described in
   /// sdf/InterfaceElements.hh.

src/Collision.cc

@@ -322,9 +322,27 @@ void Collision::CalculateInertial(
 
   if (!geomInertial)
   {
-    _errors.push_back({ErrorCode::LINK_INERTIA_INVALID,
-        "Inertia Calculated for collision: " +
-        this->dataPtr->name + " is invalid."});
+    if (_config.CalculateInertialFailurePolicy() ==
+        CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL)
+    {
+      Error err(
+          sdf::ErrorCode::WARNING,
+          "Inertia Calculated for collision: " + this->dataPtr->name +
+          " is invalid, using default inertial values.");
+      enforceConfigurablePolicyCondition(_config.WarningsPolicy(), err,
+                                         _errors);
+
+      using namespace gz::math;
+      _inertial = Inertiald(MassMatrix3d(1, Vector3d::One, Vector3d::Zero),
+                            Pose3d::Zero);
+    }
+    else if (_config.CalculateInertialFailurePolicy() ==
+             CalculateInertialFailurePolicyType::ERR)
+    {
+        _errors.push_back({ErrorCode::LINK_INERTIA_INVALID,
+            "Inertia Calculated for collision: " +
+            this->dataPtr->name + " is invalid."});
+    }
   }
   else
   {

src/Collision_TEST.cc

@@ -398,6 +398,61 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink)
   EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose());
 }
 
+/////////////////////////////////////////////////
+TEST(DOMCollision, CollisionCalculateInertialFailurePolicy)
+{
+  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 inertial
+  auto customMeshInertiaCalculator = [](
+    sdf::Errors &,
+    const sdf::CustomInertiaCalcProperties &)
+      -> std::optional<gz::math::Inertiald>
+  {
+    return std::nullopt;
+  };
+  config.RegisterCustomInertiaCalc(customMeshInertiaCalculator);
+
+  // With default inertial failure policy, there should be an error when the
+  // mesh inertial calculator returns null inertial values.
+  gz::math::Inertiald collisionInertial;
+  collision.CalculateInertial(errors, collisionInertial, config);
+  ASSERT_EQ(1u, errors.size()) << errors;
+  EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::LINK_INERTIA_INVALID);
+  const gz::math::Inertiald empty;
+  EXPECT_EQ(empty, collisionInertial);
+
+  // Set inertial failure policy to use default inertial values on failure and
+  // verify that there are no more errors.
+  errors.clear();
+  config.SetCalculateInertialFailurePolicy(
+    sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL);
+  collision.CalculateInertial(errors, collisionInertial, config);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  // Verify default inertial values are returned.
+  gz::math::Inertiald defaultInertial;
+  defaultInertial.SetMassMatrix(
+    gz::math::MassMatrix3d(1.0,
+      gz::math::Vector3d::One,
+      gz::math::Vector3d::Zero));
+  EXPECT_EQ(collisionInertial, defaultInertial);
+}
+
 /////////////////////////////////////////////////
 TEST(DOMCollision, ToElement)
 {

src/ParserConfig.cc

@@ -44,6 +44,11 @@ class sdf::ParserConfig::Implementation
   /// to behave behave differently than the `warningsPolicy`.
   public: std::optional<EnforcementPolicy> deprecatedElementsPolicy;
 
+  /// \brief Policy that is set for handling failures of the
+  /// CalculateInertial() function. By default errors are reported.
+  public: CalculateInertialFailurePolicyType calculateInertialFailurePolicy =
+    CalculateInertialFailurePolicyType::ERR;
+
   /// \brief Configuration that is set for the CalculateInertial() function
   /// By default it is set to SAVE_CALCULATION to preserve the behavior of
   /// Root::Load() generating complete inertial information.
@@ -167,6 +172,20 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const
       this->dataPtr->warningsPolicy);
 }
 
+/////////////////////////////////////////////////
+CalculateInertialFailurePolicyType
+  ParserConfig::CalculateInertialFailurePolicy() const
+{
+  return this->dataPtr->calculateInertialFailurePolicy;
+}
+
+/////////////////////////////////////////////////
+void ParserConfig::SetCalculateInertialFailurePolicy(
+  CalculateInertialFailurePolicyType _policy)
+{
+  this->dataPtr->calculateInertialFailurePolicy = _policy;
+}
+
 /////////////////////////////////////////////////
 ConfigureResolveAutoInertials
   ParserConfig::CalculateInertialConfiguration() const

src/ParserConfig_TEST.cc

@@ -68,6 +68,14 @@ TEST(ParserConfig, Construction)
   EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT,
     config.CalculateInertialConfiguration());
 
+  EXPECT_EQ(sdf::CalculateInertialFailurePolicyType::ERR,
+    config.CalculateInertialFailurePolicy());
+  config.SetCalculateInertialFailurePolicy(
+    sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL);
+  EXPECT_EQ(
+    sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL,
+    config.CalculateInertialFailurePolicy());
+
   EXPECT_FALSE(config.URDFPreserveFixedJoint());
   EXPECT_FALSE(config.StoreResolvedURIs());
 }