Print auto inertial values with gz sdf --print --expand-auto-inertials (backport #1422)

include/sdf/ParserConfig.hh

@@ -60,7 +60,13 @@ enum class ConfigureResolveAutoInertials
 
   /// \brief If this values is used, CalculateInertial() would be
   /// called and the computed inertial values would be saved
-  SAVE_CALCULATION
+  SAVE_CALCULATION,
+
+  /// \brief If this values is used, CalculateInertial() would be
+  /// called and the computed inertial values would be saved and
+  /// written to the XML Element, allowing the calculated values
+  /// to be printed with `gz sdf --print`.
+  SAVE_CALCULATION_IN_ELEMENT,
 };
 
 // Forward declare private data class.

src/Link.cc

@@ -684,6 +684,30 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
     {
       this->dataPtr->autoInertiaSaved = true;
     }
+    else if (_config.CalculateInertialConfiguration() ==
+      ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT)
+    {
+      this->dataPtr->autoInertiaSaved = true;
+      // Write calculated inertia values to //link/inertial element
+      auto inertialElem = this->dataPtr->sdf->GetElement("inertial");
+      inertialElem->GetElement("pose")->GetValue()->Set<gz::math::Pose3d>(
+        totalInertia.Pose());
+      inertialElem->GetElement("mass")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Mass());
+      auto momentOfInertiaElem = inertialElem->GetElement("inertia");
+      momentOfInertiaElem->GetElement("ixx")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Ixx());
+      momentOfInertiaElem->GetElement("ixy")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Ixy());
+      momentOfInertiaElem->GetElement("ixz")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Ixz());
+      momentOfInertiaElem->GetElement("iyy")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Iyy());
+      momentOfInertiaElem->GetElement("iyz")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Iyz());
+      momentOfInertiaElem->GetElement("izz")->GetValue()->Set<double>(
+        totalInertia.MassMatrix().Izz());
+    }
   }
   // If auto is false, this means inertial values were set
   // from user given values in Link::Load(), therefore we can return

src/ParserConfig_TEST.cc

@@ -63,6 +63,11 @@ TEST(ParserConfig, Construction)
     sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
   EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
     config.CalculateInertialConfiguration());
+  config.SetCalculateInertialConfiguration(
+    sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);
+  EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT,
+    config.CalculateInertialConfiguration());
+
   EXPECT_FALSE(config.URDFPreserveFixedJoint());
   EXPECT_FALSE(config.StoreResolvedURIs());
 }

src/cmd/cmdsdformat.rb.in

@@ -44,16 +44,18 @@ COMMANDS = {  'sdf' =>
                        "  -d [ --describe ] [SPEC VERSION]  Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" +
                        "  -g [ --graph ] <pose, frame> arg  Print the PoseRelativeTo or FrameAttachedTo graph. (WARNING: This is for advanced\n" +
                        "                                    use only and the output may change without any promise of stability)\n" +
+                       "  --inertial-stats arg              Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
                        "  -p [ --print ] arg                Print converted arg. Note the quaternion representation of the\n" +
                        "                                    rotational part of poses and unit vectors will be normalized.\n" +
                        "      -i [ --preserve-includes ]    Preserve included tags when printing converted arg (does not preserve merge-includes).\n" +
                        "      --degrees                     Pose rotation angles are printed in degrees.\n" +
+                       "      --expand-auto-inertials       Prints auto-computed inertial values for simple shapes. For meshes and other unsupported\n" +
+                       "                                    shapes, the default inertial values will be printed.\n" +
                        "      --snap-to-degrees arg         Snap pose rotation angles to this specified interval in degrees. This value must be\n" +
                        "                                    larger than 0, less than or equal to 360, and larger than the defined snap tolerance.\n" +
                        "      --snap-tolerance arg          Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" +
                        "                                    occurs. This value must be larger than 0, less than 360, and less than the defined\n" +
                        "                                    degrees value to snap to. If unspecified, its default value is 0.01.\n" +
-                       "      --inertial-stats  arg         Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
                        "      --precision arg               Set the output stream precision for floating point numbers. The arg must be a positive integer.\n" +
 
                        COMMON_OPTIONS
@@ -70,6 +72,7 @@ class Cmd
   def parse(args)
     options = {}
     options['degrees'] = 0
+    options['expand_auto_inertials'] = 0
     options['snap_tolerance'] = 0.01
     options['preserve_includes'] = 0
 
@@ -104,6 +107,9 @@ class Cmd
       opts.on('--degrees', 'Printed pose rotations are will be in degrees') do |degrees|
         options['degrees'] = 1
       end
+      opts.on('--expand-auto-inertials', 'Auto-computed inertial values will be printed') do
+        options['expand_auto_inertials'] = 1
+      end
       opts.on('--snap-to-degrees arg', Integer,
               'Printed rotations are snapped to specified degree intervals') do |arg|
         if arg == 0 || arg > 360
@@ -243,13 +249,14 @@ class Cmd
           if options.key?('precision')
             precision = options['precision']
           end
-          Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int)'
+          Importer.extern 'int cmdPrint(const char *, int in_degrees, int snap_to_degrees, float snap_tolerance, int, int, int)'
           exit(Importer.cmdPrint(File.expand_path(options['print']),
                                  options['degrees'],
                                  snap_to_degrees,
                                  options['snap_tolerance'],
                                  options['preserve_includes'],
-                                 precision))
+                                 precision,
+                                 options['expand_auto_inertials']))
         elsif options.key?('graph')
           Importer.extern 'int cmdGraph(const char *, const char *)'
           exit(Importer.cmdGraph(options['graph'][:type], File.expand_path(ARGV[1])))

