diff --git a/python/test/pyLink_TEST.py b/python/test/pyLink_TEST.py index 55b96dc81..519e5c285 100644 --- a/python/test/pyLink_TEST.py +++ b/python/test/pyLink_TEST.py @@ -571,6 +571,63 @@ def test_resolveauto_inertialsWithMass(self): 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. + # is speicifed - the auto computed inertial values should + # be scaled based on the desired mass. + sdf = "" + \ + "" + \ + " " + \ + " " + \ + " " + \ + " 4.0" + \ + " 1 1 1 2 2 2" + \ + " " + \ + " 1" + \ + " 1" + \ + " 1" + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 0.5 0 0 0" + \ + " 2.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " 0.0 0.0 -0.5 0 0 0" + \ + " 4.0" + \ + " " + \ + " " + \ + " 1 1 1" + \ + " " + \ + " " + \ + " " + \ + " " + \ + " " + \ + "" + + 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(0, 0, -0.166667, 0, 0, 0), + link.inertial().pose()) + self.assertEqual(Vector3d(1.55556, 1.55556, 0.666667), + link.inertial().mass_matrix().diagonal_moments()) + + def test_resolveauto_inertialsCalledWithAutoFalse(self): sdf = "" + \ " " + \ diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 74e22b459..d7e0b1e3c 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -875,8 +875,9 @@ TEST(DOMLink, ResolveAutoInertialsWithMassAndMultipleCollisions) EXPECT_TRUE(errors.empty()); EXPECT_DOUBLE_EQ(4.0, link->Inertial().MassMatrix().Mass()); - EXPECT_EQ(gz::math::Pose3d(0 0 -0.166667 0 0 0), link->Inertial().Pose()); - EXPECT_EQ(gz::math::Vector3d(1.55556 1.55556 0.666667), + EXPECT_EQ(gz::math::Pose3d(0, 0, -0.166667, 0, 0, 0), + link->Inertial().Pose()); + EXPECT_EQ(gz::math::Vector3d(1.55556, 1.55556, 0.666667), link->Inertial().MassMatrix().DiagonalMoments()); }