Skip to content

Commit a3a1ba5

Browse files
ahcordeahcorde
authored
Merge branch 'sdf12' into ahcorde/usd_to_sdf_links
2 parents bd2f180 + 1b66d74 commit a3a1ba5

4 files changed

Lines changed: 321 additions & 0 deletions

File tree

‎src/cmd/cmdsdformat.rb.in‎

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ COMMANDS = { 'sdf' =>
5050
" --snap-tolerance arg Used in conjunction with --snap-to-degrees, specifies the tolerance at which snapping\n" +
5151
" occurs. This value must be larger than 0, less than 360, and less than the defined\n" +
5252
" degrees value to snap to. If unspecified, its default value is 0.01.\n" +
53+
" --inertial-stats arg Prints moment of inertia, centre of mass, and total mass from a model sdf file.\n" +
5354
COMMON_OPTIONS
5455
}
5556

@@ -81,6 +82,10 @@ class Cmd
8182
'Check if an SDFormat file is valid.') do |arg|
8283
options['check'] = arg
8384
end
85+
opts.on('--inertial-stats arg', String,
86+
'Prints moment of inertia, centre of mass, and total mass from a model sdf file.') do |arg|
87+
options['inertial_stats'] = arg
88+
end
8489
opts.on('-d', '--describe [VERSION]', 'Print the aggregated SDFormat spec description. Default version (@SDF_PROTOCOL_VERSION@)') do |v|
8590
options['describe'] = v
8691
end
@@ -201,6 +206,9 @@ class Cmd
201206
if options.key?('check')
202207
Importer.extern 'int cmdCheck(const char *)'
203208
exit(Importer.cmdCheck(File.expand_path(options['check'])))
209+
elsif options.key?('inertial_stats')
210+
Importer.extern 'int cmdInertialStats(const char *)'
211+
exit(Importer.cmdInertialStats(options['inertial_stats']))
204212
elsif options.key?('describe')
205213
Importer.extern 'int cmdDescribe(const char *)'
206214
exit(Importer.cmdDescribe(options['describe']))

‎src/ign.cc‎

Lines changed: 113 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,14 +20,19 @@
2020
#include <memory>
2121
#include <string>
2222
#include <string.h>
23+
#include <vector>
2324

2425
#include "sdf/sdf_config.h"
2526
#include "sdf/Filesystem.hh"
27+
#include "sdf/Link.hh"
28+
#include "sdf/Model.hh"
2629
#include "sdf/Root.hh"
2730
#include "sdf/parser.hh"
2831
#include "sdf/PrintConfig.hh"
2932
#include "sdf/system_util.hh"
3033

