Add example joint_mimic_rack_pinion.urdf and test

Signed-off-by: Steve Peters <[email protected]>

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);
+}

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>