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

Jump to bottom

Fix xyz and rpy offsets in fixed joint reduction #500

Merged
merged 8 commits into from
Aug 30, 2021
Merged

Fix xyz and rpy offsets in fixed joint reduction #500

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
9ca30cc
parse rpyOffset as radians
iche033 Feb 18, 2021
72381f1
update tf for xyz and rpy offsets
iche033 Feb 23, 2021
de3e268
remove inverse transform function in urdf parser
iche033 Feb 26, 2021
1e81e4a
Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame
ahcorde Mar 30, 2021
9bffb10
Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame
scpeters Jul 14, 2021
536b847
inject corrected_offets tag
iche033 Jul 16, 2021
2eba874
Fix tag removal logic
scpeters Jul 23, 2021
26deff0
Merge branch 'sdf6' into iche033/urdf_parser_plugin_frame
iche033 Aug 30, 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
41 changes: 11 additions & 30 deletions src/parser_urdf.cc
View file
Open in desktop
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)
Copy link
Member

Choose a reason for hiding this comment

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

I think the logic is backwards here. Should the ! be removed?

Copy link
Member

Choose a reason for hiding this comment

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

removed in 2eba874

{
(*_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
Open in desktop
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
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>