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) {