src/gz.cc

@@ -28,6 +28,7 @@
 #include "sdf/Model.hh"
 #include "sdf/Root.hh"
 #include "sdf/parser.hh"
+#include "sdf/ParserConfig.hh"
 #include "sdf/PrintConfig.hh"
 #include "sdf/system_util.hh"
 
@@ -140,27 +141,23 @@ extern "C" SDFORMAT_VISIBLE int cmdDescribe(const char *_version)
 //////////////////////////////////////////////////
 extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path,
     int _inDegrees, int _snapToDegrees, float _snapTolerance,
-    int _preserveIncludes, int _outPrecision)
+    int _preserveIncludes, int _outPrecision, int _expandAutoInertials)
 {
   if (!sdf::filesystem::exists(_path))
   {
     std::cerr << "Error: File [" << _path << "] does not exist.\n";
     return -1;
   }
 
-  sdf::SDFPtr sdf(new sdf::SDF());
-
-  if (!sdf::init(sdf))
+  sdf::ParserConfig parserConfig;
+  if (_expandAutoInertials)
   {
-    std::cerr << "Error: SDF schema initialization failed.\n";
-    return -1;
+    parserConfig.SetCalculateInertialConfiguration(
+      sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);
   }
 
-  if (!sdf::readFile(_path, sdf))
-  {
-    std::cerr << "Error: SDF parsing the xml failed.\n";
-    return -1;
-  }
+  sdf::Root root;
+  sdf::Errors errors = root.Load(_path, parserConfig);
 
   sdf::PrintConfig config;
   if (_inDegrees != 0)
@@ -180,7 +177,16 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path,
   if (_outPrecision > 0)
     config.SetOutPrecision(_outPrecision);
 
-  sdf->PrintValues(config);
+  if (root.Element())
+  {
+    root.Element()->PrintValues(errors, "", config);
+  }
+
+  if (!errors.empty())
+  {
+    std::cerr << errors << std::endl;
+    return -1;
+  }
   return 0;
 }
 

src/gz_TEST.cc

@@ -1098,7 +1098,8 @@ TEST(print, SDF)
     // Check box_bad_test.world
     std::string output =
       custom_exec_str(GzCommand() + " sdf -p " + path + SdfVersion());
-    EXPECT_TRUE(output.find("Required attribute") != std::string::npos);
+    EXPECT_TRUE(output.find("Required attribute") != std::string::npos)
+      << output;
   }
 }
 
@@ -1726,6 +1727,35 @@ TEST(print_snap_to_degrees_tolerance_too_high, SDF)
                "1 2 3   30 50 60</pose>");
 }
 
