Skip to content

Commit 12c4c4a

Browse files
scpetersscpeters
authored
Merge pull request #1182 from gazebosim/scpeters/merge_12_to_13
Merge sdf12 ➡️ sdf13
2 parents a5be6ab + ffea2ed commit 12c4c4a

File tree

4 files changed

+229
-4
lines changed

4 files changed

+229
-4
lines changed

‎src/parser_urdf.cc

+33
Original file line numberDiff line numberDiff line change
@@ -424,6 +424,39 @@ void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link)
424424
sdfdbg << "Fixed Joint Reduction: extension lumping from ["
425425
<< _link->name << "] to [" << _link->getParent()->name << "]\n";
426426

427+
// Add //model/frame tag to memorialize reduced joint
428+
sdf::Frame jointFrame;
429+
jointFrame.SetName(_link->parent_joint->name);
430+
jointFrame.SetAttachedTo(_link->getParent()->name);
431+
jointFrame.SetRawPose(
432+
CopyPose(_link->parent_joint->parent_to_joint_origin_transform));
433+
434+
// Add //model/frame tag to memorialize reduced link
435+
sdf::Frame linkFrame;
436+
linkFrame.SetName(_link->name);
437+
linkFrame.SetAttachedTo(_link->parent_joint->name);
438+
439+
// Serialize sdf::Frame objects to xml and add to SDFExtension
440+
SDFExtensionPtr sdfExt = std::make_shared<SDFExtension>();
441+
auto sdfFrameToExtension = [&sdfExt](const sdf::Frame &_frame)
442+
{
443+
XMLDocumentPtr xmlNewDoc = std::make_shared<tinyxml2::XMLDocument>();
444+
sdf::PrintConfig config;
445+
config.SetOutPrecision(16);
446+
xmlNewDoc->Parse(_frame.ToElement()->ToString("", config).c_str());
447+
if (xmlNewDoc->Error())
448+
{
449+
sdferr << "Error while parsing serialized frames: "
450+
<< xmlNewDoc->ErrorStr() << '\n';
451+
}
452+
sdfExt->blobs.push_back(xmlNewDoc);
453+
};
454+
sdfFrameToExtension(jointFrame);
455+
sdfFrameToExtension(linkFrame);
456+
457+
// Add //frame tags to model extension vector
458+
g_extensions[""].push_back(sdfExt);
459+
427460
// lump sdf extensions to parent, (give them new reference _link names)
428461
ReduceSDFExtensionToParent(_link);
429462

‎test/integration/fixed_joint_example.urdf

+63
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
<?xml version="1.0" ?>
2+
<robot name="fixed_joint_example">
3+
4+
<link name="base">
5+
<!-- base has inertial and visual geometry of a half-cube -->
6+
<inertial>
7+
<mass value="6"/>
8+
<origin rpy="0 0 0" xyz="0 0 0.25"/>
9+
<inertia ixx="0.625" ixy="0" ixz="0" iyy="0.625" iyz="0" izz="1.0"/>
10+
</inertial>
11+
<visual>
12+
<geometry>
13+
<box size="1.0 1.0 0.5"/>
14+
</geometry>
15+
<origin rpy="0 0 0" xyz="0 0 0.25"/>
16+
</visual>
17+
</link>
18+
19+
<!-- Intermediate link with fixed joint that will be reduced. -->
20+
<joint name="intermediate_joint" type="fixed">
21+
<origin rpy="1.57079632679 0 0" xyz="0 0 0.75"/>
22+
<parent link="base"/>
23+
<child link="intermediate_link"/>
24+
</joint>
25+
<link name="intermediate_link">
26+
<!-- intermediate_link has inertial and visual geometry of a half-cube -->
27+
<inertial>
28+
<mass value="6"/>
29+
<origin rpy="0 0 0" xyz="0 0 0"/>
30+
<inertia ixx="0.625" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="0.625"/>
31+
</inertial>
32+
<visual>
33+
<geometry>
34+
<box size="1.0 0.5 1.0"/>
35+
</geometry>
36+
<origin rpy="0 0 0" xyz="0 0 0"/>
37+
</visual>
38+
</link>
39+
40+
<!-- Continuous joint connected to intermediate_link -->
41+
<joint name="rotary_joint" type="continuous">
42+
<origin rpy="0 0 1.57079632679" xyz="0 0.75 0"/>
43+
<axis xyz="1 0 0"/>
44+
<parent link="intermediate_link"/>
45+
<child link="rotary_link"/>
46+
</joint>
47+
<link name="rotary_link">
48+
<!-- rotary_link has inertial and visual geometry of a cube equivalent to the
49+
merged properties of base and intermediate_link. -->
50+
<inertial>
51+
<mass value="12"/>
52+
<origin rpy="0 0 0" xyz="0 0 0"/>
53+
<inertia ixx="2" ixy="0" ixz="0" iyy="2" iyz="0" izz="2"/>
54+
</inertial>
55+
<visual>
56+
<geometry>
57+
<box size="1.0 1.0 1.0"/>
58+
</geometry>
59+
<origin rpy="0 0 0" xyz="0 0 0"/>
60+
</visual>
61+
</link>
62+
63+
</robot>

