diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py
index a31852ebb..41391f897 100644
--- a/python/test/pyLink_TEST.py
+++ b/python/test/pyLink_TEST.py
@@ -580,7 +580,7 @@ def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
" " + \
" " + \
" " + \
- " 4.0" + \
+ " 12.0" + \
" 1 1 1 2 2 2" + \
" " + \
" 1" + \
@@ -588,21 +588,78 @@ def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
" 1" + \
" " + \
" " + \
+ " " + \
+ " 0.0 0.0 0.5 0 0 0" + \
+ " 4.0" + \
+ " " + \
+ " " + \
+ " 1 1 1" + \
+ " " + \
+ " " + \
+ " " + \
" " + \
+ " 0.0 0.0 -1.0 0 0 0" + \
+ " 1.0" + \
+ " " + \
+ " " + \
+ " 1 1 2" + \
+ " " + \
+ " " + \
+ " " + \
+ " " + \
+ " " + \
+ ""
+
+ 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.
+ # 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 = "" + \
+ "" + \
+ " " + \
+ " " + \
+ " " + \
+ " 12000.0" + \
+ " 1 1 1 2 2 2" + \
+ " " + \
+ " 1" + \
+ " 1" + \
+ " 1" + \
+ " " + \
+ " " + \
+ " " + \
" 0.0 0.0 0.5 0 0 0" + \
- " 2.0" + \
+ " 4000.0" + \
" " + \
" " + \
" 1 1 1" + \
" " + \
" " + \
" " + \
- " " + \
- " 0.0 0.0 -0.5 0 0 0" + \
- " 4.0" + \
+ " " + \
+ " 0.0 0.0 -1.0 0 0 0" + \
" " + \
" " + \
- " 1 1 1" + \
+ " 1 1 2" + \
" " + \
" " + \
" " + \
@@ -621,10 +678,10 @@ def test_resolveauto_inertialsWithMassAndMultipleCollisions(self):
root.resolve_auto_inertials(errors, sdfParserConfig)
self.assertEqual(len(errors), 0)
- self.assertEqual(4.0, link.inertial().mass_matrix().mass())
- self.assertEqual(Pose3d(0, 0, -0.166667, 0, 0, 0),
+ self.assertEqual(12000.0, link.inertial().mass_matrix().mass())
+ self.assertEqual(Pose3d.ZERO,
link.inertial().pose())
- self.assertEqual(Vector3d(1.55556, 1.55556, 0.666667),
+ self.assertEqual(Vector3d(9000.0, 9000.0, 2000.0),
link.inertial().mass_matrix().diagonal_moments())
def test_resolveauto_inertialsCalledWithAutoFalse(self):
diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc
index 1ab1404dd..a2c346c2d 100644
--- a/src/Link_TEST.cc
+++ b/src/Link_TEST.cc
@@ -929,6 +929,115 @@ TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions)
link->Inertial().MassMatrix().DiagonalMoments());
}
+/////////////////////////////////////////////////
+TEST(DOMLink, ResolveAutoInertialsWithMassAndDefaultDensity)
+{
+ // A model with link inertial auto set to true.
+ // The inertia matrix is specified but should be ignored.
+ // is specified - the auto computed inertial values should
+ // be scaled based on the desired mass.
+ // The model contains two collisions with different sizes. Density
+ // is specified for the top collision but not the bottom collision.
+ // The model should have a lumped center of mass at the link origin.
+ std::string sdf = ""
+ ""
+ " "
+ " "
+ " "
+ " 12000.0"
+ " 1 1 1 2 2 2"
+ " "
+ " 1"
+ " 1"
+ " 1"
+ " "
+ " "
+ " "
+ " 0.0 0.0 0.5 0 0 0"
+ " 4000"
+ " "
+ " "
+ " 1 1 1"
+ " "
+ " "
+ " "
+ " "
+ " 0.0 0.0 -1.0 0 0 0"
+ " "
+ " "
+ " 1 1 2"
+ " "
+ " "
+ " "
+ " "
+ " "
+ "";
+
+ sdf::Root root;
+ const sdf::ParserConfig sdfParserConfig;
+ sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
+ EXPECT_TRUE(errors.empty());
+ EXPECT_NE(nullptr, root.Element());
+
+ const sdf::Model *model = root.Model();
+ const sdf::Link *link = model->LinkByIndex(0);
+ root.ResolveAutoInertials(errors, sdfParserConfig);
+ EXPECT_TRUE(errors.empty());
+
+ // Expect mass value from //inertial/mass
+ EXPECT_DOUBLE_EQ(12000.0, link->Inertial().MassMatrix().Mass());
+
+ // Inertial values based on density before scaling to match specified mass
+ //
+ // Collision geometries:
+ //
+ // --------- z = 1
+ // | |
+ // | c | cube on top, side length 1.0, density 4000.0
+ // | |
+ // |-------| z = 0
+ // | |
+ // | |
+ // | |
+ // | c | box on bottom, size 1x1x2, default density 1000.0
+ // | |
+ // | |
+ // | |
+ // --------- z = -2
+ //
+ // Top cube: volume = 1.0, mass = 4000.0, center of mass z = 0.5
+ // Bottom box: volume = 2.0, mass = 2000.0, center of mass z = -1.0
+ //
+ // Total mass from density: 6000.0
+ // Lumped center of mass z = (4000.0 * 0.5 + 2000.0 * (-1.0)) / 6000.0 = 0.0
+ EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose());
+
+ // Moment of inertia at center of each shape
+ // Top cube: Ixx = Iyy = Izz = 4000.0 / 12 * (1*1 + 1*1) = 8000/12
+ // Bottom box: Ixx = Iyy = 2000.0 / 12 * (1*1 + 2*2) = 10000/12
+ // Izz = 2000.0 / 12 * (1*1 + 1*1) = 4000/12
+ //
+ // Lumped moment of inertia at lumped center of mass
+ // Ixx = sum(Ixx_i + mass_i * center_of_mass_z_i^2) for i in [top, bottom]
+ // Iyy = Ixx
+ // Izz = Izz_top + Izz_bottom
+ //
+ // Ixx = Iyy = (8000/12 + 4000.0 * 0.5 * 0.5) + (10000/12 + 2000.0
+ // * (-1.0) * (-1.0))
+ // = (8000/12 + 1000) + (10000/12 + 2000.0)
+ // = 20000/12 + 34000/12
+ // = 54000/12 = 9000/2 = 4500
+ //
+ // Izz = 8000/12 + 4000/12 = 1000.0
+
+ // Then scale the inertias according to the specified mass of 12000.0
+ // mass_ratio = 12000.0 / 6000.0 = 2.0
+ // Ixx = Iyy = mass_ratio * Ixx_unscaled = 2.0 * 4500.0 = 9000.0
+ // Izz = mass_ratio * Izz_unscaled = 2.0 * 1000.0 = 2000.0
+ EXPECT_EQ(gz::math::Vector3d(9000.0, 9000.0, 2000.0),
+ link->Inertial().MassMatrix().DiagonalMoments());
+}
+
/////////////////////////////////////////////////
TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse)
{