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]>
gazebosim · Oct 3, 2023 · 013bc33 · 013bc33
1 parent ea0d5ba
commit 013bc33
Show file tree

Hide file tree

Showing 4 changed files with 168 additions and 5 deletions.
diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -3148,6 +3148,18 @@ void CreateJoint(tinyxml2::XMLElement *_root,
         AddKeyValue(jointAxisLimit, "velocity",
                     Values2str(1, &_link->parent_joint->limits->velocity));
       }
+
+      if (_link->parent_joint->mimic)
+      {
+        tinyxml2::XMLElement *jointAxisMimic = doc->NewElement("mimic");
+        jointAxisMimic->SetAttribute(
+            "joint", _link->parent_joint->mimic->joint_name.c_str());
+        AddKeyValue(jointAxisMimic, "multiplier",
+                    Values2str(1, &_link->parent_joint->mimic->multiplier));
+        AddKeyValue(jointAxisMimic, "offset",
+                    Values2str(1, &_link->parent_joint->mimic->offset));
+        jointAxis->LinkEndChild(jointAxisMimic);
+      }
     }
 
     if (jtype == "fixed" && !fixedJointConvertedToRevoluteJoint)
@@ -3408,9 +3420,9 @@ void URDF2SDF::InitModelString(const std::string &_urdfStr,
 
     try
     {
-      // URDF is compatible with version 1.7. The automatic conversion script
+      // URDF is compatible with version 1.11. The automatic conversion script
       // will up-convert URDF to SDF.
-      sdf->SetAttribute("version", "1.7");
+      sdf->SetAttribute("version", "1.11");
       // add robot to sdf
       sdf->LinkEndChild(robot);
     }

diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc
@@ -100,8 +100,8 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel)
 
   // SDF -> SDF
   std::ostringstream stream;
-  // parser_urdf.cc exports version "1.7"
-  stream << "<sdf version='" << "1.7" << "'>"
+  // parser_urdf.cc exports version "1.11"
+  stream << "<sdf version='" << "1.11" << "'>"
          << "  <model name='test_robot' />"
          << "</sdf>";
   tinyxml2::XMLDocument sdf_doc;
@@ -890,7 +890,7 @@ TEST(URDFParser, CheckJointTransform)
     << "  </link>"
     << "</robot>";
 
-  std::string expectedSdf = R"(<sdf version="1.7">
+  std::string expectedSdf = R"(<sdf version="1.11">
     <model name="test_robot">
         <joint type="fixed" name="jointw_1">
             <pose relative_to="__model__">0 0 0 0 0 0</pose>

diff --git a/test/integration/joint_axis_dom.cc b/test/integration/joint_axis_dom.cc
@@ -559,3 +559,29 @@ TEST(DOMJointAxis, ParseMimicInvalidLeaderAxis)
   EXPECT_EQ(errors[2].Message(), errorMsg2) << errors[2];
   EXPECT_EQ(errors[3].Message(), errorMsg3) << errors[3];
 }
