Skip to content

Commit eee6f5a

Browse files
scpetersazeey
scpeters
and
azeey
authored
ParserConfig: save auto inertial values by default (#1325)
* Copy the inertial_stats.sdf file replace the user-defined inertial with //inertial/@auto=true and equivalent //collision/density values. * Change default value of ConfigureResolveAutoInertials to SAVE_CALCULATION to match previous API behavior. * Warn and return default inertial values if custom inertia calculator is unset. --------- Signed-off-by: Steve Peters <[email protected]> Signed-off-by: Addisu Z. Taddese <[email protected]> Co-authored-by: Addisu Z. Taddese <[email protected]>
1 parent 2ba6470 commit eee6f5a

File tree

11 files changed

+189
-42
lines changed

11 files changed

+189
-42
lines changed

‎src/Geometry.cc‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -312,6 +312,7 @@ void Geometry::SetPolylineShape(const std::vector<Polyline> &_polylines)
312312
this->dataPtr->polylines = _polylines;
313313
}
314314

315+
/////////////////////////////////////////////////
315316
std::optional<gz::math::Inertiald> Geometry::CalculateInertial(
316317
sdf::Errors &_errors, const ParserConfig &_config,
317318
double _density, sdf::ElementPtr _autoInertiaParams)

‎src/Link.cc‎

Lines changed: 5 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -191,9 +191,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
191191
{
192192
Error err(
193193
ErrorCode::WARNING,
194-
"Inertial was used with auto=true for link, " +
195-
this->dataPtr->name + " but user defined inertial values "
196-
"were found. They would be overwritten by computed ones."
194+
"Inertial was used with auto=true for the link named " +
195+
this->dataPtr->name + ", but user-defined inertial values were "
196+
"found, which will be overwritten by the computed inertial values."
197197
);
198198
enforceConfigurablePolicyCondition(
199199
_config.WarningsPolicy(), err, errors);
@@ -610,7 +610,8 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
610610
{
611611
_errors.push_back({ErrorCode::ELEMENT_MISSING,
612612
"Inertial is set to auto but there are no "
613-
"<collision> elements for the link."});
613+
"<collision> elements for the link named " +
614+
this->Name() + "."});
614615
return;
615616
}
616617

‎src/Link_TEST.cc‎

Lines changed: 6 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -293,18 +293,13 @@ TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink)
293293
sdf::Root root;
294294
const sdf::ParserConfig sdfParserConfig;
295295
sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
296-
EXPECT_TRUE(errors.empty());
296+
ASSERT_EQ(1u, errors.size()) << errors;
297+
EXPECT_EQ(sdf::ErrorCode::ELEMENT_MISSING, errors[0].Code());
298+
EXPECT_NE(std::string::npos,
299+
errors[0].Message().find(
300+
"Inertial is set to auto but there are no <collision> elements "
301+
"for the link named link."));
297302
EXPECT_NE(nullptr, root.Element());
298-
299-
const sdf::Model *model = root.Model();
300-
const sdf::Link *link = model->LinkByIndex(0);
301-
root.ResolveAutoInertials(errors, sdfParserConfig);
302-
ASSERT_FALSE(errors.empty());
303-
304-
// Default Inertial values set during load
305-
EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass());
306-
EXPECT_EQ(gz::math::Vector3d::One,
307-
link->Inertial().MassMatrix().DiagonalMoments());
308303
}
309304

310305
/////////////////////////////////////////////////

‎src/Mesh.cc‎

Lines changed: 16 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -209,6 +209,22 @@ std::optional<gz::math::Inertiald> Mesh::CalculateInertial(sdf::Errors &_errors,
209209

210210
const auto &customCalculator = _config.CustomInertiaCalc();
211211

212+
if (!customCalculator)
213+
{
214+
Error err(
215+
sdf::ErrorCode::WARNING,
216+
"Custom moment of inertia calculator for meshes not set via "
217+
"sdf::ParserConfig::RegisterCustomInertiaCalc, using default "
218+
"inertial values.");
219+
enforceConfigurablePolicyCondition(
220+
_config.WarningsPolicy(), err, _errors);
221+
222+
using namespace gz::math;
223+
return Inertiald(
224+
MassMatrix3d(1, Vector3d::One, Vector3d::Zero),
225+
Pose3d::Zero);
226+
}
227+
212228
sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties(
213229
_density, *this, _autoInertiaParams);
214230

‎src/ParserConfig.cc‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -45,10 +45,10 @@ class sdf::ParserConfig::Implementation
4545
public: std::optional<EnforcementPolicy> deprecatedElementsPolicy;
4646

4747
/// \brief Configuration that is set for the CalculateInertial() function
48-
/// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause
49-
/// Root::Load() to not call CalculateInertial()
48+
/// By default it is set to SAVE_CALCULATION to preserve the behavior of
49+
/// Root::Load() generating complete inertial information.
5050
public: ConfigureResolveAutoInertials resolveAutoInertialsConfig =
51-
ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD;
51+
ConfigureResolveAutoInertials::SAVE_CALCULATION;
5252

5353
/// \brief Collection of custom model parsers.
5454
public: std::vector<CustomModelParser> customParsers;

‎src/ParserConfig_TEST.cc‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -57,11 +57,11 @@ TEST(ParserConfig, Construction)
5757
EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy());
5858
EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy());
5959

