From 69ea0c7a8f779b7dc0b4c73f00a0742cb2908f46 Mon Sep 17 00:00:00 2001
From: Mario Prats <mario.prats@picknik.ai>
Date: Fri, 9 Aug 2024 06:00:25 +0200
Subject: [PATCH] Mujoco scene cosmetic improvements (#337)

* update scene camera position

* rotate scene 90 deg, add adapter to linear rail

---------

Co-authored-by: Michael Wrock <mike.wrock@gmail.com>
---
 .../description/picknik_ur.xacro              |  51 +++-
 .../description/scene.xml                     |  10 +-
 .../description/ur5e.xml                      | 273 +++++++++---------
 3 files changed, 190 insertions(+), 144 deletions(-)

diff --git a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro
index de03a457..ca3c56dd 100644
--- a/src/picknik_ur_mujoco_config/description/picknik_ur.xacro
+++ b/src/picknik_ur_mujoco_config/description/picknik_ur.xacro
@@ -530,7 +530,7 @@
   <joint name="world-rail" type="fixed">
     <parent link="world"/>
     <child link="linear_rail"/>
-    <origin rpy="0 0 0" xyz="0.0 0 0.6"/>
+    <origin rpy="0 0 1.5707" xyz="0.0 0 0.6"/>
   </joint>
   <!--
     Base UR robot series xacro macro.
@@ -591,7 +591,14 @@
   <!--<xacro:ur_arm_transmission prefix="${prefix}" hw_interface="${transmission_hw_interface}" />-->
   <!-- Placeholder for ros2_control transmission which don't yet exist -->
   <!-- links -  main serial chain -->
-  <link name="base_link"/>
+  <link name="base_link">
+    <visual>
+      <origin rpy="0 0 0" xyz="0.0 0.0 -0.2"/>
+      <geometry>
+        <box size="0.16 0.2 0.4"/>
+      </geometry>
+    </visual>
+  </link>
   <link name="base_link_inertia">
     <visual>
       <origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
@@ -727,7 +734,7 @@
   </link>
   <!-- base_joint fixes base_link to the environment -->
   <joint name="base_joint" type="fixed">
-    <origin rpy="0 0 0" xyz="0 -0.4 0.0"/>
+    <origin rpy="0 0 0" xyz="-0.06 -0.4 0.0"/>
     <parent link="ur_linear_rail_base"/>
     <child link="base_link"/>
   </joint>
@@ -877,6 +884,42 @@
         <mesh filename="package://picknik_accessories/descriptions/furniture/generic_table/wood_block.dae" scale="0.125 0.125 0.05"/>
       </geometry>
     </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.4 0.4 -0.25"/>
+      <geometry>
+        <box size="0.04 0.04 0.5"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.1 0.1 0.1 1.0"/>
+      </material>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.4 -0.4 -0.25"/>
+      <geometry>
+        <box size="0.04 0.04 0.5"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.1 0.1 0.1 1.0"/>
+      </material>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.4 -0.4 -0.25"/>
+      <geometry>
+        <box size="0.04 0.04 0.5"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.1 0.1 0.1 1.0"/>
+      </material>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="-0.4 0.4 -0.25"/>
+      <geometry>
+        <box size="0.04 0.04 0.5"/>
+      </geometry>
+      <material name="grey">
+        <color rgba="0.1 0.1 0.1 1.0"/>
+      </material>
+    </visual>
     <collision>
       <origin rpy="0 0 0" xyz="0 0 0"/>
       <geometry>
@@ -887,7 +930,7 @@
   <joint name="table_joint" type="fixed">
     <parent link="world"/>
     <child link="table"/>
-    <origin rpy="0 0 0" xyz="1 0 0.45"/>
+    <origin rpy="0 0 0" xyz="0 1 0.45"/>
   </joint>
   <ros2_control name="ur_mujoco_control" type="system">
     <hardware>
diff --git a/src/picknik_ur_mujoco_config/description/scene.xml b/src/picknik_ur_mujoco_config/description/scene.xml
index 1d2da458..dc0a1664 100644
--- a/src/picknik_ur_mujoco_config/description/scene.xml
+++ b/src/picknik_ur_mujoco_config/description/scene.xml
@@ -41,9 +41,9 @@
 
   <worldbody>
     <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
-    <geom name="floor" pos="0 0 -1.626" size="0 0 0.05" type="plane" material="groundplane"/>   
+    <geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane" material="groundplane"/>
     <!-- Define the table -->
-    <body name="table" pos="1 0 0.45">
+    <body name="table" pos="0 1 0.45">
         <geom type="box" size="0.5 0.5 0.025" material="wood"/>
     </body>
     <!-- Define the blocks with AprilTags on each face -->
@@ -74,9 +74,9 @@
     <key qpos="
       0 0 -1.5707 1.5707 -1.8 -1.5707 0
       0 0 0 0 0 0 0 0
-      0.75 0.2 0.51 1 0 0 0
-      0.75 0.0 0.51 1 0 0 0
-      0.75 -0.2 0.51 1 0 0 0
+      0.2 0.75 0.51 1 0 0 0
+      0.0 0.75 0.51 1 0 0 0
+      -0.2 0.75 0.51 1 0 0 0
     "
     ctrl="
       0 0 -1.5707 1.5707 -1.8 -1.5707 0 0
diff --git a/src/picknik_ur_mujoco_config/description/ur5e.xml b/src/picknik_ur_mujoco_config/description/ur5e.xml
index df4617f3..3acd0f2a 100644
--- a/src/picknik_ur_mujoco_config/description/ur5e.xml
+++ b/src/picknik_ur_mujoco_config/description/ur5e.xml
@@ -121,149 +121,152 @@
   <worldbody>
     <light name="spotlight" mode="targetbodycom" target="wrist_2_link" pos="0 -1 2"/>
 
-    <site name="scene_camera_optical_frame" pos="0.0 -2.0 3.5" euler="3.64 0.0 0.0"/>
-    <camera name="scene_camera" pos="0.0 -2.0 3.5" fovy="58" mode="fixed" resolution="640 480" euler="0.5 0.0 0.0"/>
+    <site name="scene_camera_optical_frame" pos="0.0 1.6 1.6" euler="2.2415 0 3.1415"/>
+    <camera name="scene_camera" pos="0.0 1.6 1.6" fovy="58" mode="fixed" resolution="640 480" euler="-0.9 0 3.1415"/>
 
-    <geom type="mesh" rgba="0 0.4 0.8 1" mesh="rail" pos="0 0 .6" euler="0 0 0"/>
-    <body name="base_platform" pos="0 0 .6" euler="0 0 0">
+    <geom type="mesh" rgba="0 0.4 0.8 1" mesh="rail" pos="0 0 .6" euler="0 0 1.5707"/>
+    <body name="base_platform" pos="0 0 .6" euler="0 0 1.5707">
       <joint name="linear_rail_joint" type="slide" axis="0 1 0" range="-5.0 5.0"/>
-      <body name="base" pos="0.0 -0.4 0" euler="0 0 0" childclass="ur5e">
-        <inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
-        <geom mesh="base_0" material="black" class="visual"/>
-        <geom mesh="base_1" material="jointgray" class="visual"/>
-        <body name="shoulder_link" pos="0 0 0.163">
-          <inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
-          <joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
-          <geom mesh="shoulder_0" material="urblue" class="visual"/>
-          <geom mesh="shoulder_1" material="black" class="visual"/>
-          <geom mesh="shoulder_2" material="jointgray" class="visual"/>
-          <geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
-          <body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
-            <inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
-            <joint name="shoulder_lift_joint" class="size3" axis="0 1 0"/>
-            <geom mesh="upperarm_0" material="linkgray" class="visual"/>
-            <geom mesh="upperarm_1" material="black" class="visual"/>
-            <geom mesh="upperarm_2" material="jointgray" class="visual"/>
-            <geom mesh="upperarm_3" material="urblue" class="visual"/>
-            <geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
-            <geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
-            <body name="forearm_link" pos="0 -0.131 0.425">
-              <inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
-              <joint name="elbow_joint" class="size3_limited"/>
-              <geom mesh="forearm_0" material="urblue" class="visual"/>
-              <geom mesh="forearm_1" material="linkgray" class="visual"/>
-              <geom mesh="forearm_2" material="black" class="visual"/>
-              <geom mesh="forearm_3" material="jointgray" class="visual"/>
-              <geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
-              <geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
-              <body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
-                <inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
-                <joint name="wrist_1_joint" class="size1"/>
-                <geom mesh="wrist1_0" material="black" class="visual"/>
-                <geom mesh="wrist1_1" material="urblue" class="visual"/>
-                <geom mesh="wrist1_2" material="jointgray" class="visual"/>
-                <geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
-                <body name="wrist_2_link" pos="0 0.127 0">
-                  <inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
-                  <joint name="wrist_2_joint" axis="0 0 1" class="size1"/>
-                  <geom mesh="wrist2_0" material="black" class="visual"/>
-                  <geom mesh="wrist2_1" material="urblue" class="visual"/>
-                  <geom mesh="wrist2_2" material="jointgray" class="visual"/>
-                  <geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
-                  <geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
-                  <body name="wrist_3_link" pos="0 0 0.1">
-                    <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
-                      diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
-                    <joint name="wrist_3_joint" class="size1"/>
-                    <geom material="linkgray" mesh="wrist3" class="visual"/>
-                    <!-- <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/> -->
-                    <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
-                    <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
-                    <site name="robotiq_ft_sensor" size="0.01" pos="0 0.1 0" quat="-1 1 0 0"/>
-                    <camera name="wrist_camera" pos="0.0 0.2 0.05" fovy="58" mode="fixed" resolution="640 480" euler="1.57 0.0 0.0"/>
-                    <site name="wrist_camera_optical_frame" pos="0.0 0.2 0.05" euler="-1.57 0 0"/>
-                    <body childclass="2f85" >
-                      <!-- Right-hand side 4-bar linkage -->
-                      <body name="robotiq_85_base_link" gravcomp="1" pos="0.0 0.1441 0" quat="0.5 -0.5 0.5 0.5">
-                        <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
-                          diaginertia="0.000260285 0.000225381 0.000152708"/>
-                        <geom class="visual" mesh="robotiq_85_base" material="black"/>
-                        <geom class="collision" mesh="robotiq_85_base"/>
-                        <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
+      <body name="adapter" pos="-0.06 -0.4 -0.2" euler="0 0 0">
+        <geom type="box" size="0.08 0.1 0.2" material="jointgray"/>
+        <body name="base" pos="0.0 0.0 0.2" euler="0 0 0" childclass="ur5e">
+          <inertial mass="4.0" pos="0 0 0" diaginertia="0.00443333156 0.00443333156 0.0072"/>
+          <geom mesh="base_0" material="black" class="visual"/>
+          <geom mesh="base_1" material="jointgray" class="visual"/>
+          <body name="shoulder_link" pos="0 0 0.163">
+            <inertial mass="3.7" pos="0 0 0" diaginertia="0.0102675 0.0102675 0.00666"/>
+            <joint name="shoulder_pan_joint" class="size3" axis="0 0 1"/>
+            <geom mesh="shoulder_0" material="urblue" class="visual"/>
+            <geom mesh="shoulder_1" material="black" class="visual"/>
+            <geom mesh="shoulder_2" material="jointgray" class="visual"/>
+            <geom class="collision" size="0.06 0.06" pos="0 0 -0.04"/>
+            <body name="upper_arm_link" pos="0 0.138 0" quat="1 0 1 0">
+              <inertial mass="8.393" pos="0 0 0.2125" diaginertia="0.133886 0.133886 0.0151074"/>
+              <joint name="shoulder_lift_joint" class="size3" axis="0 1 0"/>
+              <geom mesh="upperarm_0" material="linkgray" class="visual"/>
+              <geom mesh="upperarm_1" material="black" class="visual"/>
+              <geom mesh="upperarm_2" material="jointgray" class="visual"/>
+              <geom mesh="upperarm_3" material="urblue" class="visual"/>
+              <geom class="collision" pos="0 -0.04 0" quat="1 1 0 0" size="0.06 0.06"/>
+              <geom class="collision" size="0.05 0.2" pos="0 0 0.2"/>
+              <body name="forearm_link" pos="0 -0.131 0.425">
+                <inertial mass="2.275" pos="0 0 0.196" diaginertia="0.0311796 0.0311796 0.004095"/>
+                <joint name="elbow_joint" class="size3_limited"/>
+                <geom mesh="forearm_0" material="urblue" class="visual"/>
+                <geom mesh="forearm_1" material="linkgray" class="visual"/>
+                <geom mesh="forearm_2" material="black" class="visual"/>
+                <geom mesh="forearm_3" material="jointgray" class="visual"/>
+                <geom class="collision" pos="0 0.08 0" quat="1 1 0 0" size="0.055 0.06"/>
+                <geom class="collision" size="0.038 0.19" pos="0 0 0.2"/>
+                <body name="wrist_1_link" pos="0 0 0.392" quat="1 0 1 0">
+                  <inertial mass="1.219" pos="0 0.127 0" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                  <joint name="wrist_1_joint" class="size1"/>
+                  <geom mesh="wrist1_0" material="black" class="visual"/>
+                  <geom mesh="wrist1_1" material="urblue" class="visual"/>
+                  <geom mesh="wrist1_2" material="jointgray" class="visual"/>
+                  <geom class="collision" pos="0 0.05 0" quat="1 1 0 0" size="0.04 0.07"/>
+                  <body name="wrist_2_link" pos="0 0.127 0">
+                    <inertial mass="1.219" pos="0 0 0.1" diaginertia="0.0025599 0.0025599 0.0021942"/>
+                    <joint name="wrist_2_joint" axis="0 0 1" class="size1"/>
+                    <geom mesh="wrist2_0" material="black" class="visual"/>
+                    <geom mesh="wrist2_1" material="urblue" class="visual"/>
+                    <geom mesh="wrist2_2" material="jointgray" class="visual"/>
+                    <geom class="collision" size="0.04 0.06" pos="0 0 0.04"/>
+                    <geom class="collision" pos="0 0.02 0.1" quat="1 1 0 0" size="0.04 0.04"/>
+                    <body name="wrist_3_link" pos="0 0 0.1">
+                      <inertial mass="0.1889" pos="0 0.0771683 0" quat="1 0 0 1"
+                        diaginertia="0.000132134 9.90863e-05 9.90863e-05"/>
+                      <joint name="wrist_3_joint" class="size1"/>
+                      <geom material="linkgray" mesh="wrist3" class="visual"/>
+                      <!-- <geom class="eef_collision" pos="0 0.08 0" quat="1 1 0 0" size="0.04 0.02"/> -->
+                      <site name="attachment_site" pos="0 0.1 0" quat="-1 1 0 0"/>
+                      <!-- site for end effector's f/t sensor right at the base of the robotiq gripper -->
+                      <site name="robotiq_ft_sensor" size="0.01" pos="0 0.1 0" quat="-1 1 0 0"/>
+                      <camera name="wrist_camera" pos="0.0 0.2 0.05" fovy="58" mode="fixed" resolution="640 480" euler="1.57 0.0 0.0"/>
+                      <site name="wrist_camera_optical_frame" pos="0.0 0.2 0.05" euler="-1.57 0 0"/>
+                      <body childclass="2f85" >
                         <!-- Right-hand side 4-bar linkage -->
-                        <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
-                          <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
-                          <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
-                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
-                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
-                          <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
-                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
-                            <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
-                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
-                            <geom class="collision" mesh="robotiq_85_finger_link"/>
+                        <body name="robotiq_85_base_link" gravcomp="1" pos="0.0 0.1441 0" quat="0.5 -0.5 0.5 0.5">
+                          <inertial mass="0.777441" pos="0 -2.70394e-05 0.0354675" quat="1 -0.00152849 0 0"
+                            diaginertia="0.000260285 0.000225381 0.000152708"/>
+                          <geom class="visual" mesh="robotiq_85_base" material="black"/>
+                          <geom class="collision" mesh="robotiq_85_base"/>
+                          <site name="pinch" pos="0 0 0.145" type="sphere" group="5" rgba="0.9 0.9 0.9 1" size="0.005"/>
+                          <!-- Right-hand side 4-bar linkage -->
+                          <body name="robotiq_85_right_knuckle_link" gravcomp="1" pos="0 0.0306011 0.054904">
+                            <inertial mass="0.00899563" pos="2.96931e-12 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
+                              diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                            <joint name="robotiq_85_right_knuckle_joint" class="knuckle_link"/>
+                            <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                            <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                            <body name="robotiq_85_right_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                              <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
+                                diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                              <joint name="robotiq_85_right_finger_joint" class="finger_link"/>
+                              <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_finger_link"/>
+                            </body>
                           </body>
-                        </body>
-                        <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
-                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
-                          <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
-                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
-                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
-                          <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
-                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
-                            <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
-                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
-                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
-                            <body name="right_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="right_pad1"/>
-                              <geom class="pad_box2" name="right_pad2"/>
-                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
-                              <geom class="visual" mesh="robotiq_85_pad"/>
-                              <body name="right_silicone_pad">
-                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
+                          <body name="robotiq_85_right_inner_knuckle_link" gravcomp="1" pos="0 0.0132 0.0609">
+                            <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
+                              diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                            <joint name="robotiq_85_right_inner_knuckle_joint" class="inner_knuckle_link"/>
+                            <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                            <body name="robotiq_85_right_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                              <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
+                                diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                              <joint name="robotiq_85_right_finger_tip_joint" class="finger_tip_link"/>
+                              <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
+                              <body name="right_pad" pos="0 -0.0189 0.01352">
+                                <geom class="pad_box1" name="right_pad1"/>
+                                <geom class="pad_box2" name="right_pad2"/>
+                                <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="0.707107 0 0 0.707107"
+                                  diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                                <geom class="visual" mesh="robotiq_85_pad"/>
+                                <body name="right_silicone_pad">
+                                  <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
+                                </body>
                               </body>
                             </body>
                           </body>
-                        </body>
-                        <!-- Left-hand side 4-bar linkage -->
-                        <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
-                          <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
-                            diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
-                          <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
-                          <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
-                          <geom class="collision" mesh="robotiq_85_knuckle_link"/>
-                          <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
-                            <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
-                              diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
-                            <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
-                            <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
-                            <geom class="collision" mesh="robotiq_85_finger_link"/>
+                          <!-- Left-hand side 4-bar linkage -->
+                          <body name="robotiq_85_left_knuckle_link" gravcomp="1" pos="0 -0.0306011 0.054904" quat="0 0 0 1">
+                            <inertial mass="0.00899563" pos="0 0.0177547 0.00107314" quat="0.681301 0.732003 0 0"
+                              diaginertia="1.72352e-06 1.60906e-06 3.22006e-07"/>
+                            <joint name="robotiq_85_left_knuckle_joint" class="knuckle_link"/>
+                            <geom class="visual" mesh="robotiq_85_knuckle_link" material="gray"/>
+                            <geom class="collision" mesh="robotiq_85_knuckle_link"/>
+                            <body name="robotiq_85_left_finger_link" gravcomp="1" pos="0 0.0315 -0.0041">
+                              <inertial mass="0.0140974" pos="0 0.00301209 0.0232175" quat="0.705636 -0.0455904 0.0455904 0.705636"
+                                diaginertia="4.16206e-06 3.52216e-06 8.88131e-07"/>
+                              <joint name="robotiq_85_left_finger_joint" class="finger_link"/>
+                              <geom class="visual" mesh="robotiq_85_finger_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_finger_link"/>
+                            </body>
                           </body>
-                        </body>
-                        <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
-                          <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
-                            diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
-                          <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
-                          <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
-                          <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
-                          <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
-                            <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
-                              diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
-                            <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
-                            <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
-                            <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
-                            <body name="left_pad" pos="0 -0.0189 0.01352">
-                              <geom class="pad_box1" name="left_pad1"/>
-                              <geom class="pad_box2" name="left_pad2"/>
-                              <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1"
-                                diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
-                              <geom class="visual" mesh="robotiq_85_pad"/>
-                              <body name="left_silicone_pad">
-                                <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
+                          <body name="robotiq_85_left_inner_knuckle_link" gravcomp="1" pos="0 -0.0132 0.0609" quat="0 0 0 1">
+                            <inertial mass="0.0221642" pos="-8.65005e-09 0.0181624 0.0212658" quat="0.663403 -0.244737 0.244737 0.663403"
+                              diaginertia="8.96853e-06 6.71733e-06 2.63931e-06"/>
+                            <joint name="robotiq_85_left_inner_knuckle_joint" class="inner_knuckle_link"/>
+                            <geom class="visual" mesh="robotiq_85_inner_knuckle_link" material="black"/>
+                            <geom class="collision" mesh="robotiq_85_inner_knuckle_link"/>
+                            <body name="robotiq_85_left_finger_tip_link" gravcomp="1" pos="0 0.055 0.0375">
+                              <inertial mass="0.0125222" pos="0 -0.011046 0.0124786" quat="1 0.1664 0 0"
+                                diaginertia="2.67415e-06 2.4559e-06 6.02031e-07"/>
+                              <joint name="robotiq_85_left_finger_tip_joint" class="finger_tip_link"/>
+                              <geom class="visual" mesh="robotiq_85_finger_tip_link" material="black"/>
+                              <geom class="collision" mesh="robotiq_85_finger_tip_link"/>
+                              <body name="left_pad" pos="0 -0.0189 0.01352">
+                                <geom class="pad_box1" name="left_pad1"/>
+                                <geom class="pad_box2" name="left_pad2"/>
+                                <inertial mass="0.0035" pos="0 -0.0025 0.0185" quat="1 0 0 1"
+                                  diaginertia="4.73958e-07 3.64583e-07 1.23958e-07"/>
+                                <geom class="visual" mesh="robotiq_85_pad"/>
+                                <body name="left_silicone_pad">
+                                  <geom class="visual" mesh="robotiq_85_silicone_pad" material="black"/>
+                                </body>
                               </body>
                             </body>
                           </body>