+
+/////////////////////////////////////////////////
+TEST(DOMJointAxis, ParseMimicURDF)
+{
+  const std::string testFile =
+    sdf::testing::TestFile("sdf", "joint_mimic_rack_pinion.urdf");
+
+  sdf::Root root;
+  auto errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  auto model = root.Model();
+  ASSERT_NE(nullptr, model);
+  auto followerJoint = model->JointByName("rack_joint");
+  ASSERT_NE(nullptr, followerJoint);
+  auto followerJointAxis = followerJoint->Axis();
+  ASSERT_NE(nullptr, followerJointAxis);
+  auto mimicJoint = followerJointAxis->Mimic();
+  ASSERT_NE(std::nullopt, mimicJoint);
+
+  EXPECT_EQ(mimicJoint->Joint(), "upper_joint");
+  EXPECT_EQ(mimicJoint->Axis(), "axis");
+  EXPECT_DOUBLE_EQ(mimicJoint->Multiplier(), 0.105);
+  EXPECT_DOUBLE_EQ(mimicJoint->Offset(), 0);
+  EXPECT_DOUBLE_EQ(mimicJoint->Reference(), 0);
+}
diff --git a/test/sdf/joint_mimic_rack_pinion.urdf b/test/sdf/joint_mimic_rack_pinion.urdf
@@ -0,0 +1,125 @@
+<?xml version="1.0" ?>
+<robot name="rack_and_pinion">
+  <link name="base">
+    <inertial>
+      <mass value="2500"/>
+      <inertia ixx="154.0" ixy="0.0" ixz="0.0" iyy="152.0" iyz="0.0" izz="28.8"/>
+    </inertial>
+    <!-- plate on ground -->
+    <visual>
+      <geometry>
+        <cylinder length="0.02" radius="0.8"/>
+      </geometry>
+      <origin xyz="0 0 0.01"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.02" radius="0.8"/>
+      </geometry>
+      <origin xyz="0 0 0.01"/>
+    </collision>
+    <!-- pole -->
+    <visual>
+      <geometry>
+        <box size="0.2 0.2 2.2"/>
+      </geometry>
+      <origin xyz="-0.275 0 1.1"/>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.2 0.2 2.2"/>
+      </geometry>
+      <origin xyz="-0.275 0 1.1"/>
+    </collision>
+  </link>
+
+  <!-- pin joint for upper link -->
+  <joint name="upper_joint" type="continuous">
+    <origin rpy="-1.75 0 0" xyz="-0.025 0 2.1"/>
+    <axis xyz="1.0 0 0"/>
+    <parent link="base"/>
+    <child link="upper_link"/>
+  </joint>
+
+  <link name="upper_link">
+    <inertial>
+      <mass value="47"/>
+      <inertia ixx="6.7" ixy="0.0" ixz="0.0" iyy="6.7" iyz="0.0" izz="0.53"/>
+      <origin xyz="0 0 0.5"/>
+    </inertial>
+    <!-- upper joint shape -->
+    <visual>
+      <geometry>
+        <cylinder length="0.3" radius="0.1"/>
+      </geometry>
+      <origin rpy="0 1.5708 0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.3" radius="0.1"/>
+      </geometry>
+      <origin rpy="0 1.5708 0"/>
+    </collision>
+    <visual name="vis_gear">
+      <geometry>
+        <mesh scale="0.78 0.78 5.0" filename="https://fuel.gazebosim.org/1.0/openrobotics/models/Gear Part/2/files/meshes/gear.dae"/>
+      </geometry>
+      <origin xyz="-0.1 0 0" rpy="0 1.5708 0"/>
+    </visual>
+    <!-- long cylinder shape -->
+    <visual>
+      <geometry>
+        <cylinder length="0.9" radius="0.1"/>
+      </geometry>
+      <origin xyz="0 0 0.5"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.9" radius="0.1"/>
+      </geometry>
+      <origin xyz="0 0 0.5"/>
+    </collision>
+    <!-- bob shape -->
+    <visual>
+      <geometry>
+        <cylinder length="0.3" radius="0.1"/>
+      </geometry>
+      <origin rpy="0 1.5708 0" xyz="0 0 1.0"/>
+    </visual>
+    <collision>
+      <geometry>
+        <cylinder length="0.3" radius="0.1"/>
+      </geometry>
+      <origin rpy="0 1.5708 0" xyz="0 0 1.0"/>
+    </collision>
+  </link>
+
+  <!-- prismatic rack joint -->
+  <joint name="rack_joint" type="prismatic">
+    <origin xyz="-0.025 0 2.355"/>
+    <axis xyz="0 -1.0 0"/>
+    <parent link="base"/>
+    <child link="rack"/>
+    <limit lower="-1" upper="1" velocity="1000" effort="10000"/>
+    <mimic joint="upper_joint" multiplier="0.105"/>
+  </joint>
+
+  <!-- Rack coupled to upper pendulum joint -->
+  <link name="rack">
+    <inertial>
+      <mass value="9"/>
+      <inertia ixx="3.0" ixy="0.0" ixz="0.0" iyy="0.08" iyz="0.0" izz="3.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="0.15 2.0 0.3"/>
+      </geometry>
+    </visual>
+    <collision>
+      <geometry>
+        <box size="0.15 2.0 0.3"/>
+      </geometry>
+    </collision>
+  </link>
+
+</robot>