diff --git a/src/cmd/cmdsdformat.rb.in b/src/cmd/cmdsdformat.rb.in index 04562f03f..9e528ec9c 100644 --- a/src/cmd/cmdsdformat.rb.in +++ b/src/cmd/cmdsdformat.rb.in @@ -50,6 +50,7 @@ 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" + COMMON_OPTIONS } @@ -81,6 +82,10 @@ class Cmd 'Check if an SDFormat file is valid.') do |arg| options['check'] = arg end + opts.on('--inertial-stats arg', String, + 'Prints moment of inertia, centre of mass, and total mass from a model sdf file.') do |arg| + options['inertial_stats'] = arg + end opts.on('-d', '--describe [VERSION]', 'Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@)') do |v| options['describe'] = v end @@ -201,6 +206,9 @@ class Cmd if options.key?('check') Importer.extern 'int cmdCheck(const char *)' exit(Importer.cmdCheck(File.expand_path(options['check']))) + elsif options.key?('inertial_stats') + Importer.extern 'int cmdInertialStats(const char *)' + exit(Importer.cmdInertialStats(options['inertial_stats'])) elsif options.key?('describe') Importer.extern 'int cmdDescribe(const char *)' exit(Importer.cmdDescribe(options['describe'])) diff --git a/src/ign.cc b/src/ign.cc index 23f9625e8..43e39b2b9 100644 --- a/src/ign.cc +++ b/src/ign.cc @@ -20,14 +20,19 @@ #include #include #include +#include #include "sdf/sdf_config.h" #include "sdf/Filesystem.hh" +#include "sdf/Link.hh" +#include "sdf/Model.hh" #include "sdf/Root.hh" #include "sdf/parser.hh" #include "sdf/PrintConfig.hh" #include "sdf/system_util.hh" +#include "ignition/math/Inertial.hh" + #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "ign.hh" @@ -267,3 +272,111 @@ extern "C" SDFORMAT_VISIBLE int cmdGraph( return 0; } + +////////////////////////////////////////////////// +extern "C" SDFORMAT_VISIBLE int cmdInertialStats( + const char *_path) +{ + if (!sdf::filesystem::exists(_path)) + { + std::cerr << "Error: File [" << _path << "] does not exist.\n"; + return -1; + } + + sdf::Root root; + sdf::Errors errors = root.Load(_path); + if (!errors.empty()) + { + std::cerr << errors << std::endl; + return -1; + } + + if (root.WorldCount() > 0) + { + std::cerr << "Error: Expected a model file but received a world file." + << std::endl; + return -1; + } + + const sdf::Model *model = root.Model(); + if (!model) + { + std::cerr << "Error: Could not find the model." << std::endl; + return -1; + } + + if (model->ModelCount() > 0) + { + std::cout << "Warning: Inertial properties of links in nested" + " models will not be included." << std::endl; + } + + ignition::math::Inertiald totalInertial; + + for (uint64_t i = 0; i < model->LinkCount(); i++) + { + ignition::math::Pose3d linkPoseRelativeToModel; + errors = model->LinkByIndex(i)->SemanticPose(). + Resolve(linkPoseRelativeToModel, "__model__"); + + auto currentLinkInertial = model->LinkByIndex(i)->Inertial(); + currentLinkInertial.SetPose(linkPoseRelativeToModel * + currentLinkInertial.Pose()); + totalInertial += currentLinkInertial; + } + + auto totalMass = totalInertial.MassMatrix().Mass(); + auto xCentreOfMass = totalInertial.Pose().Pos().X(); + auto yCentreOfMass = totalInertial.Pose().Pos().Y(); + auto zCentreOfMass = totalInertial.Pose().Pos().Z(); + + std::cout << "Inertial statistics for model: " << model->Name() << std::endl; + std::cout << "---" << std::endl; + std::cout << "Total mass of the model: " << totalMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Centre of mass in model frame: " << std::endl; + std::cout << "X: " << xCentreOfMass << std::endl; + std::cout << "Y: " << yCentreOfMass << std::endl; + std::cout << "Z: " << zCentreOfMass << std::endl; + std::cout << "---" << std::endl; + + std::cout << "Moment of inertia matrix: " << std::endl; + + // Pretty print the MOI matrix + std::stringstream ss; + ss << totalInertial.Moi(); + + std::string s; + auto maxLength = 0u; + std::vector moiVector; + while ( std::getline(ss, s, ' ' ) ) + { + moiVector.push_back(s); + if (s.size() > maxLength) + { + maxLength = s.size(); + } + } + + for (int i = 0; i < 9; i++) + { + int spacePadding = maxLength - moiVector[i].size(); + // Print the matrix element + std::cout << moiVector[i]; + for (int j = 0; j < spacePadding; j++) + { + std::cout << " "; + } + // Add space for the next element + std::cout << " "; + // Add '\n' if the next row is about to start + if ((i+1)%3 == 0) + { + std::cout << "\n"; + } + } + std::cout << "---" << std::endl; + + return 0; +} diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc index d93172742..2376a4cf2 100644 --- a/src/ign_TEST.cc +++ b/src/ign_TEST.cc @@ -1795,6 +1795,75 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo)) EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output)); } +///////////////////////////////////////////////// +TEST(inertial_stats, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF)) +{ + std::string pathBase = PROJECT_SOURCE_PATH; + pathBase += "/test/sdf"; + + auto expectedOutput = + "Inertial statistics for model: test_model\n" + "---\n" + "Total mass of the model: 24\n" + "---\n" + "Centre of mass in model frame: \n" + "X: 0\n" + "Y: 0\n" + "Z: 0\n" + "---\n" + "Moment of inertia matrix: \n" + "304 0 0 \n" + "0 304 0 \n" + "0 0 604 \n" + "---\n"; + + // Check a good SDF file by passing the absolute path + { + std::string path = pathBase +"/inertial_stats.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + // Check a good SDF file from the same folder by passing a relative path + { + std::string path = "inertial_stats.sdf"; + + std::string output = + custom_exec_str("cd " + pathBase + " && " + + IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + expectedOutput = + "Error Code 18: Msg: A link named link has invalid inertia.\n\n"; + + // Check an invalid SDF file by passing the absolute path + { + std::string path = pathBase +"/inertial_invalid.sdf"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } + + expectedOutput = + "Error: Expected a model file but received a world file.\n"; + // Check a valid world file. + { + std::string path = pathBase +"/box_plane_low_friction_test.world"; + + std::string output = + custom_exec_str(IgnCommand() + " sdf --inertial-stats " + + path + SdfVersion()); + EXPECT_EQ(expectedOutput, output); + } +} + ///////////////////////////////////////////////// /// Main int main(int argc, char **argv) diff --git a/test/sdf/inertial_stats.sdf b/test/sdf/inertial_stats.sdf new file mode 100644 index 000000000..d8d6df8a7 --- /dev/null +++ b/test/sdf/inertial_stats.sdf @@ -0,0 +1,131 @@ + + + + + + 0 0 0 0 0 0 + + + 5 0 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + -5 0 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 5 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + 0 -5 0 0 0 0 + + 6.0 + + 1 + 1 + 1 + + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + +