Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix URDF fixed joint reduction of plugins #745

Merged
merged 3 commits into from
Nov 12, 2021
Merged

Fix URDF fixed joint reduction of plugins #745

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
787abbc
Test URDF fixed joint reduction of plugins
scpeters Nov 10, 2021
c02e63d
Fix ReduceSDFExtensionPluginFrameReplace
scpeters Nov 10, 2021
b98b827
Refactor fix with different function argument
scpeters Nov 11, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
21 changes: 11 additions & 10 deletions src/parser_urdf.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3588,17 +3588,18 @@ void ReduceSDFExtensionPluginFrameReplace(
{
std::string linkName = _link->name;
std::string parentLinkName = _link->getParent()->name;
if ((*_blobIt)->FirstChildElement()->Name() == _pluginName)
tinyxml2::XMLElement *rootElement = (*_blobIt)->FirstChildElement();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

add a comment here that there is an extra root element in tinyxml2?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ok, I think it's not an issue with the tinyxml2 API but actually a change in how we used it from #264...

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we changed what we store in the SDFExtension data structure here: 6dd6d07#diff-2bc5ca23bcfc66fe173f513399bec8065b1cb4e607d741566517d7368e6bce0fL1315-L1320

I'll update the PR description accordingly

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I just refactored the fix in b98b827: I change the function signature so it now accepts an XMLElement pointer instead of an XMLDocument iterator. Then we can call FirstChildElement() once and pass the resulting pointer, which simplifies the implementation in ReduceSDFExtensionPluginFrameReplace

I'll fix the other functions in follow-up pull requests if you think this approach is ok

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

yep the approach looks good to me.

if (rootElement->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());
rootElement->FirstChildElement(_elementName.c_str());
if (elementNode)
{
if (GetKeyValueAsString(elementNode->ToElement()) == linkName)
{
(*_blobIt)->DeleteChild(elementNode);
rootElement->DeleteChild(elementNode);
auto* doc = elementNode->GetDocument();
tinyxml2::XMLElement *bodyNameKey =
doc->NewElement(_elementName.c_str());
Expand All @@ -3607,27 +3608,27 @@ void ReduceSDFExtensionPluginFrameReplace(
tinyxml2::XMLText *bodyNameTxt =
doc->NewText(bodyNameStream.str().c_str());
bodyNameKey->LinkEndChild(bodyNameTxt);
(*_blobIt)->LinkEndChild(bodyNameKey);
rootElement->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 = rootElement->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);
rootElement->DeleteChild(xyzKey);
}
tinyxml2::XMLNode *rpyKey = (*_blobIt)->FirstChildElement("rpyOffset");
tinyxml2::XMLNode *rpyKey = rootElement->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);
rootElement->DeleteChild(rpyKey);
}

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

(*_blobIt)->LinkEndChild(xyzKey);
(*_blobIt)->LinkEndChild(rpyKey);
rootElement->LinkEndChild(xyzKey);
rootElement->LinkEndChild(rpyKey);
}
}
}
Expand Down
20 changes: 20 additions & 0 deletions test/integration/fixed_joint_reduction.cc
View file
Open in desktop
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
Open in desktop
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>