Skip to content

Commit

Permalink
Fix xyz and rpy offsets in fixed joint reduction (#500)
Browse files Browse the repository at this point in the history 
* parse rpyOffset as radians

Signed-off-by: Ian Chen <[email protected]>

* update tf for xyz and rpy offsets

Signed-off-by: Ian Chen <[email protected]>

* remove inverse transform function in urdf parser

Signed-off-by: Ian Chen <[email protected]>

* inject corrected_offets tag

Signed-off-by: Ian Chen <[email protected]>

* Fix tag removal logic

Signed-off-by: Steven Peters <[email protected]>

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Steve Peters <[email protected]>
  • Loading branch information
@iche033 @ahcorde @scpeters
3 people authored Aug 30, 2021
1 parent b06cc18 commit c02aca7
Show file tree
Hide file tree
Showing 3 changed files with 143 additions and 30 deletions.
41 changes: 11 additions & 30 deletions src/parser_urdf.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -219,10 +219,6 @@ std::string Values2str(unsigned int _count, const double *_values);

void CreateGeometry(TiXmlElement* _elem, urdf::GeometrySharedPtr _geometry);

ignition::math::Pose3d inverseTransformToParentFrame(
ignition::math::Pose3d _transformInLinkFrame,
urdf::Pose _parentToLinkTransform);

/// reduced fixed joints: transform to parent frame
urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame,
urdf::Pose _parentToLinkTransform);
Expand Down Expand Up @@ -2420,31 +2416,6 @@ ignition::math::Pose3d TransformToParentFrame(
return transformInParentLinkFrame;
}

/////////////////////////////////////////////////
/// reduced fixed joints: transform to parent frame
ignition::math::Pose3d inverseTransformToParentFrame(
ignition::math::Pose3d _transformInLinkFrame,
urdf::Pose _parentToLinkTransform)
{
ignition::math::Pose3d transformInParentLinkFrame;
// rotate link pose to parentLink frame
urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse();
ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z);
transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos();
urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse();
ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z);
transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot();
// translate link to parentLink frame
transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X()
- _parentToLinkTransform.position.x;
transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y()
- _parentToLinkTransform.position.y;
transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z()
- _parentToLinkTransform.position.z;

return transformInParentLinkFrame;
}

////////////////////////////////////////////////////////////////////////////////
void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link)
{
Expand Down Expand Up @@ -3473,14 +3444,21 @@ void ReduceSDFExtensionPluginFrameReplace(
// remove xyzOffset and rpyOffset
(*_blobIt)->RemoveChild(rpyKey);
}
TiXmlNode* correctedOffsetKey =
(*_blobIt)->FirstChild("ignition::corrected_offsets");
if (correctedOffsetKey)
{
(*_blobIt)->RemoveChild(correctedOffsetKey);
}

// pass through the parent transform from fixed joint reduction
_reductionTransform = inverseTransformToParentFrame(_reductionTransform,
_reductionTransform = TransformToParentFrame(_reductionTransform,
_link->parent_joint->parent_to_joint_origin_transform);

// create new offset xml blocks
xyzKey = new TiXmlElement("xyzOffset");
rpyKey = new TiXmlElement("rpyOffset");
correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets");

// create new offset xml blocks
urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
Expand All @@ -3501,12 +3479,15 @@ void ReduceSDFExtensionPluginFrameReplace(

TiXmlText* xyzTxt = new TiXmlText(xyzStream.str());
TiXmlText* rpyTxt = new TiXmlText(rpyStream.str());
TiXmlText* correctedOffsetTxt = new TiXmlText("1");

xyzKey->LinkEndChild(xyzTxt);
rpyKey->LinkEndChild(rpyTxt);
correctedOffsetKey->LinkEndChild(correctedOffsetTxt);

(*_blobIt)->LinkEndChild(xyzKey);
(*_blobIt)->LinkEndChild(rpyKey);
(*_blobIt)->LinkEndChild(correctedOffsetKey);
}
}
}
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 @@ -48,6 +48,9 @@ const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT =
const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF =
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration",
"fixed_joint_reduction_collision_visual_empty_root.sdf");
const std::string SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION =
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration",
"fixed_joint_reduction_plugin_frame_extension.urdf");

const double gc_tolerance = 1e-6;

Expand Down Expand Up @@ -736,3 +739,29 @@ 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 the offsets have the correct transfrom and frame of
// reference
TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
{
sdf::SDFPtr robot(new sdf::SDF());
sdf::init(robot);
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION, robot));

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

auto xyzOffset = plugin->Get<ignition::math::Vector3d>("xyzOffset");
auto rpyOffset = plugin->Get<ignition::math::Vector3d>("rpyOffset");
auto bodyName = plugin->Get<std::string>("bodyName");
EXPECT_EQ("base_link", bodyName);
EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset);
EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset);

bool correctedOffset = plugin->Get<bool>("ignition::corrected_offsets");
EXPECT_TRUE(correctedOffset);
}

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 c02aca7

Please sign in to comment.