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.

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 xyz and rpy offsets in fixed joint reduction #500

Merged
merged 8 commits into from
Aug 30, 2021
Merged
Show file tree
Hide file tree
Changes from 6 commits
Commits
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
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
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
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>