Skip to content

Commit 013bc33

Browse files
scpetersahcorde
scpeters
and
ahcorde
authored
URDF parser: use SDFormat 1.11, parse joint mimic (#1333)
This updates the URDF parser to generate SDFormat 1.11 files and adds support for parsing the URDF //joint/mimic tags into SDFormat //joint/axis/mimic tags. An example joint_mimic_rack_pinion.urdf is added with a test. Signed-off-by: Steve Peters <[email protected]> Co-authored-by: Alejandro Hernández Cordero <[email protected]>
1 parent ea0d5ba commit 013bc33

File tree

4 files changed

+168
-5
lines changed

4 files changed

+168
-5
lines changed

‎src/parser_urdf.cc‎

Lines changed: 14 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -3148,6 +3148,18 @@ void CreateJoint(tinyxml2::XMLElement *_root,
31483148
AddKeyValue(jointAxisLimit, "velocity",
31493149
Values2str(1, &_link->parent_joint->limits->velocity));
31503150
}
3151+
3152+
if (_link->parent_joint->mimic)
3153+
{
3154+
tinyxml2::XMLElement *jointAxisMimic = doc->NewElement("mimic");
3155+
jointAxisMimic->SetAttribute(
3156+
"joint", _link->parent_joint->mimic->joint_name.c_str());
3157+
AddKeyValue(jointAxisMimic, "multiplier",
3158+
Values2str(1, &_link->parent_joint->mimic->multiplier));
3159+
AddKeyValue(jointAxisMimic, "offset",
3160+
Values2str(1, &_link->parent_joint->mimic->offset));
3161+
jointAxis->LinkEndChild(jointAxisMimic);
3162+
}
31513163
}
31523164

31533165
if (jtype == "fixed" && !fixedJointConvertedToRevoluteJoint)
@@ -3408,9 +3420,9 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr,
34083420

34093421
try
34103422
{
3411-
// URDF is compatible with version 1.7. The automatic conversion script
3423+
// URDF is compatible with version 1.11. The automatic conversion script
34123424
// will up-convert URDF to SDF.
3413-
sdf->SetAttribute("version", "1.7");
3425+
sdf->SetAttribute("version", "1.11");
34143426
// add robot to sdf
34153427
sdf->LinkEndChild(robot);
34163428
}

‎src/parser_urdf_TEST.cc‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -100,8 +100,8 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel)
100100

101101
// SDF -> SDF
102102
std::ostringstream stream;
103-
// parser_urdf.cc exports version "1.7"
104-
stream << "<sdf version='" << "1.7" << "'>"
103+
// parser_urdf.cc exports version "1.11"
104+
stream << "<sdf version='" << "1.11" << "'>"
105105
<< " <model name='test_robot' />"
106106
<< "</sdf>";
107107
tinyxml2::XMLDocument sdf_doc;
@@ -890,7 +890,7 @@ TEST(URDFParser, CheckJointTransform)
890890
<< " </link>"
891891
<< "</robot>";
892892

893-
std::string expectedSdf = R"(<sdf version="1.7">
893+
std::string expectedSdf = R"(<sdf version="1.11">
894894
<model name="test_robot">
895895
<joint type="fixed" name="jointw_1">
896896
<pose relative_to="__model__">0 0 0 0 0 0</pose>

‎test/integration/joint_axis_dom.cc‎

Lines changed: 26 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -559,3 +559,29 @@ TEST(DOMJointAxis, ParseMimicInvalidLeaderAxis)
559559
EXPECT_EQ(errors[2].Message(), errorMsg2) << errors[2];
560560
EXPECT_EQ(errors[3].Message(), errorMsg3) << errors[3];
561561
}
562+
563+
/////////////////////////////////////////////////
564+
TEST(DOMJointAxis, ParseMimicURDF)
565+
{
566+
const std::string testFile =
567+
sdf::testing::TestFile("sdf", "joint_mimic_rack_pinion.urdf");
568+
569+
sdf::Root root;
570+
auto errors = root.Load(testFile);
571+
EXPECT_TRUE(errors.empty()) << errors;
572+
573+
auto model = root.Model();
574+
ASSERT_NE(nullptr, model);
575+
auto followerJoint = model->JointByName("rack_joint");
576+
ASSERT_NE(nullptr, followerJoint);
577+
auto followerJointAxis = followerJoint->Axis();
578+
ASSERT_NE(nullptr, followerJointAxis);
579+
auto mimicJoint = followerJointAxis->Mimic();
580+
ASSERT_NE(std::nullopt, mimicJoint);
581+
582+
EXPECT_EQ(mimicJoint->Joint(), "upper_joint");
583+
EXPECT_EQ(mimicJoint->Axis(), "axis");
584+
EXPECT_DOUBLE_EQ(mimicJoint->Multiplier(), 0.105);
585+
EXPECT_DOUBLE_EQ(mimicJoint->Offset(), 0);
586+
EXPECT_DOUBLE_EQ(mimicJoint->Reference(), 0);
587+
}

