Resolve auto inertia based on input mass (#1513)

Support auto-inertia computation using mass and density. Implemented based on the suggestions in #1482. Inertia is first auto resolved from all collisions as usual. If mass is specified, we normalize the inertia to get unit inertia, then scaling is applied to match the desired mass.

---------

Signed-off-by: Steve Peters <[email protected]>
Signed-off-by: Ian Chen <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
(cherry picked from commit aaadeea)

Author: iche033

include/sdf/Collision.hh

@@ -170,12 +170,15 @@ namespace sdf
     /// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params
     /// element to be used if the auto_inertia_params have not been set in this
     /// collision.
+    /// \param[in] _warnIfDensityIsUnset True to generate a warning that
+    /// the default density value will be used if it is not explicitly set.
     public: void CalculateInertial(
                     sdf::Errors &_errors,
                     gz::math::Inertiald &_inertial,
                     const ParserConfig &_config,
                     const std::optional<double> &_density,
-                    sdf::ElementPtr _autoInertiaParams);
+                    sdf::ElementPtr _autoInertiaParams,
+                    bool _warnIfDensityIsUnset = true);
 
     /// \brief Get a pointer to the SDF element that was used during
     /// load.

include/sdf/Link.hh

@@ -351,9 +351,13 @@ namespace sdf
                                    const std::string &_resolveTo = "") const;
 
     /// \brief Calculate & set inertial values(mass, mass matrix
-    /// & inertial pose) for the link. Inertial values can be provided
-    /// by the user through the SDF or can be calculated automatically
-    /// by setting the auto attribute to true.
+    /// & inertial pose) for the link. This function will calculate
+    /// the inertial values if the auto attribute is set to true.
+    /// If mass is not provided by the user, the inertial values will be
+    /// determined based on either link density or collision density if
+    /// explicitly set. Otherwise, if mass is specified, the inertia matrix
+    /// will be scaled to match the desired mass, while respecting
+    /// the ratio of density values between collisions.
     /// \param[out] _errors A vector of Errors object. Each object
     /// would contain an error code and an error message.
     /// \param[in] _config Custom parser configuration

python/test/pyLink_TEST.py

@@ -479,12 +479,14 @@ def test_resolveauto_inertialsWithMultipleCollisions(self):
             link.inertial().mass_matrix().diagonal_moments())
 
     def test_inertial_values_given_with_auto_set_to_true(self):
+        # The inertia matrix is specified but should be ignored.
+        # <mass> is not speicifed so the inertial values should be computed
+        # based on the collision density value.
         sdf = "<?xml version=\"1.0\"?>" + \
         "<sdf version=\"1.11\">" + \
         "  <model name='compound_model'>" + \
         "   <link name='compound_link'>" + \
         "     <inertial auto='True'>" + \
-        "       <mass>4.0</mass>" + \
         "       <pose>1 1 1 2 2 2</pose>" + \
         "       <inertia>" + \
         "         <ixx>1</ixx>" + \
@@ -515,11 +517,169 @@ def test_inertial_values_given_with_auto_set_to_true(self):
         root.resolve_auto_inertials(errors, sdfParserConfig)
         self.assertEqual(len(errors), 0)
 
-        self.assertNotEqual(4.0, link.inertial().mass_matrix().mass())
+        self.assertEqual(2.0, link.inertial().mass_matrix().mass())
         self.assertEqual(Pose3d.ZERO, link.inertial().pose())
         self.assertEqual(Vector3d(0.33333, 0.33333, 0.33333),
             link.inertial().mass_matrix().diagonal_moments())
 
