diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 8476a57a0..9baa677a0 100644 --- a/include/sdf/ParserConfig.hh +++ b/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. diff --git a/src/Link.cc b/src/Link.cc index 892cf3643..06de2fff8 100644 --- a/src/Link.cc +++ b/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( + totalInertia.Pose()); + inertialElem->GetElement("mass")->GetValue()->Set( + totalInertia.MassMatrix().Mass()); + auto momentOfInertiaElem = inertialElem->GetElement("inertia"); + momentOfInertiaElem->GetElement("ixx")->GetValue()->Set( + totalInertia.MassMatrix().Ixx()); + momentOfInertiaElem->GetElement("ixy")->GetValue()->Set( + totalInertia.MassMatrix().Ixy()); + momentOfInertiaElem->GetElement("ixz")->GetValue()->Set( + totalInertia.MassMatrix().Ixz()); + momentOfInertiaElem->GetElement("iyy")->GetValue()->Set( + totalInertia.MassMatrix().Iyy()); + momentOfInertiaElem->GetElement("iyz")->GetValue()->Set( + totalInertia.MassMatrix().Iyz()); + momentOfInertiaElem->GetElement("izz")->GetValue()->Set( + totalInertia.MassMatrix().Izz()); + } } // If auto is false, this means inertial values were set // from user given values in Link::Load(), therefore we can return diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 6d1e681a9..314a05fae 100644 --- a/src/ParserConfig_TEST.cc +++ b/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()); } diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index c4bd35f4c..8ba2c25fb 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/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 ] 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]))) diff --git a/src/gz.cc b/src/gz.cc index 7f9c881a8..5b79685e4 100644 --- a/src/gz.cc +++ b/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,7 +141,7 @@ 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)) { @@ -148,19 +149,15 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, 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; } diff --git a/src/gz_TEST.cc b/src/gz_TEST.cc index 2e16d19db..d58121345 100644 --- a/src/gz_TEST.cc +++ b/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"); } +///////////////////////////////////////////////// +TEST(print_auto_inertial, SDF) +{ + const auto path = sdf::testing::TestFile("sdf", "inertial_stats_auto.sdf"); + + { + // Print without --expand-auto-inertials + // expect no or elements + std::string output = custom_exec_str( + GzCommand() + " sdf -p " + path + + SdfVersion()); + ASSERT_FALSE(output.empty()); + EXPECT_PRED2(sdf::testing::notContains, output, ""); + EXPECT_PRED2(sdf::testing::notContains, output, ""); + } + + { + // Print with --expand-auto-inertials + // expect and 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, ""); + EXPECT_PRED2(sdf::testing::contains, output, ""); + } +} + ///////////////////////////////////////////////// TEST(GraphCmd, WorldPoseRelativeTo) { diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc index 31d493fe9..6d875b223 100644 --- a/test/integration/link_dom.cc +++ b/test/integration/link_dom.cc @@ -930,3 +930,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("mass")); + EXPECT_EQ(gz::math::Pose3d::Zero, + inertialElem->Get("pose")); + auto momentOfInertiaElem = inertialElem->FindElement("inertia"); + ASSERT_TRUE(momentOfInertiaElem != nullptr); + EXPECT_TRUE(momentOfInertiaElem->HasElement("ixx")); + EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get("ixx")); + EXPECT_TRUE(momentOfInertiaElem->HasElement("iyy")); + EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get("iyy")); + EXPECT_TRUE(momentOfInertiaElem->HasElement("izz")); + EXPECT_DOUBLE_EQ(1.0, momentOfInertiaElem->Get("izz")); + EXPECT_TRUE(momentOfInertiaElem->HasElement("ixy")); + EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get("ixy")); + EXPECT_TRUE(momentOfInertiaElem->HasElement("ixz")); + EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get("ixz")); + EXPECT_TRUE(momentOfInertiaElem->HasElement("iyz")); + EXPECT_DOUBLE_EQ(0.0, momentOfInertiaElem->Get("iyz")); +}