From 686dc2452fd4f798c967115586cf0edff920f010 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@openrobotics.org>
Date: Mon, 9 Dec 2024 14:08:20 -0800
Subject: [PATCH] Add tests for auto-inertial with explicit mass (#1514)

* link_dom test: Fix typo

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

* Test case for auto-inertials with explicit mass

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

---------

Signed-off-by: Steve Peters <scpeters@openrobotics.org>
---
 src/gz_TEST.cc                        |  13 +++
 test/integration/link_dom.cc          |  38 ++++++-
 test/sdf/inertial_stats_auto_mass.sdf | 140 ++++++++++++++++++++++++++
 3 files changed, 189 insertions(+), 2 deletions(-)
 create mode 100644 test/sdf/inertial_stats_auto_mass.sdf

diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc
index dc54a45d2..864de685e 100644
--- a/src/gz_TEST.cc
+++ b/src/gz_TEST.cc
@@ -2021,6 +2021,19 @@ TEST(inertial_stats, SDF)
     EXPECT_EQ(expectedOutput, output);
   }
 
+  // Check a good SDF file with auto-inertials and explicit mass
+  // from the same folder by passing a relative path
+  {
+    std::string path = "inertial_stats_auto_mass.sdf";
+    const auto pathBase = sdf::testing::TestFile("sdf");
+
+    std::string output =
+      custom_exec_str("cd " + pathBase + " && " +
+                      GzCommand() + " sdf --inertial-stats " +
+                      path + SdfVersion());
+    EXPECT_EQ(expectedOutput, output);
+  }
+
   expectedOutput =
           "Error Code " +
           std::to_string(static_cast<int>(
diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc
index 6d875b223..0a3b2add7 100644
--- a/test/integration/link_dom.cc
+++ b/test/integration/link_dom.cc
@@ -948,7 +948,7 @@ TEST(DOMLink, InertialAuto)
   const sdf::Link *link = model->LinkByName("link_1");
   ASSERT_NE(link, nullptr);
 
-  // Verify inertial values for link_1 match thos in inertial_stats.sdf
+  // Verify inertial values for link_1 match those in inertial_stats.sdf
   gz::math::Inertiald inertial = link->Inertial();
   gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
   EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
@@ -969,6 +969,40 @@ TEST(DOMLink, InertialAuto)
   EXPECT_FALSE(inertialElem->HasElement("pose"));
 }
 
+/////////////////////////////////////////////////
+TEST(DOMLink, InertialAutoExplicitMass)
+{
+  const std::string testFile = sdf::testing::TestFile("sdf",
+        "inertial_stats_auto_mass.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(model, nullptr);
+
+  std::vector<std::string> linkNames{"link_1", "link_2", "link_3", "link_4"};
+  for (const std::string &linkName : linkNames)
+  {
+    const sdf::Link *link = model->LinkByName(linkName);
+    ASSERT_NE(link, nullptr);
+
+    // Verify inertial values for link_i match those in inertial_stats.sdf
+    gz::math::Inertiald inertial = link->Inertial();
+    gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
+    EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
+    EXPECT_DOUBLE_EQ(6.0, massMatrix.Mass());
+    EXPECT_DOUBLE_EQ(1.0, massMatrix.Ixx());
+    EXPECT_DOUBLE_EQ(1.0, massMatrix.Iyy());
+    EXPECT_DOUBLE_EQ(1.0, massMatrix.Izz());
+    EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixy());
+    EXPECT_DOUBLE_EQ(0.0, massMatrix.Ixz());
+    EXPECT_DOUBLE_EQ(0.0, massMatrix.Iyz());
+  }
+}
+
 /////////////////////////////////////////////////
 TEST(DOMLink, InertialAutoSaveInElement)
 {
@@ -990,7 +1024,7 @@ TEST(DOMLink, InertialAutoSaveInElement)
   const sdf::Link *link = model->LinkByName("link_1");
   ASSERT_NE(link, nullptr);
 
-  // Verify inertial values for link_1 match thos in inertial_stats.sdf
+  // Verify inertial values for link_1 match those in inertial_stats.sdf
   gz::math::Inertiald inertial = link->Inertial();
   gz::math::MassMatrix3d massMatrix = inertial.MassMatrix();
   EXPECT_EQ(gz::math::Pose3d::Zero, inertial.Pose());
diff --git a/test/sdf/inertial_stats_auto_mass.sdf b/test/sdf/inertial_stats_auto_mass.sdf
new file mode 100644
index 000000000..b2b2c0381
--- /dev/null
+++ b/test/sdf/inertial_stats_auto_mass.sdf
@@ -0,0 +1,140 @@
+<?xml version="1.0" ?>
+<sdf version="1.11">
+  <!---
+  Model consists of 4 cubes places symmetrically in the XY plane.
+              +y
+              │
+            ┌─┼─┐
+         L3 │ │ │(0,5,0)
+            └─┼─┘
+              │
+    L2┌───┐   │     ┌───┐L1
+  ────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
+      └───┘   │     └───┘
+    (-5,0,0)  │     (5,0,0)
+            ┌─┼─┐
+            │ │ │(0,-5,0)
+            └─┼─┘
+            L4│
+  -->
+  <![CDATA[
+  This model is used to verify the "gz sdf --inertial-stats" tool.
+  ]]>
+    <model name="test_model">
+      <pose>0 0 0 0 0 0</pose>
+
+      <!-- set //inertial/density -->
+      <link name="link_1">
+        <pose>5 0 0 0 0 0</pose>
+        <inertial auto="true">
+          <density>6</density>
+        </inertial>
+        <collision name="collision_1">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_1">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <!-- set //inertial/mass and default collision density -->
+      <link name="link_2">
+        <pose>-5 0 0 0 0 0</pose>
+        <inertial auto="true">
+          <mass>6</mass>
+        </inertial>
+        <collision name="collision_2">
+          <density>1000</density>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_2">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <!-- use two half-cube collisions instead of single cube -->
+      <!-- set //inertial/mass and default collision density -->
+      <link name="link_3">
+        <pose>0 5 0 0 0 0</pose>
+        <inertial auto="true">
+          <mass>6</mass>
+        </inertial>
+        <collision name="collision_3_up">
+          <pose>0 0 0.25 0 0 0</pose>
+          <density>1000</density>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <collision name="collision_3_down">
+          <pose>0 0 -0.25 0 0 0</pose>
+          <density>1000</density>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_3">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <!-- use two half-cube collisions instead of single cube -->
+      <!-- set //inertial/mass and identical non-default collision densities -->
+      <link name="link_4">
+        <pose>0 -5 0 0 0 0</pose>
+        <inertial auto="true">
+          <mass>6</mass>
+        </inertial>
+        <collision name="collision_4_up">
+          <pose>0 0 0.25 0 0 0</pose>
+          <density>123.456</density>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <collision name="collision_4_down">
+          <pose>0 0 -0.25 0 0 0</pose>
+          <density>123.456</density>
+          <geometry>
+            <box>
+              <size>1 1 0.5</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_4">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+</sdf>