34+
#include "ignition/math/Inertial.hh"
35+
3136
#include "FrameSemantics.hh"
3237
#include "ScopedGraph.hh"
3338
#include "ign.hh"
@@ -267,3 +272,111 @@ extern "C" SDFORMAT_VISIBLE int cmdGraph(
267272

268273
return 0;
269274
}
275+
276+
//////////////////////////////////////////////////
277+
extern "C" SDFORMAT_VISIBLE int cmdInertialStats(
278+
const char *_path)
279+
{
280+
if (!sdf::filesystem::exists(_path))
281+
{
282+
std::cerr << "Error: File [" << _path << "] does not exist.\n";
283+
return -1;
284+
}
285+
286+
sdf::Root root;
287+
sdf::Errors errors = root.Load(_path);
288+
if (!errors.empty())
289+
{
290+
std::cerr << errors << std::endl;
291+
return -1;
292+
}
293+
294+
if (root.WorldCount() > 0)
295+
{
296+
std::cerr << "Error: Expected a model file but received a world file."
297+
<< std::endl;
298+
return -1;
299+
}
300+
301+
const sdf::Model *model = root.Model();
302+
if (!model)
303+
{
304+
std::cerr << "Error: Could not find the model." << std::endl;
305+
return -1;
306+
}
307+
308+
if (model->ModelCount() > 0)
309+
{
310+
std::cout << "Warning: Inertial properties of links in nested"
311+
" models will not be included." << std::endl;
312+
}
313+
314+
ignition::math::Inertiald totalInertial;
315+
316+
for (uint64_t i = 0; i < model->LinkCount(); i++)
317+
{
318+
ignition::math::Pose3d linkPoseRelativeToModel;
319+
errors = model->LinkByIndex(i)->SemanticPose().
320+
Resolve(linkPoseRelativeToModel, "__model__");
321+
322+
auto currentLinkInertial = model->LinkByIndex(i)->Inertial();
323+
currentLinkInertial.SetPose(linkPoseRelativeToModel *
324+
currentLinkInertial.Pose());
325+
totalInertial += currentLinkInertial;
326+
}
327+
328+
auto totalMass = totalInertial.MassMatrix().Mass();
329+
auto xCentreOfMass = totalInertial.Pose().Pos().X();
330+
auto yCentreOfMass = totalInertial.Pose().Pos().Y();
331+
auto zCentreOfMass = totalInertial.Pose().Pos().Z();
332+
333+
std::cout << "Inertial statistics for model: " << model->Name() << std::endl;
334+
std::cout << "---" << std::endl;
335+
std::cout << "Total mass of the model: " << totalMass << std::endl;
336+
std::cout << "---" << std::endl;
337+
338+
std::cout << "Centre of mass in model frame: " << std::endl;
339+
std::cout << "X: " << xCentreOfMass << std::endl;
340+
std::cout << "Y: " << yCentreOfMass << std::endl;
341+
std::cout << "Z: " << zCentreOfMass << std::endl;
342+
std::cout << "---" << std::endl;
343+
344+
std::cout << "Moment of inertia matrix: " << std::endl;
345+
346+
// Pretty print the MOI matrix
347+
std::stringstream ss;
348+
ss << totalInertial.Moi();
349+
350+
std::string s;
351+
auto maxLength = 0u;
352+
std::vector<std::string> moiVector;
353+
while ( std::getline(ss, s, ' ' ) )
354+
{
355+
moiVector.push_back(s);
356+
if (s.size() > maxLength)
357+
{
358+
maxLength = s.size();
359+
}
360+
}
361+
362+
for (int i = 0; i < 9; i++)
363+
{
364+
int spacePadding = maxLength - moiVector[i].size();
365+
// Print the matrix element
366+
std::cout << moiVector[i];
367+
for (int j = 0; j < spacePadding; j++)
368+
{
369+
std::cout << " ";
370+
}
371+
// Add space for the next element
372+
std::cout << " ";
373+
// Add '\n' if the next row is about to start
374+
if ((i+1)%3 == 0)
375+
{
376+
std::cout << "\n";
377+
}
378+
}
379+
std::cout << "---" << std::endl;
380+
381+
return 0;
382+
}

‎src/ign_TEST.cc‎

Lines changed: 69 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1795,6 +1795,75 @@ TEST(GraphCmd, IGN_UTILS_TEST_DISABLED_ON_WIN32(ModelFrameAttachedTo))
17951795
EXPECT_EQ(sdf::trim(expected.str()), sdf::trim(output));
17961796
}
17971797

1798+
/////////////////////////////////////////////////
1799+
TEST(inertial_stats, IGN_UTILS_TEST_DISABLED_ON_WIN32(SDF))
1800+
{
1801+
std::string pathBase = PROJECT_SOURCE_PATH;
1802+
pathBase += "/test/sdf";
1803+
1804+
auto expectedOutput =
1805+
"Inertial statistics for model: test_model\n"
1806+
"---\n"
1807+
"Total mass of the model: 24\n"
1808+
"---\n"
1809+
"Centre of mass in model frame: \n"
1810+
"X: 0\n"
1811+
"Y: 0\n"
1812+
"Z: 0\n"
1813+
"---\n"
1814+
"Moment of inertia matrix: \n"
1815+
"304 0 0 \n"
1816+
"0 304 0 \n"
1817+
"0 0 604 \n"
1818+
"---\n";
1819+
1820+
// Check a good SDF file by passing the absolute path
1821+
{
1822+
std::string path = pathBase +"/inertial_stats.sdf";
1823+
1824+
std::string output =
1825+
custom_exec_str(IgnCommand() + " sdf --inertial-stats " +
1826+
path + SdfVersion());
1827+
EXPECT_EQ(expectedOutput, output);
1828+
}
1829+
1830+
// Check a good SDF file from the same folder by passing a relative path
1831+
{
1832+
std::string path = "inertial_stats.sdf";
1833+
1834+
std::string output =
1835+
custom_exec_str("cd " + pathBase + " && " +
1836+
IgnCommand() + " sdf --inertial-stats " +
1837+
path + SdfVersion());
1838+
EXPECT_EQ(expectedOutput, output);
1839+
}
1840+
1841+
expectedOutput =
1842+
"Error Code 18: Msg: A link named link has invalid inertia.\n\n";
1843+
1844+
// Check an invalid SDF file by passing the absolute path
1845+
{
1846+
std::string path = pathBase +"/inertial_invalid.sdf";
1847+
1848+
std::string output =
1849+
custom_exec_str(IgnCommand() + " sdf --inertial-stats " +
1850+
path + SdfVersion());
1851+
EXPECT_EQ(expectedOutput, output);
1852+
}
1853+
1854+
expectedOutput =
1855+
"Error: Expected a model file but received a world file.\n";
1856+
// Check a valid world file.
1857+
{
1858+
std::string path = pathBase +"/box_plane_low_friction_test.world";
1859+
1860+
std::string output =
1861+
custom_exec_str(IgnCommand() + " sdf --inertial-stats " +
1862+
path + SdfVersion());
1863+
EXPECT_EQ(expectedOutput, output);
1864+
}
1865+
}
1866+
17981867
/////////////////////////////////////////////////
17991868
/// Main
18001869
int main(int argc, char **argv)