‎test/sdf/joint_mimic_rack_pinion.urdf‎

Lines changed: 125 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,125 @@
1+
<?xml version="1.0" ?>
2+
<robot name="rack_and_pinion">
3+
<link name="base">
4+
<inertial>
5+
<mass value="2500"/>
6+
<inertia ixx="154.0" ixy="0.0" ixz="0.0" iyy="152.0" iyz="0.0" izz="28.8"/>
7+
</inertial>
8+
<!-- plate on ground -->
9+
<visual>
10+
<geometry>
11+
<cylinder length="0.02" radius="0.8"/>
12+
</geometry>
13+
<origin xyz="0 0 0.01"/>
14+
</visual>
15+
<collision>
16+
<geometry>
17+
<cylinder length="0.02" radius="0.8"/>
18+
</geometry>
19+
<origin xyz="0 0 0.01"/>
20+
</collision>
21+
<!-- pole -->
22+
<visual>
23+
<geometry>
24+
<box size="0.2 0.2 2.2"/>
25+
</geometry>
26+
<origin xyz="-0.275 0 1.1"/>
27+
</visual>
28+
<collision>
29+
<geometry>
30+
<box size="0.2 0.2 2.2"/>
31+
</geometry>
32+
<origin xyz="-0.275 0 1.1"/>
33+
</collision>
34+
</link>
35+
36+
<!-- pin joint for upper link -->
37+
<joint name="upper_joint" type="continuous">
38+
<origin rpy="-1.75 0 0" xyz="-0.025 0 2.1"/>
39+
<axis xyz="1.0 0 0"/>
40+
<parent link="base"/>
41+
<child link="upper_link"/>
42+
</joint>
43+
44+
<link name="upper_link">
45+
<inertial>
46+
<mass value="47"/>
47+
<inertia ixx="6.7" ixy="0.0" ixz="0.0" iyy="6.7" iyz="0.0" izz="0.53"/>
48+
<origin xyz="0 0 0.5"/>
49+
</inertial>
50+
<!-- upper joint shape -->
51+
<visual>
52+
<geometry>
53+
<cylinder length="0.3" radius="0.1"/>
54+
</geometry>
55+
<origin rpy="0 1.5708 0"/>
56+
</visual>
57+
<collision>
58+
<geometry>
59+
<cylinder length="0.3" radius="0.1"/>
60+
</geometry>
61+
<origin rpy="0 1.5708 0"/>
62+
</collision>
63+
<visual name="vis_gear">
64+
<geometry>
65+
<mesh scale="0.78 0.78 5.0" filename="https://fuel.gazebosim.org/1.0/openrobotics/models/Gear Part/2/files/meshes/gear.dae"/>
66+
</geometry>
67+
<origin xyz="-0.1 0 0" rpy="0 1.5708 0"/>
68+
</visual>
69+
<!-- long cylinder shape -->
70+
<visual>
71+
<geometry>
72+
<cylinder length="0.9" radius="0.1"/>
73+
</geometry>
74+
<origin xyz="0 0 0.5"/>
75+
</visual>
76+
<collision>
77+
<geometry>
78+
<cylinder length="0.9" radius="0.1"/>
79+
</geometry>
80+
<origin xyz="0 0 0.5"/>
81+
</collision>
82+
<!-- bob shape -->
83+
<visual>
84+
<geometry>
85+
<cylinder length="0.3" radius="0.1"/>
86+
</geometry>
87+
<origin rpy="0 1.5708 0" xyz="0 0 1.0"/>
88+
</visual>
89+
<collision>
90+
<geometry>
91+
<cylinder length="0.3" radius="0.1"/>
92+
</geometry>
93+
<origin rpy="0 1.5708 0" xyz="0 0 1.0"/>
94+
</collision>
95+
</link>
96+
97+
<!-- prismatic rack joint -->
98+
<joint name="rack_joint" type="prismatic">
99+
<origin xyz="-0.025 0 2.355"/>
100+
<axis xyz="0 -1.0 0"/>
101+
<parent link="base"/>
102+
<child link="rack"/>
103+
<limit lower="-1" upper="1" velocity="1000" effort="10000"/>
104+
<mimic joint="upper_joint" multiplier="0.105"/>
105+
</joint>
106+
107+
<!-- Rack coupled to upper pendulum joint -->
108+
<link name="rack">
109+
<inertial>
110+
<mass value="9"/>
111+
<inertia ixx="3.0" ixy="0.0" ixz="0.0" iyy="0.08" iyz="0.0" izz="3.0"/>
112+
</inertial>
113+
<visual>
114+
<geometry>
115+
<box size="0.15 2.0 0.3"/>
116+
</geometry>
117+
</visual>
118+
<collision>
119+
<geometry>
120+
<box size="0.15 2.0 0.3"/>
121+
</geometry>
122+
</collision>
123+
</link>
124+
125+
</robot>

0 commit comments

Comments
 (0)