+    def test_resolveauto_inertialsWithMass(self):
+        # The inertia matrix is specified but should be ignored.
+        # <mass> is speicifed - the auto computed inertial values should
+        # be scaled based on the desired mass.
+        sdf = "<?xml version=\"1.0\"?>" + \
+        "<sdf version=\"1.11\">" + \
+        "  <model name='compound_model'>" + \
+        "   <link name='compound_link'>" + \
+        "     <inertial auto='True'>" + \
+        "       <mass>4.0</mass>" + \
+        "       <pose>1 1 1 2 2 2</pose>" + \
+        "       <inertia>" + \
+        "         <ixx>1</ixx>" + \
+        "         <iyy>1</iyy>" + \
+        "         <izz>1</izz>" + \
+        "       </inertia>" + \
+        "     </inertial>" + \
+        "     <collision name='box_collision'>" + \
+        "      <density>2.0</density>" + \
+        "      <geometry>" + \
+        "        <box>" + \
+        "          <size>1 1 1</size>" + \
+        "        </box>" + \
+        "        </geometry>" + \
+        "      </collision>" + \
+        "    </link>" + \
+        "   </model>" + \
+        "  </sdf>"
+
+        root = Root()
+        sdfParserConfig = ParserConfig()
+        errors = root.load_sdf_string(sdf, sdfParserConfig)
+        self.assertEqual(errors, None)
+
+        model = root.model()
+        link = model.link_by_index(0)
+        errors = []
+        root.resolve_auto_inertials(errors, sdfParserConfig)
+        self.assertEqual(len(errors), 0)
+
+        self.assertEqual(4.0, link.inertial().mass_matrix().mass())
+        self.assertEqual(Pose3d.ZERO, link.inertial().pose())
+        self.assertEqual(Vector3d(0.66666, 0.66666, 0.66666),
+            link.inertial().mass_matrix().diagonal_moments())
+
+    def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
+        # The inertia matrix is specified but should be ignored.
+        # <mass> is speicifed - the auto computed inertial values should
+        # be scaled based on the desired mass.
+        sdf = "<?xml version=\"1.0\"?>" + \
+        "<sdf version=\"1.11\">" + \
+        "  <model name='compound_model'>" + \
+        "   <link name='compound_link'>" + \
+        "     <inertial auto='True'>" + \
+        "       <mass>12.0</mass>" + \
+        "       <pose>1 1 1 2 2 2</pose>" + \
+        "       <inertia>" + \
+        "         <ixx>1</ixx>" + \
+        "         <iyy>1</iyy>" + \
+        "         <izz>1</izz>" + \
+        "       </inertia>" + \
+        "     </inertial>" + \
+        "     <collision name='cube_collision'>" + \
+        "       <pose>0.0 0.0 0.5 0 0 0</pose>" + \
+        "      <density>4.0</density>" + \
+        "      <geometry>" + \
+        "        <box>" + \
+        "          <size>1 1 1</size>" + \
+        "        </box>" + \
+        "        </geometry>" + \
+        "      </collision>" + \
+        "     <collision name='box_collision'>" + \
+        "       <pose>0.0 0.0 -1.0 0 0 0</pose>" + \
+        "       <density>1.0</density>" + \
+        "       <geometry>" + \
+        "         <box>" + \
+        "           <size>1 1 2</size>" + \
+        "         </box>" + \
+        "       </geometry>" + \
+        "    </collision>" + \
+        "  </link>" + \
+        " </model>" + \
+        "</sdf>"
+
+        root = Root()
+        sdfParserConfig = ParserConfig()
+        errors = root.load_sdf_string(sdf, sdfParserConfig)
+        self.assertEqual(errors, None)
+
+        model = root.model()
+        link = model.link_by_index(0)
+        errors = []
+        root.resolve_auto_inertials(errors, sdfParserConfig)
+        self.assertEqual(len(errors), 0)
+
+        self.assertEqual(12.0, link.inertial().mass_matrix().mass())
+        self.assertEqual(Pose3d.ZERO,
+                         link.inertial().pose())
+        self.assertEqual(Vector3d(9.0, 9.0, 2.0),
+            link.inertial().mass_matrix().diagonal_moments())
+
+    def test_resolveauto_inertialsWithMassAndDefaultDensity(self):
+        # The inertia matrix is specified but should be ignored.
+        # <mass> is speicifed - the auto computed inertial values should
+        # be scaled based on the desired mass.
+        # Density is not specified for the bottom collision - it should
+        # use the default value
+        sdf = "<?xml version=\"1.0\"?>" + \
+        "<sdf version=\"1.11\">" + \
+        "  <model name='compound_model'>" + \
+        "   <link name='compound_link'>" + \
+        "     <inertial auto='True'>" + \
+        "       <mass>12000.0</mass>" + \
+        "       <pose>1 1 1 2 2 2</pose>" + \
+        "       <inertia>" + \
+        "         <ixx>1</ixx>" + \
+        "         <iyy>1</iyy>" + \
+        "         <izz>1</izz>" + \
+        "       </inertia>" + \
+        "     </inertial>" + \
+        "     <collision name='cube_collision'>" + \
+        "       <pose>0.0 0.0 0.5 0 0 0</pose>" + \
+        "      <density>4000.0</density>" + \
+        "      <geometry>" + \
+        "        <box>" + \
+        "          <size>1 1 1</size>" + \
+        "        </box>" + \
+        "        </geometry>" + \
+        "      </collision>" + \
+        "     <collision name='box_collision'>" + \
+        "       <pose>0.0 0.0 -1.0 0 0 0</pose>" + \
+        "       <geometry>" + \
+        "         <box>" + \
+        "           <size>1 1 2</size>" + \
+        "         </box>" + \
+        "       </geometry>" + \
+        "    </collision>" + \
+        "  </link>" + \
+        " </model>" + \
+        "</sdf>"
+
+        root = Root()
+        sdfParserConfig = ParserConfig()
+        errors = root.load_sdf_string(sdf, sdfParserConfig)
+        self.assertEqual(errors, None)
+
+        model = root.model()
+        link = model.link_by_index(0)
+        errors = []
+        root.resolve_auto_inertials(errors, sdfParserConfig)
+        self.assertEqual(len(errors), 0)
+
+        self.assertEqual(12000.0, link.inertial().mass_matrix().mass())
+        self.assertEqual(Pose3d.ZERO,
+                         link.inertial().pose())
+        self.assertEqual(Vector3d(9000.0, 9000.0, 2000.0),
+            link.inertial().mass_matrix().diagonal_moments())
+
     def test_resolveauto_inertialsCalledWithAutoFalse(self):
         sdf = "<?xml version=\"1.0\"?>" + \
         " <sdf version=\"1.11\">" + \