‎test/sdf/inertial_stats.sdf‎

Lines changed: 131 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,131 @@
1+
<?xml version="1.0" ?>
2+
<!---
3+
Model consists of 4 cubes places symmetrically
4+
in the XY plane. This model is used to verify the
5+
"ign sdf --inertial-stats" tool .
6+
+y
7+
│
8+
┌─┼─┐
9+
L3 │ │ │(0,5,0)
10+
└─┼─┘
11+
│
12+
L2┌───┐ │ ┌───┐L1
13+
────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
14+
└───┘ │ └───┘
15+
(-5,0,0) │ (5,0,0)
16+
┌─┼─┐
17+
│ │ │(0,-5,0)
18+
└─┼─┘
19+
L4│
20+
-->
21+
<sdf version="1.6">
22+
23+
<model name="test_model">
24+
<pose>0 0 0 0 0 0</pose>
25+
26+
<link name="link_1">
27+
<pose>5 0 0 0 0 0</pose>
28+
<inertial>
29+
<mass>6.0</mass>
30+
<inertia>
31+
<ixx>1</ixx>
32+
<iyy>1</iyy>
33+
<izz>1</izz>
34+
</inertia>
35+
</inertial>
36+
<collision name="collision_1">
37+
<geometry>
38+
<box>
39+
<size>1 1 1</size>
40+
</box>
41+
</geometry>
42+
</collision>
43+
<visual name="visual_1">
44+
<geometry>
45+
<box>
46+
<size>1 1 1</size>
47+
</box>
48+
</geometry>
49+
</visual>
50+
</link>
51+
52+
<link name="link_2">
53+
<pose>-5 0 0 0 0 0</pose>
54+
<inertial>
55+
<mass>6.0</mass>
56+
<inertia>
57+
<ixx>1</ixx>
58+
<iyy>1</iyy>
59+
<izz>1</izz>
60+
</inertia>
61+
</inertial>
62+
<collision name="collision_2">
63+
<geometry>
64+
<box>
65+
<size>1 1 1</size>
66+
</box>
67+
</geometry>
68+
</collision>
69+
<visual name="visual_2">
70+
<geometry>
71+
<box>
72+
<size>1 1 1</size>
73+
</box>
74+
</geometry>
75+
</visual>
76+
</link>
77+
78+
<link name="link_3">
79+
<pose>0 5 0 0 0 0</pose>
80+
<inertial>
81+
<mass>6.0</mass>
82+
<inertia>
83+
<ixx>1</ixx>
84+
<iyy>1</iyy>
85+
<izz>1</izz>
86+
</inertia>
87+
</inertial>
88+
<collision name="collision_3">
89+
<geometry>
90+
<box>
91+
<size>1 1 1</size>
92+
</box>
93+
</geometry>
94+
</collision>
95+
<visual name="visual_3">
96+
<geometry>
97+
<box>
98+
<size>1 1 1</size>
99+
</box>
100+
</geometry>
101+
</visual>
102+
</link>
103+
104+
<link name="link_4">
105+
<pose>0 -5 0 0 0 0</pose>
106+
<inertial>
107+
<mass>6.0</mass>
108+
<inertia>
109+
<ixx>1</ixx>
110+
<iyy>1</iyy>
111+
<izz>1</izz>
112+
</inertia>
113+
</inertial>
114+
<collision name="collision_4">
115+
<geometry>
116+
<box>
117+
<size>1 1 1</size>
118+
</box>
119+
</geometry>
120+
</collision>
121+
<visual name="visual_4">
122+
<geometry>
123+
<box>
124+
<size>1 1 1</size>
125+
</box>
126+
</geometry>
127+
</visual>
128+
</link>
129+
</model>
130+
131+
</sdf>

0 commit comments

Comments
 (0)