Skip to content

Commit

Permalink
Fix URDF fixed joint reduction of plugins (#745)
Browse files Browse the repository at this point in the history 
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 <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Nov 12, 2021
1 parent 2905643 commit 424bf9c
Show file tree
Hide file tree
Showing 3 changed files with 141 additions and 18 deletions.
36 changes: 18 additions & 18 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -183,7 +183,7 @@ void ReduceSDFExtensionProjectorFrameReplace(
/// reduced fixed joints: apply appropriate frame updates in plugins
/// inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionPluginFrameReplace(
std::vector<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link, const std::string &_pluginName,
const std::string &_elementName,
ignition::math::Pose3d _reductionTransform);
Expand Down Expand Up @@ -2579,12 +2579,12 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
<< debugStreamIn.CStr() << "]\n";

ReduceSDFExtensionContactSensorFrameReplace(blobIt, _link);
ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
"plugin", "bodyName",
_ge->reductionTransform);
ReduceSDFExtensionPluginFrameReplace(blobIt, _link,
"plugin", "frameName",
_ge->reductionTransform);
ReduceSDFExtensionPluginFrameReplace(
(*blobIt)->FirstChildElement(), _link, "plugin", "bodyName",
_ge->reductionTransform);
ReduceSDFExtensionPluginFrameReplace(
(*blobIt)->FirstChildElement(), _link, "plugin", "frameName",
_ge->reductionTransform);
ReduceSDFExtensionProjectorFrameReplace(blobIt, _link);
ReduceSDFExtensionGripperFrameReplace(blobIt, _link);
ReduceSDFExtensionJointFrameReplace(blobIt, _link);
Expand Down Expand Up @@ -3581,24 +3581,24 @@ void ReduceSDFExtensionContactSensorFrameReplace(

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionPluginFrameReplace(
std::vector<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link,
const std::string &_pluginName, const std::string &_elementName,
ignition::math::Pose3d _reductionTransform)
{
std::string linkName = _link->name;
std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->FirstChildElement()->Name() == _pluginName)
if (_blob->Name() == _pluginName)
{
// replace element containing _link names to parent link names
// find first instance of xyz and rpy, replace with reduction transform
tinyxml2::XMLNode *elementNode =
(*_blobIt)->FirstChildElement(_elementName.c_str());
_blob->FirstChildElement(_elementName.c_str());
if (elementNode)
{
if (GetKeyValueAsString(elementNode->ToElement()) == linkName)
{
(*_blobIt)->DeleteChild(elementNode);
_blob->DeleteChild(elementNode);
auto* doc = elementNode->GetDocument();
tinyxml2::XMLElement *bodyNameKey =
doc->NewElement(_elementName.c_str());
Expand All @@ -3607,27 +3607,27 @@ void ReduceSDFExtensionPluginFrameReplace(
tinyxml2::XMLText *bodyNameTxt =
doc->NewText(bodyNameStream.str().c_str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
_blob->LinkEndChild(bodyNameKey);
/// @todo update transforms for this sdf plugin too

// look for offset transforms, add reduction transform
tinyxml2::XMLNode *xyzKey = (*_blobIt)->FirstChildElement("xyzOffset");
tinyxml2::XMLNode *xyzKey = _blob->FirstChildElement("xyzOffset");
if (xyzKey)
{
urdf::Vector3 v1 = ParseVector3(xyzKey);
_reductionTransform.Pos() =
ignition::math::Vector3d(v1.x, v1.y, v1.z);
// remove xyzOffset and rpyOffset
(*_blobIt)->DeleteChild(xyzKey);
_blob->DeleteChild(xyzKey);
}
tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset");
tinyxml2::XMLNode *rpyKey = _blob->FirstChildElement("rpyOffset");
if (rpyKey)
{
urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
_reductionTransform.Rot() =
ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
// remove xyzOffset and rpyOffset
(*_blobIt)->DeleteChild(rpyKey);
_blob->DeleteChild(rpyKey);
}

// pass through the parent transform from fixed joint reduction
Expand Down Expand Up @@ -3661,8 +3661,8 @@ void ReduceSDFExtensionPluginFrameReplace(
xyzKey->LinkEndChild(xyzTxt);
rpyKey->LinkEndChild(rpyTxt);

(*_blobIt)->LinkEndChild(xyzKey);
(*_blobIt)->LinkEndChild(rpyKey);
_blob->LinkEndChild(xyzKey);
_blob->LinkEndChild(rpyKey);
}
}
}
Expand Down
20 changes: 20 additions & 0 deletions test/integration/fixed_joint_reduction.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
"fixed_joint_reduction_collision_visual_empty_root.urdf";
const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
"fixed_joint_reduction_collision_visual_empty_root.sdf";
const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
"fixed_joint_reduction_plugin_frame_extension.urdf";

static std::string GetFullTestFilePath(const char *_input)
{
Expand Down Expand Up @@ -734,3 +736,21 @@ TEST(SDFParser, FixedJointReductionSimple)
EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
}
}

/////////////////////////////////////////////////
// This test uses a urdf that has chained fixed joints with plugin that
// contains bodyName, xyzOffset and rpyOffset.
// Test to make sure that the bodyName is updated to the new base link.
TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
{
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(
GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION), robot));

sdf::ElementPtr model = robot->Root()->GetElement("model");
sdf::ElementPtr plugin = model->GetElement("plugin");

auto bodyName = plugin->Get<std::string>("bodyName");
EXPECT_EQ("base_link", bodyName);
}
103 changes: 103 additions & 0 deletions test/integration/fixed_joint_reduction_plugin_frame_extension.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,103 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="chained_fixed_joint_links">
<gazebo>
<plugin name='test_plugin' filename='libtest_plugin.so'>
<serviceName>/test/plugin/service</serviceName>
<topicName>/test/plugin/topic</topicName>
<bodyName>link2</bodyName>
<updateRate>100</updateRate>
<xyzOffset>0 0 0</xyzOffset>
<rpyOffset>0 0 0</rpyOffset>
</plugin>
</gazebo>

<!-- Base Link -->
<link name="base_link">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 1.0"/>
<geometry>
<box size="0.1 0.1 2"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 1 -->
<link name="link1">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Link 2 -->
<link name="link2">
<collision>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="1.0 1.0 1"/>
</geometry>
</collision>
<visual>
<origin rpy="0 0 0" xyz="0 0 0.0"/>
<geometry>
<box size="0.1 0.1 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 1" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>

<!-- Joint 1 -->
<joint name="joint1" type="fixed">
<parent link="base_link"/>
<child link="link1"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<dynamics damping="0.7"/>
</joint>

<!-- Joint 2 -->
<joint name="joint2" type="fixed">
<parent link="link1"/>
<child link="link2"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
</robot>

0 comments on commit 424bf9c

Please sign in to comment.