fix build

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Dec 11, 2024 · 410d4e6 · 410d4e6
1 parent aa78c6a
commit 410d4e6
Show file tree

Hide file tree

Showing 2 changed files with 60 additions and 2 deletions.
diff --git 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.
+        # <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'>" + \
+        "       <pose>0.0 0.0 0.5 0 0 0</pose>" + \
+        "      <density>2.0</density>" + \
+        "      <geometry>" + \
+        "        <box>" + \
+        "          <size>1 1 1</size>" + \
+        "        </box>" + \
+        "        </geometry>" + \
+        "      </collision>" + \
+        "     <collision name='box_collision2'>" + \
+        "       <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>" + \
+        "  </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(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 = "<?xml version=\"1.0\"?>" + \
         " <sdf version=\"1.11\">" + \

diff --git 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());
 }