ParserConfig: save auto inertial values by default (#1325)

* Copy the inertial_stats.sdf file replace the user-defined inertial with //inertial/@auto=true and equivalent //collision/density values. * Change default value of ConfigureResolveAutoInertials to SAVE_CALCULATION to match previous API behavior. * Warn and return default inertial values if custom inertia calculator is unset. --------- Signed-off-by: Steve Peters <[email protected]> Signed-off-by: Addisu Z. Taddese <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
gazebosim · Sep 27, 2023 · eee6f5a · eee6f5a
1 parent 2ba6470
commit eee6f5a
Show file tree

Hide file tree

Showing 11 changed files with 189 additions and 42 deletions.
diff --git a/src/Geometry.cc b/src/Geometry.cc
@@ -312,6 +312,7 @@ void Geometry::SetPolylineShape(const std::vector<Polyline> &_polylines)
   this->dataPtr->polylines = _polylines;
 }
 
+/////////////////////////////////////////////////
 std::optional<gz::math::Inertiald> Geometry::CalculateInertial(
   sdf::Errors &_errors, const ParserConfig &_config,
   double _density, sdf::ElementPtr _autoInertiaParams)

diff --git a/src/Link.cc b/src/Link.cc
@@ -191,9 +191,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
       {
         Error err(
           ErrorCode::WARNING,
-          "Inertial was used with auto=true for link, " +
-          this->dataPtr->name + " but user defined inertial values "
-          "were found. They would be overwritten by computed ones."
+          "Inertial was used with auto=true for the link named " +
+          this->dataPtr->name + ", but user-defined inertial values were "
+          "found, which will be overwritten by the computed inertial values."
         );
         enforceConfigurablePolicyCondition(
           _config.WarningsPolicy(), err, errors);
@@ -610,7 +610,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
     {
       _errors.push_back({ErrorCode::ELEMENT_MISSING,
                         "Inertial is set to auto but there are no "
-                        "<collision> elements for the link."});
+                        "<collision> elements for the link named " +
+                        this->Name() + "."});
       return;
     }
 

diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc
@@ -293,18 +293,13 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink)
   sdf::Root root;
   const sdf::ParserConfig sdfParserConfig;
   sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
-  EXPECT_TRUE(errors.empty());
+  ASSERT_EQ(1u, errors.size()) << errors;
+  EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
+  EXPECT_NE(std::string::npos,
+            errors[0].Message().find(
+                "Inertial is set to auto but there are no <collision> elements "
+                "for the link named link."));
   EXPECT_NE(nullptr, root.Element());
-
-  const sdf::Model *model = root.Model();
-  const sdf::Link *link = model->LinkByIndex(0);
-  root.ResolveAutoInertials(errors, sdfParserConfig);
-  ASSERT_FALSE(errors.empty());
-
-  // Default Inertial values set during load
-  EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass());
-  EXPECT_EQ(gz::math::Vector3d::One,
-    link->Inertial().MassMatrix().DiagonalMoments());
 }
 
 /////////////////////////////////////////////////

diff --git a/src/Mesh.cc b/src/Mesh.cc
@@ -209,6 +209,22 @@ std::optional<gz::math::Inertiald> Mesh::CalculateInertial(sdf::Errors &_errors,
 
   const auto &customCalculator = _config.CustomInertiaCalc();
 
+  if (!customCalculator)
+  {
+    Error err(
+        sdf::ErrorCode::WARNING,
+        "Custom moment of inertia calculator for meshes not set via "
+        "sdf::ParserConfig::RegisterCustomInertiaCalc, using default "
+        "inertial values.");
+    enforceConfigurablePolicyCondition(
+          _config.WarningsPolicy(), err, _errors);
+
+    using namespace gz::math;
+    return Inertiald(
+        MassMatrix3d(1, Vector3d::One, Vector3d::Zero),
+        Pose3d::Zero);
+  }
+
   sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties(
     _density, *this, _autoInertiaParams);
 

diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc
@@ -45,10 +45,10 @@ class sdf::ParserConfig::Implementation
   public: std::optional<EnforcementPolicy> deprecatedElementsPolicy;
 
   /// \brief Configuration that is set for the CalculateInertial() function
-  /// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause
-  /// Root::Load() to not call CalculateInertial()
+  /// By default it is set to SAVE_CALCULATION to preserve the behavior of
+  /// Root::Load() generating complete inertial information.
   public: ConfigureResolveAutoInertials resolveAutoInertialsConfig =
-    ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD;
+    ConfigureResolveAutoInertials::SAVE_CALCULATION;
 
   /// \brief Collection of custom model parsers.
   public: std::vector<CustomModelParser> customParsers;

diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc
@@ -57,11 +57,11 @@ TEST(ParserConfig, Construction)
   EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy());
   EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy());
 
