Skip to content

Commit

Permalink
Ported changes from #936
Browse files Browse the repository at this point in the history 
Signed-off-by: Aditya <[email protected]>
  • Loading branch information
@adityapande-1995
adityapande-1995 committed Apr 8, 2022
1 parent 72b77c2 commit 72a7303
Show file tree
Hide file tree
Showing 4 changed files with 324 additions and 0 deletions.
8 changes: 8 additions & 0 deletions src/cmd/cmdsdformat.rb.in
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@ COMMANDS = { 'sdf' =>
" -k [ --check ] arg Check if an SDFormat file is valid.\n" +
" -d [ --describe ] [SPEC VERSION] Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@).\n" +
" -p [ --print ] arg Print converted arg.\n" +
" --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
COMMON_OPTIONS
}

Expand Down Expand Up @@ -70,6 +71,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
Expand Down Expand Up @@ -150,6 +155,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']))
Expand Down
116 changes: 116 additions & 0 deletions src/ign.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,13 +17,18 @@

#include <iostream>
#include <string.h>
#include <vector>

#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/system_util.hh"

#include "ignition/math/Inertial.hh"

#include "ign.hh"

//////////////////////////////////////////////////
Expand Down Expand Up @@ -153,3 +158,114 @@ extern "C" SDFORMAT_VISIBLE int cmdPrint(const char *_path)

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())
{
for (auto &error : errors)
{
std::cerr << "Error: " << error.Message() << 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.ModelByIndex(0);
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<std::string> 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;
}
69 changes: 69 additions & 0 deletions src/ign_TEST.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -732,6 +732,75 @@ TEST(print, SDF)
}
}

/////////////////////////////////////////////////
TEST(inertial_stats, 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)
Expand Down
131 changes: 131 additions & 0 deletions test/sdf/inertial_stats.sdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,131 @@
<?xml version="1.0" ?>
<!---
Model consists of 4 cubes places symmetrically
in the XY plane. This model is used to verify the
"ign 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 name="test_model">
<pose>0 0 0 0 0 0</pose>

<link name="link_1">
<pose>5 0 0 0 0 0</pose>
<inertial>
<mass>6.0</mass>
<inertia>
<ixx>1</ixx>
<iyy>1</iyy>
<izz>1</izz>
</inertia>
</inertial>
<collision name="collision_1">
<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>
<mass>6.0</mass>
<inertia>
<ixx>1</ixx>
<iyy>1</iyy>
<izz>1</izz>
</inertia>
</inertial>
<collision name="collision_2">
<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>
<mass>6.0</mass>
<inertia>
<ixx>1</ixx>
<iyy>1</iyy>
<izz>1</izz>
</inertia>
</inertial>
<collision name="collision_3">
<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>
<mass>6.0</mass>
<inertia>
<ixx>1</ixx>
<iyy>1</iyy>
<izz>1</izz>
</inertia>
</inertial>
<collision name="collision_4">
<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>

0 comments on commit 72a7303

Please sign in to comment.