Skip to content

Commit c02aca7

Browse files
iche033ahcordescpeters
authored
Fix xyz and rpy offsets in fixed joint reduction (#500)
* parse rpyOffset as radians Signed-off-by: Ian Chen <ichen@osrfoundation.org> * update tf for xyz and rpy offsets Signed-off-by: Ian Chen <ichen@osrfoundation.org> * remove inverse transform function in urdf parser Signed-off-by: Ian Chen <ichen@osrfoundation.org> * inject corrected_offets tag Signed-off-by: Ian Chen <ichen@osrfoundation.org> * Fix tag removal logic Signed-off-by: Steven Peters <scpeters@openrobotics.org> Co-authored-by: Alejandro Hernández Cordero <alejandro@openrobotics.org> Co-authored-by: Steve Peters <scpeters@openrobotics.org>
1 parent b06cc18 commit c02aca7

3 files changed

Lines changed: 143 additions & 30 deletions

File tree

‎src/parser_urdf.cc‎

Lines changed: 11 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -219,10 +219,6 @@ std::string Values2str(unsigned int _count, const double *_values);
219219

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

222-
ignition::math::Pose3d inverseTransformToParentFrame(
223-
ignition::math::Pose3d _transformInLinkFrame,
224-
urdf::Pose _parentToLinkTransform);
225-
226222
/// reduced fixed joints: transform to parent frame
227223
urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame,
228224
urdf::Pose _parentToLinkTransform);
@@ -2420,31 +2416,6 @@ ignition::math::Pose3d TransformToParentFrame(
24202416
return transformInParentLinkFrame;
24212417
}
24222418

2423-
/////////////////////////////////////////////////
2424-
/// reduced fixed joints: transform to parent frame
2425-
ignition::math::Pose3d inverseTransformToParentFrame(
2426-
ignition::math::Pose3d _transformInLinkFrame,
2427-
urdf::Pose _parentToLinkTransform)
2428-
{
2429-
ignition::math::Pose3d transformInParentLinkFrame;
2430-
// rotate link pose to parentLink frame
2431-
urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse();
2432-
ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z);
2433-
transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos();
2434-
urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse();
2435-
ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z);
2436-
transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot();
2437-
// translate link to parentLink frame
2438-
transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X()
2439-
- _parentToLinkTransform.position.x;
2440-
transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y()
2441-
- _parentToLinkTransform.position.y;
2442-
transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z()
2443-
- _parentToLinkTransform.position.z;
2444-
2445-
return transformInParentLinkFrame;
2446-
}
2447-
24482419
////////////////////////////////////////////////////////////////////////////////
24492420
void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link)
24502421
{
@@ -3473,14 +3444,21 @@ void ReduceSDFExtensionPluginFrameReplace(
34733444
// remove xyzOffset and rpyOffset
34743445
(*_blobIt)->RemoveChild(rpyKey);
34753446
}
3447+
TiXmlNode* correctedOffsetKey =
3448+
(*_blobIt)->FirstChild("ignition::corrected_offsets");
3449+
if (correctedOffsetKey)
3450+
{
3451+
(*_blobIt)->RemoveChild(correctedOffsetKey);
3452+
}
34763453

34773454
// pass through the parent transform from fixed joint reduction
3478-
_reductionTransform = inverseTransformToParentFrame(_reductionTransform,
3455+
_reductionTransform = TransformToParentFrame(_reductionTransform,
34793456
_link->parent_joint->parent_to_joint_origin_transform);
34803457

34813458
// create new offset xml blocks
34823459
xyzKey = new TiXmlElement("xyzOffset");
34833460
rpyKey = new TiXmlElement("rpyOffset");
3461+
correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets");
34843462

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

35023480
TiXmlText* xyzTxt = new TiXmlText(xyzStream.str());
35033481
TiXmlText* rpyTxt = new TiXmlText(rpyStream.str());
3482+
TiXmlText* correctedOffsetTxt = new TiXmlText("1");
35043483

35053484
xyzKey->LinkEndChild(xyzTxt);
35063485
rpyKey->LinkEndChild(rpyTxt);
3486+
correctedOffsetKey->LinkEndChild(correctedOffsetTxt);
35073487

35083488
(*_blobIt)->LinkEndChild(xyzKey);
35093489
(*_blobIt)->LinkEndChild(rpyKey);
3490+
(*_blobIt)->LinkEndChild(correctedOffsetKey);
35103491
}
35113492
}
35123493
}

‎test/integration/fixed_joint_reduction.cc‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,9 @@ const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT =
4848
const std::string SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF =
4949
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration",
5050
"fixed_joint_reduction_collision_visual_empty_root.sdf");
51+
const std::string SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION =
52+
sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "integration",
53+
"fixed_joint_reduction_plugin_frame_extension.urdf");
5154

5255
const double gc_tolerance = 1e-6;
5356

