Skip to content

Commit 424bf9c

Browse files
scpetersscpeters
authored
Fix URDF fixed joint reduction of plugins (#745)
There is special handling in the parser_urdf code to update plugin fields when links are merged together during fixed joint reduction. A test for this was added to sdf6 in #500. A portion of this test is applied directly to the sdf10 branch to illustrate a problem with ReduceSDFExtensionPluginFrameReplace in parser_urdf.cc. The original migration to use tinyxml2 in #264 changed the data structure used in SDFExtension to store XMLDocuments instead of XMLPointers, which requires an extra call to FirstChildElement, but the ReduceSDFExtension*FrameReplace functions did not receive this update. The fix here refactors the function arguments to pass the first child element directly, which simplifies the helper function implementation. Signed-off-by: Steve Peters <scpeters@openrobotics.org>
1 parent 2905643 commit 424bf9c

3 files changed

Lines changed: 141 additions & 18 deletions

File tree

‎src/parser_urdf.cc‎

Lines changed: 18 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -183,7 +183,7 @@ void ReduceSDFExtensionProjectorFrameReplace(
183183
/// reduced fixed joints: apply appropriate frame updates in plugins
184184
/// inside urdf extensions when doing fixed joint reduction
185185
void ReduceSDFExtensionPluginFrameReplace(
186-
std::vector<XMLDocumentPtr>::iterator _blobIt,
186+
tinyxml2::XMLElement *_blob,
187187
urdf::LinkSharedPtr _link, const std::string &_pluginName,
188188
const std::string &_elementName,
189189
ignition::math::Pose3d _reductionTransform);
@@ -2579,12 +2579,12 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
25792579
<< debugStreamIn.CStr() << "]\n";
25802580

25812581
ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link);
2582-
ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
2583-
"plugin", "bodyName",
2584-
_ge->reductionTransform);
2585-
ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
2586-
"plugin", "frameName",
2587-
_ge->reductionTransform);
2582+
ReduceSDFExtensionPluginFrameReplace(
2583+
(*blobIt)->FirstChildElement(), _link, "plugin", "bodyName",
2584+
_ge->reductionTransform);
2585+
ReduceSDFExtensionPluginFrameReplace(
2586+
(*blobIt)->FirstChildElement(), _link, "plugin", "frameName",
2587+
_ge->reductionTransform);
25882588
ReduceSDFExtensionProjectorFrameReplace(blobIt, _link);
25892589
ReduceSDFExtensionGripperFrameReplace(blobIt, _link);
25902590
ReduceSDFExtensionJointFrameReplace(blobIt, _link);
@@ -3581,24 +3581,24 @@ void ReduceSDFExtensionContactSensorFrameReplace(
35813581

35823582
////////////////////////////////////////////////////////////////////////////////
35833583
void ReduceSDFExtensionPluginFrameReplace(
3584-
std::vector<XMLDocumentPtr>::iterator _blobIt,
3584+
tinyxml2::XMLElement *_blob,
35853585
urdf::LinkSharedPtr _link,
35863586
const std::string &_pluginName, const std::string &_elementName,
35873587
ignition::math::Pose3d _reductionTransform)
35883588
{
35893589
std::string linkName = _link->name;
35903590
std::string parentLinkName = _link->getParent()->name;
3591-
if ((*_blobIt)->FirstChildElement()->Name() == _pluginName)
3591+
if (_blob->Name() == _pluginName)
35923592
{
35933593
// replace element containing _link names to parent link names
35943594
// find first instance of xyz and rpy, replace with reduction transform
35953595
tinyxml2::XMLNode *elementNode =
3596-
(*_blobIt)->FirstChildElement(_elementName.c_str());
3596+
_blob->FirstChildElement(_elementName.c_str());
35973597
if (elementNode)
35983598
{
35993599
if (GetKeyValueAsString(elementNode->ToElement()) == linkName)
36003600
{
3601-
(*_blobIt)->DeleteChild(elementNode);
3601+
_blob->DeleteChild(elementNode);
36023602
auto* doc = elementNode->GetDocument();
36033603
tinyxml2::XMLElement *bodyNameKey =
36043604
doc->NewElement(_elementName.c_str());
@@ -3607,27 +3607,27 @@ void ReduceSDFExtensionPluginFrameReplace(
36073607
tinyxml2::XMLText *bodyNameTxt =
36083608
doc->NewText(bodyNameStream.str().c_str());
36093609
bodyNameKey->LinkEndChild(bodyNameTxt);
3610-
(*_blobIt)->LinkEndChild(bodyNameKey);
3610+
_blob->LinkEndChild(bodyNameKey);
36113611
/// @todo update transforms for this sdf plugin too
36123612

36133613
// look for offset transforms, add reduction transform
3614-
tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset");
3614+
tinyxml2::XMLNode *xyzKey = _blob->FirstChildElement("xyzOffset");
36153615
if (xyzKey)
36163616
{
36173617
urdf::Vector3 v1 = ParseVector3(xyzKey);
36183618
_reductionTransform.Pos() =
36193619
ignition::math::Vector3d(v1.x, v1.y, v1.z);
36203620
// remove xyzOffset and rpyOffset
3621-
(*_blobIt)->DeleteChild(xyzKey);
3621+
_blob->DeleteChild(xyzKey);
36223622
}
3623-
tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset");
3623+
tinyxml2::XMLNode *rpyKey = _blob->FirstChildElement("rpyOffset");
36243624
if (rpyKey)
36253625
{
36263626
urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
36273627
_reductionTransform.Rot() =
36283628
ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
36293629
// remove xyzOffset and rpyOffset
3630-
(*_blobIt)->DeleteChild(rpyKey);
3630+
_blob->DeleteChild(rpyKey);
36313631
}
36323632

36333633
// pass through the parent transform from fixed joint reduction
@@ -3661,8 +3661,8 @@ void ReduceSDFExtensionPluginFrameReplace(
36613661
xyzKey->LinkEndChild(xyzTxt);
36623662
rpyKey->LinkEndChild(rpyTxt);
36633663

3664-
(*_blobIt)->LinkEndChild(xyzKey);
3665-
(*_blobIt)->LinkEndChild(rpyKey);
3664+
_blob->LinkEndChild(xyzKey);
3665+
_blob->LinkEndChild(rpyKey);
36663666
}
36673667
}
36683668
}

‎test/integration/fixed_joint_reduction.cc‎

Lines changed: 20 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
3636
"fixed_joint_reduction_collision_visual_empty_root.urdf";
3737
const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
3838
"fixed_joint_reduction_collision_visual_empty_root.sdf";
39+
const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
40+
"fixed_joint_reduction_plugin_frame_extension.urdf";
3941

4042
static std::string GetFullTestFilePath(const char *_input)
4143
{
@@ -734,3 +736,21 @@ TEST(SDFParser, FixedJointReductionSimple)
734736
EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
735737
}
736738
}
739+
740+
/////////////////////////////////////////////////
741+
// This test uses a urdf that has chained fixed joints with plugin that
742+
// contains bodyName, xyzOffset and rpyOffset.
743+
// Test to make sure that the bodyName is updated to the new base link.
744+
TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
745+
{
746+
sdf::SDFPtr robot(new sdf::SDF());
747+
sdf::init(robot);
748+
ASSERT_TRUE(sdf::readFile(
749+
GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), robot));
750+
751+
sdf::ElementPtr model = robot->Root()->GetElement("model");
752+
sdf::ElementPtr plugin = model->GetElement("plugin");
753+
754+
auto bodyName = plugin->Get<std::string>("bodyName");
755+
EXPECT_EQ("base_link", bodyName);
756+
}

‎test/integration/fixed_joint_reduction_plugin_frame_extension.urdf‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<robot name="chained_fixed_joint_links">
3+
<gazebo>
4+
<plugin name='test_plugin' filename='libtest_plugin.so'>
5+
<serviceName>/test/plugin/service</serviceName>
6+
<topicName>/test/plugin/topic</topicName>
7+
<bodyName>link2</bodyName>
8+
<updateRate>100</updateRate>
9+
<xyzOffset>0 0 0</xyzOffset>
10+
<rpyOffset>0 0 0</rpyOffset>
11+
</plugin>
12+
</gazebo>
13+
14+
<!-- Base Link -->
15+
<link name="base_link">
16+
<collision>
17+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
18+
<geometry>
19+
<box size="1.0 1.0 1"/>
20+
</geometry>
21+
</collision>
22+
<visual>
23+
<origin rpy="0 0 0" xyz="0 0 1.0"/>
24+
<geometry>
25+
<box size="0.1 0.1 2"/>
26+
</geometry>
27+
</visual>
28+
<inertial>
29+
<origin xyz="0 0 1" rpy="0 0 0"/>
30+
<mass value="1"/>
31+
<inertia
32+
ixx="1.0" ixy="0.0" ixz="0.0"
33+
iyy="1.0" iyz="0.0"
34+
izz="1.0"/>
35+
</inertial>
36+
</link>
37+
38+
<!-- Link 1 -->
39+
<link name="link1">
40+
<collision>
41+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
42+
<geometry>
43+
<box size="1.0 1.0 1"/>
44+
</geometry>
45+
</collision>
46+
<visual>
47+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
48+
<geometry>
49+
<box size="0.1 0.1 1"/>
50+
</geometry>
51+
</visual>
52+
<inertial>
53+
<origin xyz="0 0 1" rpy="0 0 0"/>
54+
<mass value="1"/>
55+
<inertia
56+
ixx="1.0" ixy="0.0" ixz="0.0"
57+
iyy="1.0" iyz="0.0"
58+
izz="1.0"/>
59+
</inertial>
60+
</link>
61+
62+
<!-- Link 2 -->
63+
<link name="link2">
64+
<collision>
65+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
66+
<geometry>
67+
<box size="1.0 1.0 1"/>
68+
</geometry>
69+
</collision>
70+
<visual>
71+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
72+
<geometry>
73+
<box size="0.1 0.1 1"/>
74+
</geometry>
75+
</visual>
76+
<inertial>
77+
<origin xyz="0 0 1" rpy="0 0 0"/>
78+
<mass value="1"/>
79+
<inertia
80+
ixx="1.0" ixy="0.0" ixz="0.0"
81+
iyy="1.0" iyz="0.0"
82+
izz="1.0"/>
83+
</inertial>
84+
</link>
85+
86+
<!-- Joint 1 -->
87+
<joint name="joint1" type="fixed">
88+
<parent link="base_link"/>
89+
<child link="link1"/>
90+
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
91+
<dynamics damping="0.7"/>
92+
</joint>
93+
94+
<!-- Joint 2 -->
95+
<joint name="joint2" type="fixed">
96+
<parent link="link1"/>
97+
<child link="link2"/>
98+
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
99+
<axis xyz="0 1 0"/>
100+
<dynamics damping="0.7"/>
101+
</joint>
102+
</robot>
103+

0 commit comments

Comments
 (0)