src/Collision.cc

@@ -272,7 +272,8 @@ void Collision::CalculateInertial(
   gz::math::Inertiald &_inertial,
   const ParserConfig &_config,
   const std::optional<double> &_density,
-  sdf::ElementPtr _autoInertiaParams)
+  sdf::ElementPtr _autoInertiaParams,
+  bool _warnIfDensityIsUnset)
 {
   // Order of precedence for density:
   double density;
@@ -291,18 +292,21 @@ void Collision::CalculateInertial(
   // 3. DensityDefault value.
   else
   {
-    // If density was not explicitly set, send a warning
-    // about the default value being used
+    // If density was not explicitly set, default value is used
+    // Send a warning about the default value being used if needed
     density = DensityDefault();
-    Error densityMissingErr(
-      ErrorCode::ELEMENT_MISSING,
-      "Collision is missing a <density> child element. "
-      "Using a default density value of " +
-      std::to_string(DensityDefault()) + " kg/m^3. "
-    );
-    enforceConfigurablePolicyCondition(
-      _config.WarningsPolicy(), densityMissingErr, _errors
-    );
+    if (_warnIfDensityIsUnset)
+    {
+      Error densityMissingErr(
+        ErrorCode::ELEMENT_MISSING,
+        "Collision is missing a <density> child element. "
+        "Using a default density value of " +
+        std::to_string(DensityDefault()) + " kg/m^3. "
+      );
+      enforceConfigurablePolicyCondition(
+        _config.WarningsPolicy(), densityMissingErr, _errors
+      );
+    }
   }
 
   // If this Collision's auto inertia params have not been set, then use the

src/Link.cc

@@ -208,7 +208,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
       // If auto is to true but user has still provided
       // inertial values
       if (inertialElem->HasElement("pose") ||
-          inertialElem->HasElement("mass") ||
           inertialElem->HasElement("inertia"))
       {
         Error err(
@@ -664,18 +663,43 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
       return;
     }
 
+    auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
+    bool massSpecified = inertialElem->HasElement("mass");
+    // Warn about using default collision density value if mass is not specified
+    bool warnUseDefaultDensity = !massSpecified;
+
     gz::math::Inertiald totalInertia;
 
     for (sdf::Collision &collision : this->dataPtr->collisions)
     {
       gz::math::Inertiald collisionInertia;
       collision.CalculateInertial(_errors, collisionInertia, _config,
                                   this->dataPtr->density,
-                                  this->dataPtr->autoInertiaParams);
+                                  this->dataPtr->autoInertiaParams,
+                                  warnUseDefaultDensity);
       totalInertia = totalInertia + collisionInertia;
     }
 
-    this->dataPtr->inertial = totalInertia;
+    // If mass is specified, scale inertia to match desired mass
+    if (massSpecified)
+    {
+      double mass = inertialElem->Get<double>("mass");
+      const gz::math::MassMatrix3d &totalMassMatrix = totalInertia.MassMatrix();
+      // normalize to get the unit mass matrix
+      gz::math::MassMatrix3d unitMassMatrix(1.0,
+          totalMassMatrix.DiagonalMoments() / totalMassMatrix.Mass(),
+          totalMassMatrix.OffDiagonalMoments() / totalMassMatrix.Mass());
+      // scale the final inertia to match specified mass
+      this->dataPtr->inertial = gz::math::Inertiald(
+          gz::math::MassMatrix3d(mass,
+              mass * unitMassMatrix.DiagonalMoments(),
+              mass * unitMassMatrix.OffDiagonalMoments()),
+          totalInertia.Pose());
+    }
+    else
+    {
+      this->dataPtr->inertial = totalInertia;
+    }
 
     // If CalculateInertial() was called with SAVE_CALCULATION
     // configuration then set autoInertiaSaved to true
@@ -689,7 +713,6 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
     {
       this->dataPtr->autoInertiaSaved = true;
       // Write calculated inertia values to //link/inertial element
-      auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
       inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
         totalInertia.Pose());
       inertialElem->GetElement("mass")->GetValue()->Set<double>(