+/////////////////////////////////////////////////
+TEST(print_auto_inertial, SDF)
+{
+  const auto path = sdf::testing::TestFile("sdf", "inertial_stats_auto.sdf");
+
+  {
+    // Print without --expand-auto-inertials
+    // expect no <mass> or <inertia> elements
+    std::string output = custom_exec_str(
+        GzCommand() + " sdf -p " + path +
+        SdfVersion());
+    ASSERT_FALSE(output.empty());
+    EXPECT_PRED2(sdf::testing::notContains, output, "<mass>");
+    EXPECT_PRED2(sdf::testing::notContains, output, "<inertia>");
+  }
+
+  {
+    // Print with --expand-auto-inertials
+    // expect <mass> and <inertia> elements
+    std::string output = custom_exec_str(
+        GzCommand() + " sdf -p " + path +
+        " --expand-auto-inertials " +
+        SdfVersion());
+    ASSERT_FALSE(output.empty());
+    EXPECT_PRED2(sdf::testing::contains, output, "<mass>");
+    EXPECT_PRED2(sdf::testing::contains, output, "<inertia>");
+  }
+}
+
 /////////////////////////////////////////////////
 TEST(GraphCmd, WorldPoseRelativeTo)
 {

test/integration/link_dom.cc

@@ -929,3 +929,98 @@ TEST(DOMLink, InvalidInertialPoseRelTo)
   EXPECT_EQ(link->Inertial().Pose(),
       gz::math::Pose3d(0.1, 1, 0.2, 0, 0, -0.52));
 }
+
+/////////////////////////////////////////////////
+TEST(DOMLink, InertialAuto)
+{
+  const std::string testFile = sdf::testing::TestFile("sdf",
+        "inertial_stats_auto.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);
+
+  const sdf::Link *link = model->LinkByName("link_1");
+  ASSERT_NE(link, nullptr);
+
+  // Verify inertial values for link_1 match thos 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());
+
+  // Expect //link/inertial element to not contain inertia, mass, or pose
+  auto linkElem = link->Element();
+  auto inertialElem = linkElem->FindElement("inertial");
+  ASSERT_TRUE(inertialElem != nullptr);
+  EXPECT_FALSE(inertialElem->HasElement("inertia"));
+  EXPECT_FALSE(inertialElem->HasElement("mass"));
+  EXPECT_FALSE(inertialElem->HasElement("pose"));
+}
+
+/////////////////////////////////////////////////
+TEST(DOMLink, InertialAutoSaveInElement)
+{
+  const std::string testFile = sdf::testing::TestFile("sdf",
+        "inertial_stats_auto.sdf");
+
+  // Load the SDF file with SAVE_CALCULATION_IN_ELEMENT
+  sdf::ParserConfig config;
+  config.SetCalculateInertialConfiguration(
+    sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT);
+
+  sdf::Root root;
+  auto errors = root.Load(testFile, config);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(model, nullptr);
+
+  const sdf::Link *link = model->LinkByName("link_1");
+  ASSERT_NE(link, nullptr);
+
+  // Verify inertial values for link_1 match thos 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());
+
+  // Expect //link/inertial element to contain inertia values
+  auto linkElem = link->Element();
+  auto inertialElem = linkElem->FindElement("inertial");
+  ASSERT_TRUE(inertialElem != nullptr);
+  EXPECT_TRUE(inertialElem->HasElement("mass"));
+  EXPECT_DOUBLE_EQ(6.0, inertialElem->Get<double>("mass"));
+  EXPECT_EQ(gz::math::Pose3d::Zero,
+    inertialElem->Get<gz::math::Pose3d>("pose"));
+  auto momentOfInertiaElem = inertialElem->FindElement("inertia");
+  ASSERT_TRUE(momentOfInertiaElem != nullptr);
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("ixx"));
+  EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("ixx"));
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("iyy"));
+  EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("iyy"));
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("izz"));
+  EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get<double>("izz"));
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("ixy"));
+  EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("ixy"));
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("ixz"));
+  EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("ixz"));
+  EXPECT_TRUE(momentOfInertiaElem->HasElement("iyz"));
+  EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get<double>("iyz"));
+}