From 6380fb72fda9be703add2581feb4d7d2072cb0ce Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 16:26:25 -0700 Subject: [PATCH 1/6] ParserConfig: support printing auto-inertial data This adds a SAVE_CALCULATION_IN_ELEMENT value to the ConfigureResolveAutoInertials enum, which is similar to the SAVE_CALCULATION value but also stores the resulting inertial data in the Link's Element so that it can be printed. Signed-off-by: Steve Peters --- include/sdf/ParserConfig.hh | 8 +++++++- src/ParserConfig_TEST.cc | 5 +++++ 2 files changed, 12 insertions(+), 1 deletion(-) 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/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()); } From 6b5b24577dad1c09a4f4f01e37dd3fa155b4dc20 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 16:33:26 -0700 Subject: [PATCH 2/6] Link: store auto-computed inertial data in Element If the SAVE_CALCULATION_IN_ELEMENT value is set in ParserConfig, then store auto-computed inertial data in the Link's Element. Confirm with a test added to INTEGRATION_link_dom. Signed-off-by: Steve Peters --- src/Link.cc | 24 +++++++++ test/integration/link_dom.cc | 95 ++++++++++++++++++++++++++++++++++++ 2 files changed, 119 insertions(+) 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/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")); +} From b17da3f8cee6e431360475515a1af0be13a63741 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 16:35:29 -0700 Subject: [PATCH 3/6] cmdsdformat: clean up help string The --inertial-stats argument represents a command, so move its help string and change indentation to match other commands, so that it doesn't look like a sub-parameter of --print. Signed-off-by: Steve Peters --- src/cmd/cmdsdformat.rb.in | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 7c93d7680..cd44234cf 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -42,6 +42,7 @@ 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" + @@ -51,7 +52,6 @@ COMMANDS = { 'sdf' => " --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 From 25f2dea950537d79b6112b69861d89819f3c3f1f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 16:36:50 -0700 Subject: [PATCH 4/6] gz sdf --print: add --expand-auto-inertials The `--expand-auto-inertials` option to `gz sdf --print` will print auto-computed inertial values. Signed-off-by: Steve Peters --- src/cmd/cmdsdformat.rb.in | 10 ++++++++-- src/gz.cc | 27 +++++++++++++++------------ src/gz_TEST.cc | 33 ++++++++++++++++++++++++++++++++- 3 files changed, 55 insertions(+), 15 deletions(-) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index cd44234cf..0eb7a4f5d 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -47,6 +47,7 @@ COMMANDS = { 'sdf' => " 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.\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" + @@ -68,6 +69,7 @@ class Cmd def parse(args) options = {} options['degrees'] = 0 + options['expand_auto_inertials'] = 0 options['snap_tolerance'] = 0.01 options['preserve_includes'] = 0 @@ -102,6 +104,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 @@ -235,13 +240,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..b482bed77 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,13 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, if (_outPrecision > 0) config.SetOutPrecision(_outPrecision); - sdf->PrintValues(config); + 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 816882f56..1d7288bd9 100644 --- a/src/gz_TEST.cc +++ b/src/gz_TEST.cc @@ -1098,7 +1098,8 @@ TEST(print, GZ_UTILS_TEST_DISABLED_ON_WIN32(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; } } @@ -1730,6 +1731,36 @@ TEST(print_snap_to_degrees_tolerance_too_high, "1 2 3 30 50 60"); } +///////////////////////////////////////////////// +TEST(print_auto_inertial, + GZ_UTILS_TEST_DISABLED_ON_WIN32(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, GZ_UTILS_TEST_DISABLED_ON_WIN32(WorldPoseRelativeTo)) { From 2ffda3d91282565c4d2ef1d6a76349c7537a7448 Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Mon, 20 May 2024 17:26:57 -0700 Subject: [PATCH 5/6] fix test Signed-off-by: Steve Peters --- src/gz.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/gz.cc b/src/gz.cc index b482bed77..5b79685e4 100644 --- a/src/gz.cc +++ b/src/gz.cc @@ -177,7 +177,10 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path, if (_outPrecision > 0) config.SetOutPrecision(_outPrecision); - root.Element()->PrintValues(errors, "", config); + if (root.Element()) + { + root.Element()->PrintValues(errors, "", config); + } if (!errors.empty()) { From f32393b711bf77948c44abfbb8f7844792d2fa0f Mon Sep 17 00:00:00 2001 From: Steve Peters Date: Fri, 24 May 2024 12:46:00 -0700 Subject: [PATCH 6/6] Clarify that mesh inertial data is not computed Update doc-string for `gz sdf --print --expand-auto-inertials` to clarify that mesh inertial data is not computed. Signed-off-by: Steve Peters --- src/cmd/cmdsdformat.rb.in | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index ba44814c1..8ba2c25fb 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -49,7 +49,8 @@ COMMANDS = { 'sdf' => " 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.\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" +