Skip to content

Commit

Permalink
Fix URDF fixed joint reduction of SDF joints (#1089)
Browse files Browse the repository at this point in the history 
Fix ReduceSDFExtensionJointFrameReplace by
repeating fix from #745 to account for the extra root
element in documents created with tinyxml2.

This includes a test case for a URDF with a chain of links
with fixed joint reduction and an SDFormat joint embedded
in a <gazebo> block whose parent and child are
reduced links. This confirms that the parent and child link
names are updated properly during fixed joint reduction.

Signed-off-by: Steve Peters <[email protected]>
  • Loading branch information
@scpeters
scpeters authored Jul 27, 2022
1 parent 42b101f commit f776995
Show file tree
Hide file tree
Showing 3 changed files with 205 additions and 11 deletions.
22 changes: 11 additions & 11 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ void CreateLink(tinyxml2::XMLElement *_root, urdf::LinkConstSharedPtr _link,
/// reduced fixed joints: apply appropriate frame updates in joint
/// inside urdf extensions when doing fixed joint reduction
void ReduceSDFExtensionJointFrameReplace(
std::vector<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link);

/// reduced fixed joints: apply appropriate frame updates in gripper
Expand Down Expand Up @@ -2558,7 +2558,7 @@ void ReduceSDFExtensionFrameReplace(SDFExtensionPtr _ge,
_ge->reductionTransform);
ReduceSDFExtensionProjectorFrameReplace(blobIt, _link);
ReduceSDFExtensionGripperFrameReplace(blobIt, _link);
ReduceSDFExtensionJointFrameReplace(blobIt, _link);
ReduceSDFExtensionJointFrameReplace((*blobIt)->FirstChildElement(), _link);
}
}

Expand Down Expand Up @@ -3800,45 +3800,45 @@ void ReduceSDFExtensionGripperFrameReplace(

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionJointFrameReplace(
std::vector<XMLDocumentPtr>::iterator _blobIt,
tinyxml2::XMLElement *_blob,
urdf::LinkSharedPtr _link)
{
std::string linkName = _link->name;
std::string parentLinkName = _link->getParent()->name;
auto* doc = (*_blobIt)->GetDocument();
auto* doc = _blob->GetDocument();

if (strcmp((*_blobIt)->FirstChildElement()->Name(), "joint") == 0)
if (strcmp(_blob->Name(), "joint") == 0)
{
// parse it and add/replace the reduction transform
// find first instance of xyz and rpy, replace with reduction transform
tinyxml2::XMLNode *parent = (*_blobIt)->FirstChildElement("parent");
tinyxml2::XMLNode *parent = _blob->FirstChildElement("parent");
if (parent)
{
if (GetKeyValueAsString(parent->ToElement()) == linkName)
{
(*_blobIt)->DeleteChild(parent);
_blob->DeleteChild(parent);
tinyxml2::XMLElement *parentNameKey = doc->NewElement("parent");
std::ostringstream parentNameStream;
parentNameStream << parentLinkName;
tinyxml2::XMLText *parentNameTxt =
doc->NewText(parentNameStream.str().c_str());
parentNameKey->LinkEndChild(parentNameTxt);
(*_blobIt)->LinkEndChild(parentNameKey);
_blob->LinkEndChild(parentNameKey);
}
}
tinyxml2::XMLNode *child = (*_blobIt)->FirstChildElement("child");
tinyxml2::XMLNode *child = _blob->FirstChildElement("child");
if (child)
{
if (GetKeyValueAsString(child->ToElement()) == linkName)
{
(*_blobIt)->DeleteChild(child);
_blob->DeleteChild(child);
tinyxml2::XMLElement *childNameKey = doc->NewElement("child");
std::ostringstream childNameStream;
childNameStream << parentLinkName;
tinyxml2::XMLText *childNameTxt =
doc->NewText(childNameStream.str().c_str());
childNameKey->LinkEndChild(childNameTxt);
(*_blobIt)->LinkEndChild(childNameKey);
_blob->LinkEndChild(childNameKey);
}
}
/// @todo add anchor offsets if parent link changes location!
Expand Down
29 changes: 29 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_JOINT_FRAME_EXTENSION[] =
"fixed_joint_reduction_joint_frame_extension.urdf";
const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
"fixed_joint_reduction_plugin_frame_extension.urdf";

Expand Down Expand Up @@ -737,6 +739,33 @@ TEST(SDFParser, FixedJointReductionSimple)
}
}

/////////////////////////////////////////////////
// This test uses a urdf that has an SDFormat ball joint embedded in a
// <gazebo> tag, and the parent link part of a chain of fixed joints
// that reduces to the base_link.
// Test to make sure that the parent link name changes appropriately.
TEST(SDFParser, FixedJointReductionJointFrameExtensionTest)
{
sdf::Root root;
auto errors =
root.Load(GetFullTestFilePath(SDF_TEST_FILE_JOINT_FRAME_EXTENSION));
EXPECT_TRUE(errors.empty()) << errors[0].Message();

// Get the first model
const sdf::Model *model = root.Model();
ASSERT_NE(nullptr, model);
EXPECT_EQ("chained_fixed_joint_links", model->Name());

EXPECT_EQ(3u, model->JointCount());
const std::string ball_joint_name = "sdf_ball_joint";
EXPECT_TRUE(model->JointNameExists(ball_joint_name));
const sdf::Joint *joint = model->JointByName(ball_joint_name);
ASSERT_NE(nullptr, joint);

EXPECT_EQ("link3", joint->ChildLinkName());
EXPECT_EQ("base_link", joint->ParentLinkName());
}

/////////////////////////////////////////////////
// This test uses a urdf that has chained fixed joints with plugin that
// contains bodyName, xyzOffset and rpyOffset.
Expand Down
165 changes: 165 additions & 0 deletions test/integration/fixed_joint_reduction_joint_frame_extension.urdf
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,165 @@
<?xml version="1.0" encoding="utf-8"?>
<robot name="chained_fixed_joint_links">
<gazebo>
<joint name="sdf_ball_joint" type="ball">
<parent>link1</parent>
<child>link4</child>
</joint>
</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>

<!-- Link 3 -->
<link name="link3">
<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 4 -->
<link name="link4">
<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="continuous">
<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>

<!-- Joint 3 -->
<joint name="joint3" type="continuous">
<parent link="link2"/>
<child link="link3"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>

<!-- Joint 4 -->
<joint name="joint4" type="fixed">
<parent link="link3"/>
<child link="link4"/>
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
<dynamics damping="0.7"/>
</joint>

</robot>

0 comments on commit f776995

Please sign in to comment.