Resolve auto inertia based on input mass

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

@@ -483,12 +483,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>" + \
@@ -519,11 +521,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

@@ -211,7 +211,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(
@@ -670,18 +669,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
@@ -695,7 +719,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>(