@@ -736,3 +739,29 @@ TEST(SDFParser, FixedJointReductionSimple)
736739
EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
737740
}
738741
}
742+
743+
/////////////////////////////////////////////////
744+
// This test uses a urdf that has chained fixed joints with plugin that
745+
// contains bodyName, xyzOffset and rpyOffset.
746+
// Test to make sure the offsets have the correct transfrom and frame of
747+
// reference
748+
TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
749+
{
750+
sdf::SDFPtr robot(new sdf::SDF());
751+
sdf::init(robot);
752+
ASSERT_TRUE(sdf::readFile(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION, robot));
753+
754+
sdf::ElementPtr model = robot->Root()->GetElement("model");
755+
sdf::ElementPtr plugin = model->GetElement("plugin");
756+
757+
auto xyzOffset = plugin->Get<ignition::math::Vector3d>("xyzOffset");
758+
auto rpyOffset = plugin->Get<ignition::math::Vector3d>("rpyOffset");
759+
auto bodyName = plugin->Get<std::string>("bodyName");
760+
EXPECT_EQ("base_link", bodyName);
761+
EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset);
762+
EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset);
763+
764+
bool correctedOffset = plugin->Get<bool>("ignition::corrected_offsets");
765+
EXPECT_TRUE(correctedOffset);
766+
}
767+

‎test/integration/fixed_joint_reduction_plugin_frame_extension.urdf‎

Lines changed: 103 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,103 @@
1+
<?xml version="1.0" encoding="utf-8"?>
2+
<robot name="chained_fixed_joint_links">
3+
<gazebo>
4+
<plugin name='test_plugin' filename='libtest_plugin.so'>
5+
<serviceName>/test/plugin/service</serviceName>
6+
<topicName>/test/plugin/topic</topicName>
7+
<bodyName>link2</bodyName>
8+
<updateRate>100</updateRate>
9+
<xyzOffset>0 0 0</xyzOffset>
10+
<rpyOffset>0 0 0</rpyOffset>
11+
</plugin>
12+
</gazebo>
13+
14+
<!-- Base Link -->
15+
<link name="base_link">
16+
<collision>
17+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
18+
<geometry>
19+
<box size="1.0 1.0 1"/>
20+
</geometry>
21+
</collision>
22+
<visual>
23+
<origin rpy="0 0 0" xyz="0 0 1.0"/>
24+
<geometry>
25+
<box size="0.1 0.1 2"/>
26+
</geometry>
27+
</visual>
28+
<inertial>
29+
<origin xyz="0 0 1" rpy="0 0 0"/>
30+
<mass value="1"/>
31+
<inertia
32+
ixx="1.0" ixy="0.0" ixz="0.0"
33+
iyy="1.0" iyz="0.0"
34+
izz="1.0"/>
35+
</inertial>
36+
</link>
37+
38+
<!-- Link 1 -->
39+
<link name="link1">
40+
<collision>
41+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
42+
<geometry>
43+
<box size="1.0 1.0 1"/>
44+
</geometry>
45+
</collision>
46+
<visual>
47+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
48+
<geometry>
49+
<box size="0.1 0.1 1"/>
50+
</geometry>
51+
</visual>
52+
<inertial>
53+
<origin xyz="0 0 1" rpy="0 0 0"/>
54+
<mass value="1"/>
55+
<inertia
56+
ixx="1.0" ixy="0.0" ixz="0.0"
57+
iyy="1.0" iyz="0.0"
58+
izz="1.0"/>
59+
</inertial>
60+
</link>
61+
62+
<!-- Link 2 -->
63+
<link name="link2">
64+
<collision>
65+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
66+
<geometry>
67+
<box size="1.0 1.0 1"/>
68+
</geometry>
69+
</collision>
70+
<visual>
71+
<origin rpy="0 0 0" xyz="0 0 0.0"/>
72+
<geometry>
73+
<box size="0.1 0.1 1"/>
74+
</geometry>
75+
</visual>
76+
<inertial>
77+
<origin xyz="0 0 1" rpy="0 0 0"/>
78+
<mass value="1"/>
79+
<inertia
80+
ixx="1.0" ixy="0.0" ixz="0.0"
81+
iyy="1.0" iyz="0.0"
82+
izz="1.0"/>
83+
</inertial>
84+
</link>
85+
86+
<!-- Joint 1 -->
87+
<joint name="joint1" type="fixed">
88+
<parent link="base_link"/>
89+
<child link="link1"/>
90+
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
91+
<dynamics damping="0.7"/>
92+
</joint>
93+
94+
<!-- Joint 2 -->
95+
<joint name="joint2" type="fixed">
96+
<parent link="link1"/>
97+
<child link="link2"/>
98+
<origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
99+
<axis xyz="0 1 0"/>
100+
<dynamics damping="0.7"/>
101+
</joint>
102+
</robot>
103+

0 commit comments

Comments
 (0)