60-
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
60+
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION,
6161
config.CalculateInertialConfiguration());
6262
config.SetCalculateInertialConfiguration(
63-
sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION);
64-
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION,
63+
sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD);
64+
EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD,
6565
config.CalculateInertialConfiguration());
6666
EXPECT_FALSE(config.URDFPreserveFixedJoint());
6767
EXPECT_FALSE(config.StoreResolvedURIs());

‎src/gz_TEST.cc‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1889,7 +1889,7 @@ TEST(inertial_stats, GZ_UTILS_TEST_DISABLED_ON_WIN32(SDF))
18891889

18901890
// Check a good SDF file from the same folder by passing a relative path
18911891
{
1892-
std::string path = "inertial_stats.sdf";
1892+
std::string path = "inertial_stats_auto.sdf";
18931893

18941894
std::string output =
18951895
custom_exec_str("cd " + pathBase + " && " +

‎test/integration/root_dom.cc‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -300,3 +300,13 @@ TEST(DOMRoot, CreateMulipleWorlds)
300300
testFunc(root);
301301
testFunc(root2);
302302
}
303+
304+
/////////////////////////////////////////////////
305+
TEST(DOMRoot, LoadWithoutMeshAutoInertialCalculator)
306+
{
307+
const std::string testFile =
308+
sdf::testing::TestFile("sdf", "mesh_auto_inertial.sdf");
309+
sdf::Root root;
310+
sdf::Errors errors = root.Load(testFile);
311+
EXPECT_TRUE(errors.empty()) << errors;
312+
}

‎test/sdf/inertial_stats.sdf‎

Lines changed: 20 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,25 @@
11
<?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-
"gz 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-
-->
212
<sdf version="1.6">
22-
3+
<!---
4+
Model consists of 4 cubes places symmetrically in the XY plane.
5+
+y
6+
│
7+
┌─┼─┐
8+
L3 │ │ │(0,5,0)
9+
└─┼─┘
10+
│
11+
L2┌───┐ │ ┌───┐L1
12+
────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
13+
└───┘ │ └───┘
14+
(-5,0,0) │ (5,0,0)
15+
┌─┼─┐
16+
│ │ │(0,-5,0)
17+
└─┼─┘
18+
L4│
19+
-->
20+
<![CDATA[
21+
This model is used to verify the "gz sdf --inertial-stats" tool.
22+
]]>
2323
<model name="test_model">
2424
<pose>0 0 0 0 0 0</pose>
2525

‎test/sdf/inertial_stats_auto.sdf‎

Lines changed: 107 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,107 @@
1+
<?xml version="1.0" ?>
2+
<sdf version="1.11">
3+
<!---
4+
Model consists of 4 cubes places symmetrically in the XY plane.
5+
+y
6+
│
7+
┌─┼─┐
8+
L3 │ │ │(0,5,0)
9+
└─┼─┘
10+
│
11+
L2┌───┐ │ ┌───┐L1
12+
────┼┼┼┼┼───┼─────┼┼┼┼┼─── +x
13+
└───┘ │ └───┘
14+
(-5,0,0) │ (5,0,0)
15+
┌─┼─┐
16+
│ │ │(0,-5,0)
17+
└─┼─┘
18+
L4│
19+
-->
20+
<![CDATA[
21+
This model is used to verify the "gz sdf --inertial-stats" tool.
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 auto='true' />
29+
<collision name="collision_1">
30+
<density>6</density>
31+
<geometry>
32+
<box>
33+
<size>1 1 1</size>
34+
</box>
35+
</geometry>
36+
</collision>
37+
<visual name="visual_1">
38+
<geometry>
39+
<box>
40+
<size>1 1 1</size>
41+
</box>
42+
</geometry>
43+
</visual>
44+
</link>
45+
46+
<link name="link_2">
47+
<pose>-5 0 0 0 0 0</pose>
48+
<inertial auto='true' />
49+
<collision name="collision_2">
50+
<density>6</density>
51+
<geometry>
52+
<box>
53+
<size>1 1 1</size>
54+
</box>
55+
</geometry>
56+
</collision>
57+
<visual name="visual_2">
58+
<geometry>
59+
<box>
60+
<size>1 1 1</size>
61+
</box>
62+
</geometry>
63+
</visual>
64+
</link>
65+
66+
<link name="link_3">
67+
<pose>0 5 0 0 0 0</pose>
68+
<inertial auto='true' />
69+
<collision name="collision_3">
70+
<density>6</density>
71+
<geometry>
72+
<box>
73+
<size>1 1 1</size>
74+
</box>
75+
</geometry>
76+
</collision>
77+
<visual name="visual_3">
78+
<geometry>
79+
<box>
80+
<size>1 1 1</size>
81+
</box>
82+
</geometry>
83+
</visual>
84+
</link>
85+
86+
<link name="link_4">
87+
<pose>0 -5 0 0 0 0</pose>
88+
<inertial auto='true' />
89+
<collision name="collision_4">
90+
<density>6</density>
91+
<geometry>
92+
<box>
93+
<size>1 1 1</size>
94+
</box>
95+
</geometry>
96+
</collision>
97+
<visual name="visual_4">
98+
<geometry>
99+
<box>
100+
<size>1 1 1</size>
101+
</box>
102+
</geometry>
103+
</visual>
104+
</link>
105+
</model>
106+
107+
</sdf>

0 commit comments

Comments
 (0)