‎test/integration/fixed_joint_simple.urdf

+34
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
<?xml version="1.0" ?>
2+
<robot name="fixed_joint_simple">
3+
4+
<link name="base">
5+
<inertial>
6+
<mass value="1"/>
7+
<origin rpy="0 0 0" xyz="0 0 0"/>
8+
<inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
9+
</inertial>
10+
</link>
11+
12+
<!-- Intermediate fixed joint that will be reduced. -->
13+
<joint name="fixed_joint" type="fixed">
14+
<origin rpy="0 0 0" xyz="0 0 0"/>
15+
<parent link="base"/>
16+
<child link="child_link"/>
17+
</joint>
18+
19+
<!-- Child link with visual that will be merged to base link -->
20+
<link name="child_link">
21+
<inertial>
22+
<mass value="1"/>
23+
<origin rpy="0 0 0" xyz="0 0 0"/>
24+
<inertia ixx="1" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1"/>
25+
</inertial>
26+
<visual>
27+
<geometry>
28+
<box size="1.0 1.0 1.0"/>
29+
</geometry>
30+
<origin rpy="0 0 0" xyz="0 0 0"/>
31+
</visual>
32+
</link>
33+
34+
</robot>

‎test/integration/urdf_gazebo_extensions.cc

+99-4
Original file line numberDiff line numberDiff line change
@@ -29,11 +29,13 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
2929
const std::string urdfTestFile =
3030
sdf::testing::TestFile("integration", "urdf_gazebo_extensions.urdf");
3131

32-
sdf::SDFPtr robot(new sdf::SDF());
33-
sdf::init(robot);
34-
ASSERT_TRUE(sdf::readFile(urdfTestFile, robot));
32+
sdf::Root root;
33+
auto errors = root.Load(urdfTestFile);
34+
EXPECT_TRUE(errors.empty()) << errors;
3535