-  EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
+  EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION,
     config.CalculateInertialConfiguration());
   config.SetCalculateInertialConfiguration(
-    sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION);
-  EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION,
+    sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
+  EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
     config.CalculateInertialConfiguration());
   EXPECT_FALSE(config.URDFPreserveFixedJoint());
   EXPECT_FALSE(config.StoreResolvedURIs());

diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc
@@ -1889,7 +1889,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF))
 
   // Check a good SDF file from the same folder by passing a relative path
   {
-    std::string path = "inertial_stats.sdf";
+    std::string path = "inertial_stats_auto.sdf";
 
     std::string output =
       custom_exec_str("cd " + pathBase + " && " +

diff --git a/test/integration/root_dom.cc b/test/integration/root_dom.cc
@@ -300,3 +300,13 @@ TEST(DOMRoot, CreateMulipleWorlds)
   testFunc(root);
   testFunc(root2);
 }
+
+/////////////////////////////////////////////////
+TEST(DOMRoot, LoadWithoutMeshAutoInertialCalculator)
+{
+  const std::string testFile =
+    sdf::testing::TestFile("sdf", "mesh_auto_inertial.sdf");
+  sdf::Root root;
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+}
diff --git a/test/sdf/inertial_stats.sdf b/test/sdf/inertial_stats.sdf
@@ -1,25 +1,25 @@
 <?xml version="1.0" ?>
-<!---
-Model consists of 4 cubes places symmetrically
-in the XY plane. This model is used to verify the
-"gz sdf --inertial-stats" tool .
-            +y
-            │
-          ┌─┼─┐
-       L3 │ │ │(0,5,0)
-          └─┼─┘
-            │
-  L2┌───┐   │     ┌───┐L1
-────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
-    └───┘   │     └───┘
-  (-5,0,0)  │     (5,0,0)
-          ┌─┼─┐
-          │ │ │(0,-5,0)
-          └─┼─┘
-          L4│
--->
 <sdf version="1.6">
-
+  <!---
+  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>
 

diff --git a/test/sdf/inertial_stats_auto.sdf b/test/sdf/inertial_stats_auto.sdf
@@ -0,0 +1,107 @@
+<?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>
+
+      <link name="link_1">
+        <pose>5 0 0 0 0 0</pose>
+        <inertial auto='true' />
+        <collision name="collision_1">
+          <density>6</density>
+          <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>
+
+      <link name="link_2">
+        <pose>-5 0 0 0 0 0</pose>
+        <inertial auto='true' />
+        <collision name="collision_2">
+          <density>6</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>
+
+      <link name="link_3">
+        <pose>0 5 0 0 0 0</pose>
+        <inertial auto='true' />
+        <collision name="collision_3">
+          <density>6</density>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_3">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+
+      <link name="link_4">
+        <pose>0 -5 0 0 0 0</pose>
+        <inertial auto='true' />
+        <collision name="collision_4">
+          <density>6</density>
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </collision>
+        <visual name="visual_4">
+          <geometry>
+            <box>
+              <size>1 1 1</size>
+            </box>
+          </geometry>
+        </visual>
+      </link>
+    </model>
+
+</sdf>
diff --git a/test/sdf/mesh_auto_inertial.sdf b/test/sdf/mesh_auto_inertial.sdf
@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<!-- Since sdformat itself doesn't have an moment of inertia calculator for meshes, this file is not expected to load without errors -->
+<sdf version='1.11'>
+  <model name='test_model'>
+    <link name='L1'>
+      <inertial auto='true'/>
+      <collision name="collision">
+        <geometry>
+          <mesh>
+            <uri>box.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+    </link>
+  </model>
+</sdf>
+