36-
sdf::ElementPtr model = robot->Root()->GetElement("model");
36+
auto modelDom = root.Model();
37+
ASSERT_NE(nullptr, modelDom);
38+
sdf::ElementPtr model = modelDom->Element();
3739
for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
3840
joint = joint->GetNextElement("joint"))
3941
{
@@ -299,4 +301,97 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
299301
checkElementPoses("light");
300302
checkElementPoses("projector");
301303
checkElementPoses("sensor");
304+
305+
// Check that //model/frame elements are added for reduced joints
306+
EXPECT_EQ(14u, modelDom->FrameCount());
307+
308+
EXPECT_TRUE(modelDom->FrameNameExists("issue378_link_joint"));
309+
EXPECT_TRUE(modelDom->FrameNameExists("jCamera"));
310+
EXPECT_TRUE(modelDom->FrameNameExists("jointSensorNoPose"));
311+
EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPose"));
312+
EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseRelative"));
313+
EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel"));
314+
EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel2"));
315+
316+
EXPECT_TRUE(modelDom->FrameNameExists("issue378_link"));
317+
EXPECT_TRUE(modelDom->FrameNameExists("Camera"));
318+
EXPECT_TRUE(modelDom->FrameNameExists("linkSensorNoPose"));
319+
EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPose"));
320+
EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseRelative"));
321+
EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel"));
322+
EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel2"));
323+
}
324+
325+
/////////////////////////////////////////////////
326+
TEST(SDFParser, FixedJointExample)
327+
{
328+
const std::string urdfTestFile =
329+
sdf::testing::TestFile("integration", "fixed_joint_example.urdf");
330+
331+
sdf::Root root;
332+
auto errors = root.Load(urdfTestFile);
333+
EXPECT_TRUE(errors.empty()) << errors;
334+
335+
auto model = root.Model();
336+
ASSERT_NE(nullptr, model);
337+
EXPECT_EQ("fixed_joint_example", model->Name());
338+
339+
EXPECT_EQ(2u, model->LinkCount());
340+
EXPECT_TRUE(model->LinkNameExists("base"));
341+
EXPECT_TRUE(model->LinkNameExists("rotary_link"));
342+
343+
// Expect MassMatrix3 values to match for links
344+
auto link1 = model->LinkByName("base");
345+
auto link2 = model->LinkByName("rotary_link");
346+
ASSERT_NE(nullptr, link1);
347+
ASSERT_NE(nullptr, link2);
348+
auto massMatrix1 = link1->Inertial().MassMatrix();
349+
auto massMatrix2 = link2->Inertial().MassMatrix();
350+
EXPECT_DOUBLE_EQ(massMatrix1.Mass(), massMatrix2.Mass());
351+
EXPECT_EQ(massMatrix1.Moi(), massMatrix2.Moi());
352+
353+
EXPECT_EQ(1u, model->JointCount());
354+
EXPECT_TRUE(model->JointNameExists("rotary_joint"));
355+
356+
EXPECT_EQ(2u, model->FrameCount());
357+
ASSERT_TRUE(model->FrameNameExists("intermediate_joint"));
358+
ASSERT_TRUE(model->FrameNameExists("intermediate_link"));
359+
360+
const std::string j = "intermediate_joint";
361+
const std::string l = "intermediate_link";
362+
std::string body;
363+
EXPECT_TRUE(model->FrameByName(j)->ResolveAttachedToBody(body).empty());
364+
EXPECT_EQ("base", body);
365+
EXPECT_TRUE(model->FrameByName(l)->ResolveAttachedToBody(body).empty());
366+
EXPECT_EQ("base", body);
367+
}
368+
369+
/////////////////////////////////////////////////
370+
TEST(SDFParser, FixedJointSimple)
371+
{
372+
const std::string urdfTestFile =
373+
sdf::testing::TestFile("integration", "fixed_joint_simple.urdf");
374+
375+
sdf::Root root;
376+
auto errors = root.Load(urdfTestFile);
377+
EXPECT_TRUE(errors.empty()) << errors;
378+
379+
auto model = root.Model();
380+
ASSERT_NE(nullptr, model);
381+
EXPECT_EQ("fixed_joint_simple", model->Name());
382+
383+
EXPECT_EQ(1u, model->LinkCount());
384+
EXPECT_TRUE(model->LinkNameExists("base"));
385+
386+
auto link = model->LinkByName("base");
387+
ASSERT_NE(nullptr, link);
388+
auto massMatrix = link->Inertial().MassMatrix();
389+
EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass());
390+
EXPECT_EQ(2.0 * gz::math::Matrix3d::Identity, massMatrix.Moi());
391+
392+
EXPECT_EQ(0u, model->JointCount());
393+
394+
EXPECT_EQ(2u, model->FrameCount());
395+
ASSERT_TRUE(model->FrameNameExists("fixed_joint"));
396+
ASSERT_TRUE(model->FrameNameExists("child_link"));
302397
}

0 commit comments

Comments
 (0)