diff --git a/Simulation/SO101/README.md b/Simulation/SO101/README.md
new file mode 100644
index 0000000..7b77320
--- /dev/null
+++ b/Simulation/SO101/README.md
@@ -0,0 +1,34 @@
+# SO101 Robot - URDF and MuJoCo Description
+
+This repository contains the URDF and MuJoCo (MJCF) files for the SO101 robot.
+
+## Overview
+
+- The robot model files were generated using the [onshape-to-robot](https://github.com/Rhoban/onshape-to-robot) plugin from a CAD model designed in Onshape.
+- Base collision meshes were removed due to problematic collision behavior during simulation and planning.
+
+## Calibration Methods
+
+The MuJoCo file `scene.xml` supports two differenly calibrated SO101 robot files:
+
+- **New Calibration (Default)**: Each joint's virtual zero is set to the **middle** of its joint range. Use -> `so101_new_calib.xml`. 
+- **Old Calibration**: Each joint's virtual zero is set to the configuration where the robot is **fully extended horizontally**. Use -> `so101_old_calib.xml`.
+
+To switch between calibration methods, modify the included robot file in `scene.xml`.
+
+## Motor Parameters
+
+Motor properties for the STS3215 motors used in the robot are adapted from the [Open Duck Mini project](https://github.com/apirrone/Open_Duck_Mini).
+
+## Gripper Note
+
+In LeRobot, the gripper is represented as a **linear joint**, where:
+
+* `0` = fully closed
+* `100` = fully open
+
+This mapping is **not yet reflected** in the current URDF and MuJoCo files. 
+
+---
+
+Feel free to open an issue or contribute improvements!
diff --git a/Simulation/SO101/joints_properties.xml b/Simulation/SO101/joints_properties.xml
index 8b98629..0e2b2a5 100644
--- a/Simulation/SO101/joints_properties.xml
+++ b/Simulation/SO101/joints_properties.xml
@@ -1,4 +1,3 @@
-<!-- Taken from https://github.com/apirrone/Open_Duck_Playground -->
 <default>
     <default class="sts3215">
         <geom contype="0" conaffinity="0"/>
diff --git a/Simulation/SO101/scene.xml b/Simulation/SO101/scene.xml
index 0b5c370..255b970 100644
--- a/Simulation/SO101/scene.xml
+++ b/Simulation/SO101/scene.xml
@@ -1,5 +1,5 @@
 <mujoco model="scene">
-    <include file="so101.xml" />
+    <include file="so101_new_calib.xml" />
 
     <visual>
         <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
diff --git a/Simulation/SO101/so101.urdf b/Simulation/SO101/so101_new_calib.urdf
similarity index 86%
rename from Simulation/SO101/so101.urdf
rename to Simulation/SO101/so101_new_calib.urdf
index 0aba5de..c55a1d7 100644
--- a/Simulation/SO101/so101.urdf
+++ b/Simulation/SO101/so101_new_calib.urdf
@@ -1,6 +1,7 @@
 <?xml version="1.0" ?>
 <!-- Generated using onshape-to-robot -->
-<robot name="so101">
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/826c553ede3b7592eb9ca800 -->
+<robot name="so101_new_calib">
   <!-- Link base -->
   <link name="base">
     <inertial>
@@ -153,7 +154,7 @@
     </inertial>
     <!-- Part sts3215_03a_v1_3 -->
     <visual>
-      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -1.31839e-15 -1.5708"/>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -6.8695e-16 -1.5708"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_v1.stl"/>
       </geometry>
@@ -162,14 +163,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -1.31839e-15 -1.5708"/>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -6.8695e-16 -1.5708"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_v1.stl"/>
       </geometry>
     </collision>
     <!-- Part upper_arm_so101_v1 -->
     <visual>
-      <origin xyz="-0.065085 0.012 0.0182" rpy="-3.14159 -0 0"/>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -9.35612e-32 0"/>
       <geometry>
         <mesh filename="package://assets/upper_arm_so101_v1.stl"/>
       </geometry>
@@ -178,7 +179,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.065085 0.012 0.0182" rpy="-3.14159 -0 0"/>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -9.35612e-32 0"/>
       <geometry>
         <mesh filename="package://assets/upper_arm_so101_v1.stl"/>
       </geometry>
@@ -193,7 +194,7 @@
     </inertial>
     <!-- Part under_arm_so101_v1 -->
     <visual>
-      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 9.86076e-32 7.88861e-31"/>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="-3.14159 -0 3.9443e-31"/>
       <geometry>
         <mesh filename="package://assets/under_arm_so101_v1.stl"/>
       </geometry>
@@ -202,14 +203,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 9.86076e-32 7.88861e-31"/>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="-3.14159 -0 3.9443e-31"/>
       <geometry>
         <mesh filename="package://assets/under_arm_so101_v1.stl"/>
       </geometry>
     </collision>
     <!-- Part motor_holder_so101_wrist_v1 -->
     <visual>
-      <origin xyz="-0.0648499 -0.032 0.018" rpy="3.14159 1.58207e-15 7.14706e-16"/>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 4.73317e-30 7.88861e-31"/>
       <geometry>
         <mesh filename="package://assets/motor_holder_so101_wrist_v1.stl"/>
       </geometry>
@@ -218,14 +219,14 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.0648499 -0.032 0.018" rpy="3.14159 1.58207e-15 7.14706e-16"/>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 4.73317e-30 7.88861e-31"/>
       <geometry>
         <mesh filename="package://assets/motor_holder_so101_wrist_v1.stl"/>
       </geometry>
     </collision>
     <!-- Part sts3215_03a_v1_4 -->
     <visual>
-      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -6.93889e-15 -3.14159"/>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -3.58047e-15 -3.14159"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_v1.stl"/>
       </geometry>
@@ -234,7 +235,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -6.93889e-15 -3.14159"/>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -3.58047e-15 -3.14159"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_v1.stl"/>
       </geometry>
@@ -249,7 +250,7 @@
     </inertial>
     <!-- Part sts3215_03a_no_horn_v1 -->
     <visual>
-      <origin xyz="0 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <origin xyz="5.55112e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_no_horn_v1.stl"/>
       </geometry>
@@ -258,7 +259,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="0 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <origin xyz="5.55112e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
       <geometry>
         <mesh filename="package://assets/sts3215_03a_no_horn_v1.stl"/>
       </geometry>
@@ -305,7 +306,7 @@
     </collision>
     <!-- Part wrist_roll_follower_so101_v1 -->
     <visual>
-      <origin xyz="5.55112e-17 -0.000218214 0.000949706" rpy="3.14159 -5.55112e-17 -2.54515e-32"/>
+      <origin xyz="5.55112e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 -9.17912e-24"/>
       <geometry>
         <mesh filename="package://assets/wrist_roll_follower_so101_v1.stl"/>
       </geometry>
@@ -314,7 +315,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="5.55112e-17 -0.000218214 0.000949706" rpy="3.14159 -5.55112e-17 -2.54515e-32"/>
+      <origin xyz="5.55112e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 -9.17912e-24"/>
       <geometry>
         <mesh filename="package://assets/wrist_roll_follower_so101_v1.stl"/>
       </geometry>
@@ -330,7 +331,7 @@
     </inertial>
   </link>
   <joint name="gripper_frame" type="fixed">
-    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="-1.5708 -0 -1.5708"/>
+    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="-1.5708 1.97215e-31 -1.5708"/>
     <parent link="gripper"/>
     <child link="gripper"/>
     <axis xyz="0 0 0"/>
@@ -344,7 +345,7 @@
     </inertial>
     <!-- Part moving_jaw_so101_v1 -->
     <visual>
-      <origin xyz="2.77556e-17 -1.31397e-17 0.0189" rpy="9.53145e-17 1.02153e-32 1.57772e-30"/>
+      <origin xyz="-5.55112e-17 -1.94746e-17 0.0189" rpy="9.53145e-17 -4.66093e-24 0"/>
       <geometry>
         <mesh filename="package://assets/moving_jaw_so101_v1.stl"/>
       </geometry>
@@ -353,7 +354,7 @@
       </material>
     </visual>
     <collision>
-      <origin xyz="2.77556e-17 -1.31397e-17 0.0189" rpy="9.53145e-17 1.02153e-32 1.57772e-30"/>
+      <origin xyz="-5.55112e-17 -1.94746e-17 0.0189" rpy="9.53145e-17 -4.66093e-24 0"/>
       <geometry>
         <mesh filename="package://assets/moving_jaw_so101_v1.stl"/>
       </geometry>
@@ -361,15 +362,15 @@
   </link>
   <!-- Joint from gripper to moving_jaw_so101_v1 -->
   <joint name="6" type="revolute">
-    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 4.00612e-15 -2.2678e-16"/>
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -5.14108e-17 -1.38655e-14"/>
     <parent link="gripper"/>
     <child link="moving_jaw_so101_v1"/>
     <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.261799" upper="1.74533"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
   </joint>
   <!-- Joint from wrist to gripper -->
   <joint name="5" type="revolute">
-    <origin xyz="-2.77556e-17 -0.0611 0.0181" rpy="1.5708 2.18394e-16 3.14159"/>
+    <origin xyz="0 -0.0611 0.0181" rpy="1.5708 -9.38083e-08 3.14159"/>
     <parent link="wrist"/>
     <child link="gripper"/>
     <axis xyz="0 0 1"/>
@@ -377,7 +378,7 @@
   </joint>
   <!-- Joint from lower_arm to wrist -->
   <joint name="4" type="revolute">
-    <origin xyz="-0.1349 0.0052 3.21791e-16" rpy="-1.11022e-16 1.11022e-16 -1.5708"/>
+    <origin xyz="-0.1349 0.0052 1.65232e-16" rpy="3.2474e-15 2.86219e-15 -1.5708"/>
     <parent link="lower_arm"/>
     <child link="wrist"/>
     <axis xyz="0 0 1"/>
@@ -385,19 +386,19 @@
   </joint>
   <!-- Joint from upper_arm to lower_arm -->
   <joint name="3" type="revolute">
-    <origin xyz="-0.11257 -0.028 2.29417e-16" rpy="-2.26208e-15 -1.3739e-15 3.14159"/>
+    <origin xyz="-0.11257 -0.028 2.46331e-16" rpy="-1.22818e-15 5.75928e-16 1.5708"/>
     <parent link="upper_arm"/>
     <child link="lower_arm"/>
     <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-3.22886" upper="0.0872665"/>
+    <limit effort="10" velocity="10" lower="-1.74533" upper="1.5708"/>
   </joint>
   <!-- Joint from shoulder to upper_arm -->
   <joint name="2" type="revolute">
-    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="1.5708 8.18789e-16 3.14159"/>
+    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="-1.5708 -1.5708 0"/>
     <parent link="shoulder"/>
     <child link="upper_arm"/>
     <axis xyz="0 0 1"/>
-    <limit effort="10" velocity="10" lower="-0.174533" upper="3.31613"/>
+    <limit effort="10" velocity="10" lower="-1.74533" upper="1.74533"/>
   </joint>
   <!-- Joint from base to shoulder -->
   <joint name="1" type="revolute">
diff --git a/Simulation/SO101/so101.xml b/Simulation/SO101/so101_new_calib.xml
similarity index 80%
rename from Simulation/SO101/so101.xml
rename to Simulation/SO101/so101_new_calib.xml
index fa9157b..a95af55 100644
--- a/Simulation/SO101/so101.xml
+++ b/Simulation/SO101/so101_new_calib.xml
@@ -1,9 +1,10 @@
 <?xml version="1.0" ?>
 <!-- Generated using onshape-to-robot -->
-<mujoco model="so101">
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/826c553ede3b7592eb9ca800 -->
+<mujoco model="so101_new_calib">
   <compiler angle="radian" meshdir="assets" autolimits="true"/>
   <default>
-    <default class="so101">
+    <default class="so101_new_calib">
       <joint frictionloss="0.1" armature="0.005"/>
       <position kp="50" dampratio="1"/>
       <default class="visual">
@@ -14,7 +15,7 @@
       </default>
     </default>
   </default>
-  <!-- Additional joints_properties.xml, from https://github.com/apirrone/Open_Duck_Playground -->
+  <!-- Additional joints_properties.xml -->
   <default>
     <default class="sts3215">
       <geom contype="0" conaffinity="0"/>
@@ -28,7 +29,7 @@
   </default>
   <worldbody>
     <!-- Link base -->
-    <body name="base" pos="0 0 0" quat="1 0 0 0" childclass="so101">
+    <body name="base" pos="0 0 0" quat="1 0 0 0" childclass="so101_new_calib">
       <inertial pos="0.020739 0.00204287 0.065966" mass="0.147" fullinertia="0.000136117 0.000114686 0.000130364 4.59787e-07 9.75275e-08 -4.97151e-06"/>
       <!-- Part base_motor_holder_so101_v1 -->
       <geom type="mesh" class="visual" pos="0.0206915 0.0221255 0.0300817" quat="0.707107 0.707107 7.85046e-16 8.68107e-16" mesh="base_motor_holder_so101_v1" material="base_motor_holder_so101_v1_material"/>
@@ -55,62 +56,62 @@
         <geom type="mesh" class="visual" pos="0.0122008 2.22413e-05 0.0464" quat="0.707107 -0.707107 0 0" mesh="rotation_pitch_so101_v1" material="rotation_pitch_so101_v1_material"/>
         <geom type="mesh" class="collision" pos="0.0122008 2.22413e-05 0.0464" quat="0.707107 -0.707107 0 0" mesh="rotation_pitch_so101_v1" material="rotation_pitch_so101_v1_material"/>
         <!-- Link upper_arm -->
-        <body name="upper_arm" pos="-0.0303992 -0.0182778 -0.0542" quat="1.88657e-16 -0 0.707107 0.707107">
+        <body name="upper_arm" pos="-0.0303992 -0.0182778 -0.0542" quat="0.5 -0.5 -0.5 -0.5">
           <!-- Joint from shoulder to upper_arm -->
-          <joint axis="0 0 1" name="2" type="hinge" range="-0.17453292519943472 3.3161255787892245" class="sts3215"/>
+          <joint axis="0 0 1" name="2" type="hinge" range="-1.7453292519943269 1.7453292519943322" class="sts3215"/>
           <inertial pos="-0.0898471 -0.00838224 0.0184089" mass="0.103" fullinertia="4.08002e-05 0.000147318 0.000142487 -1.97819e-05 -4.03016e-08 8.97326e-09"/>
           <!-- Part sts3215_03a_v1_3 -->
-          <geom type="mesh" class="visual" pos="-0.11257 -0.0155 0.0187" quat="8.61227e-17 -0.707107 0.707107 -6.73749e-18" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
-          <geom type="mesh" class="collision" pos="-0.11257 -0.0155 0.0187" quat="8.61227e-17 -0.707107 0.707107 -6.73749e-18" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+          <geom type="mesh" class="visual" pos="-0.11257 -0.0155 0.0187" quat="4.31775e-16 -0.707107 0.707107 -2.94392e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+          <geom type="mesh" class="collision" pos="-0.11257 -0.0155 0.0187" quat="4.31775e-16 -0.707107 0.707107 -2.94392e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
           <!-- Part upper_arm_so101_v1 -->
           <geom type="mesh" class="visual" pos="-0.065085 0.012 0.0182" quat="0 1 0 0" mesh="upper_arm_so101_v1" material="upper_arm_so101_v1_material"/>
           <geom type="mesh" class="collision" pos="-0.065085 0.012 0.0182" quat="0 1 0 0" mesh="upper_arm_so101_v1" material="upper_arm_so101_v1_material"/>
           <!-- Link lower_arm -->
-          <body name="lower_arm" pos="-0.11257 -0.028 2.29417e-16" quat="1.34913e-15 3.69655e-16 -7.40756e-16 1">
+          <body name="lower_arm" pos="-0.11257 -0.028 2.46331e-16" quat="0.707107 -2.94392e-16 -3.85906e-16 0.707107">
             <!-- Joint from upper_arm to lower_arm -->
-            <joint axis="0 0 1" name="3" type="hinge" range="-3.2288591161895077 0.08726646259971825" class="sts3215"/>
+            <joint axis="0 0 1" name="3" type="hinge" range="-1.7453292519943295 1.5707963267948966" class="sts3215"/>
             <inertial pos="-0.0980701 0.00324376 0.0182831" mass="0.104" fullinertia="2.87438e-05 0.000159844 0.00014529 7.41152e-06 1.26409e-06 -4.90188e-08"/>
             <!-- Part under_arm_so101_v1 -->
             <geom type="mesh" class="visual" pos="-0.0648499 -0.032 0.0182" quat="0 1 0 0" mesh="under_arm_so101_v1" material="under_arm_so101_v1_material"/>
             <geom type="mesh" class="collision" pos="-0.0648499 -0.032 0.0182" quat="0 1 0 0" mesh="under_arm_so101_v1" material="under_arm_so101_v1_material"/>
             <!-- Part motor_holder_so101_wrist_v1 -->
-            <geom type="mesh" class="visual" pos="-0.0648499 -0.032 0.018" quat="1.48409e-16 1 9.52363e-16 -5.84602e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
-            <geom type="mesh" class="collision" pos="-0.0648499 -0.032 0.018" quat="1.48409e-16 1 9.52363e-16 -5.84602e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
+            <geom type="mesh" class="visual" pos="-0.0648499 -0.032 0.018" quat="3.3891e-16 -1 -1.9186e-15 6.33174e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
+            <geom type="mesh" class="collision" pos="-0.0648499 -0.032 0.018" quat="3.3891e-16 -1 -1.9186e-15 6.33174e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
             <!-- Part sts3215_03a_v1_4 -->
-            <geom type="mesh" class="visual" pos="-0.1224 0.0052 0.0187" quat="1.69977e-15 -2.89309e-15 -1 6.11166e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
-            <geom type="mesh" class="collision" pos="-0.1224 0.0052 0.0187" quat="1.69977e-15 -2.89309e-15 -1 6.11166e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <geom type="mesh" class="visual" pos="-0.1224 0.0052 0.0187" quat="8.32667e-16 -1.56949e-15 -1 5.55112e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <geom type="mesh" class="collision" pos="-0.1224 0.0052 0.0187" quat="8.32667e-16 -1.56949e-15 -1 5.55112e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
             <!-- Link wrist -->
-            <body name="wrist" pos="-0.1349 0.0052 3.21791e-16" quat="0.707107 -1.48288e-20 1.96685e-16 -0.707107">
+            <body name="wrist" pos="-0.1349 0.0052 1.65232e-16" quat="0.707107 1.31614e-15 9.90601e-17 -0.707107">
               <!-- Joint from lower_arm to wrist -->
-              <joint axis="0 0 1" name="4" type="hinge" range="-1.658062789394615 1.6580627893946114" class="sts3215"/>
+              <joint axis="0 0 1" name="4" type="hinge" range="-1.6580627969561903 1.658062781833036" class="sts3215"/>
               <inertial pos="-0.000103312 -0.0386143 0.0281156" mass="0.079" fullinertia="3.68263e-05 2.5391e-05 2.1e-05 1.7893e-08 -5.28128e-08 3.6412e-06"/>
               <!-- Part sts3215_03a_no_horn_v1 -->
-              <geom type="mesh" class="visual" pos="0 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
-              <geom type="mesh" class="collision" pos="0 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
+              <geom type="mesh" class="visual" pos="5.55112e-17 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
+              <geom type="mesh" class="collision" pos="5.55112e-17 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
               <!-- Part wrist_roll_pitch_so101_v2 -->
               <geom type="mesh" class="visual" pos="0 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
               <geom type="mesh" class="collision" pos="0 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
               <!-- Link gripper -->
-              <body name="gripper" pos="-2.77556e-17 -0.0611 0.0181" quat="4.29003e-16 1.03623e-16 0.707107 0.707107">
+              <body name="gripper" pos="0 -0.0611 0.0181" quat="3.31663e-08 -3.31663e-08 -0.707107 -0.707107">
                 <!-- Joint from wrist to gripper -->
-                <joint axis="0 0 1" name="5" type="hinge" range="-2.792526803190913 2.7925268031909414" class="sts3215"/>
+                <joint axis="0 0 1" name="5" type="hinge" range="-2.7925268969992407 2.7925267093826136" class="sts3215"/>
                 <inertial pos="0.000213627 0.000245138 -0.025187" mass="0.087" fullinertia="2.75087e-05 4.33657e-05 3.45059e-05 -3.35241e-07 -5.7352e-06 -5.17847e-08"/>
                 <!-- Part sts3215_03a_v1_5 -->
-                <geom type="mesh" class="visual" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.59247e-15 1.57708e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
-                <geom type="mesh" class="collision" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.59247e-15 1.57708e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+                <geom type="mesh" class="visual" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.48145e-15 1.58472e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+                <geom type="mesh" class="collision" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.48145e-15 1.58472e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
                 <!-- Part wrist_roll_follower_so101_v1 -->
                 <geom type="mesh" class="visual" pos="5.55112e-17 -0.000218214 0.000949706" quat="0 1 0 0" mesh="wrist_roll_follower_so101_v1" material="wrist_roll_follower_so101_v1_material"/>
                 <geom type="mesh" class="collision" pos="5.55112e-17 -0.000218214 0.000949706" quat="0 1 0 0" mesh="wrist_roll_follower_so101_v1" material="wrist_roll_follower_so101_v1_material"/>
                 <!-- Frame gripper -->
                 <site group="3" name="gripper" pos="-0.0079 -0.000218121 -0.0981274" quat="0.5 -0.5 0.5 -0.5"/>
                 <!-- Link moving_jaw_so101_v1 -->
-                <body name="moving_jaw_so101_v1" pos="0.0202 0.0188 -0.0234" quat="0.707107 0.707107 1.41308e-15 -1.5309e-15">
+                <body name="moving_jaw_so101_v1" pos="0.0202 0.0188 -0.0234" quat="0.707107 0.707107 1.47485e-15 -6.52758e-15">
                   <!-- Joint from gripper to moving_jaw_so101_v1 -->
-                  <joint axis="0 0 1" name="6" type="hinge" range="-0.2617993877991352 1.7453292519943437" class="sts3215"/>
+                  <joint axis="0 0 1" name="6" type="hinge" range="-0.17453292519943295 1.7453292519943295" class="sts3215"/>
                   <inertial pos="-0.00157495 -0.0300244 0.0192755" mass="0.012" fullinertia="6.61427e-06 1.89032e-06 5.28738e-06 -3.19807e-07 -5.90717e-09 -1.09945e-07"/>
                   <!-- Part moving_jaw_so101_v1 -->
-                  <geom type="mesh" class="visual" pos="2.77556e-17 -1.31397e-17 0.0189" quat="1 -0 2.22045e-16 4.44203e-31" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
-                  <geom type="mesh" class="collision" pos="2.77556e-17 -1.31397e-17 0.0189" quat="1 -0 2.22045e-16 4.44203e-31" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                  <geom type="mesh" class="visual" pos="-5.55112e-17 -1.94746e-17 0.0189" quat="1 -0 1.86338e-16 6.90343e-24" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                  <geom type="mesh" class="collision" pos="-5.55112e-17 -1.94746e-17 0.0189" quat="1 -0 1.86338e-16 6.90343e-24" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
                 </body>
               </body>
             </body>
@@ -120,19 +121,19 @@
     </body>
   </worldbody>
   <asset>
-    <mesh file="waveshare_mounting_plate_so101_v2.stl"/>
-    <mesh file="wrist_roll_follower_so101_v1.stl"/>
-    <mesh file="upper_arm_so101_v1.stl"/>
+    <mesh file="rotation_pitch_so101_v1.stl"/>
+    <mesh file="moving_jaw_so101_v1.stl"/>
     <mesh file="sts3215_03a_v1.stl"/>
+    <mesh file="motor_holder_so101_wrist_v1.stl"/>
+    <mesh file="wrist_roll_follower_so101_v1.stl"/>
+    <mesh file="base_so101_v2.stl"/>
+    <mesh file="under_arm_so101_v1.stl"/>
+    <mesh file="wrist_roll_pitch_so101_v2.stl"/>
+    <mesh file="waveshare_mounting_plate_so101_v2.stl"/>
+    <mesh file="sts3215_03a_no_horn_v1.stl"/>
     <mesh file="motor_holder_so101_base_v1.stl"/>
     <mesh file="base_motor_holder_so101_v1.stl"/>
-    <mesh file="wrist_roll_pitch_so101_v2.stl"/>
-    <mesh file="moving_jaw_so101_v1.stl"/>
-    <mesh file="sts3215_03a_no_horn_v1.stl"/>
-    <mesh file="base_so101_v2.stl"/>
-    <mesh file="rotation_pitch_so101_v1.stl"/>
-    <mesh file="motor_holder_so101_wrist_v1.stl"/>
-    <mesh file="under_arm_so101_v1.stl"/>
+    <mesh file="upper_arm_so101_v1.stl"/>
     <material name="base_motor_holder_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
     <material name="base_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
     <material name="sts3215_03a_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
diff --git a/Simulation/SO101/so101_old_calib.urdf b/Simulation/SO101/so101_old_calib.urdf
new file mode 100644
index 0000000..fc41e30
--- /dev/null
+++ b/Simulation/SO101/so101_old_calib.urdf
@@ -0,0 +1,423 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/bbd5da878b987933c7940f7d -->
+<robot name="so101_old_calib">
+  <!-- Link base -->
+  <link name="base">
+    <inertial>
+      <origin xyz="0.020739 0.00204287 0.065966" rpy="0 0 0"/>
+      <mass value="0.147"/>
+      <inertia ixx="0.000136117" ixy="4.59787e-07" ixz="9.75275e-08" iyy="0.000114686" iyz="-4.97151e-06" izz="0.000130364"/>
+    </inertial>
+    <!-- Part base_motor_holder_so101_v1 -->
+    <visual>
+      <origin xyz="0.0206915 0.0221255 0.0300817" rpy="1.5708 -1.23909e-16 2.33147e-15"/>
+      <geometry>
+        <mesh filename="package://assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+      <material name="base_motor_holder_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0206915 0.0221255 0.0300817" rpy="1.5708 -1.23909e-16 2.33147e-15"/>
+      <geometry>
+        <mesh filename="package://assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part base_so101_v2 -->
+    <visual>
+      <origin xyz="0.0207909 0.0221255 0.0300817" rpy="1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/base_so101_v2.stl"/>
+      </geometry>
+      <material name="base_so101_v2_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0207909 0.0221255 0.0300817" rpy="1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/base_so101_v2.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1 -->
+    <visual>
+      <origin xyz="0.0207909 -0.0105745 0.0761817" rpy="-2.20282e-15 2.77556e-17 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_v1_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0207909 -0.0105745 0.0761817" rpy="-2.20282e-15 2.77556e-17 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part waveshare_mounting_plate_so101_v2 -->
+    <visual>
+      <origin xyz="0.0205915 0.0467435 0.0798817" rpy="1.5708 -1.21716e-14 2.33147e-15"/>
+      <geometry>
+        <mesh filename="package://assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+      <material name="waveshare_mounting_plate_so101_v2_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0205915 0.0467435 0.0798817" rpy="1.5708 -1.21716e-14 2.33147e-15"/>
+      <geometry>
+        <mesh filename="package://assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Frame base (dummy link + fixed joint) -->
+  <link name="base">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="base_frame" type="fixed">
+    <origin xyz="0.020791 0.0157608 0.0324817" rpy="-3.14159 -9.52796e-30 -2.64454e-46"/>
+    <parent link="base"/>
+    <child link="base"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Link shoulder -->
+  <link name="shoulder">
+    <inertial>
+      <origin xyz="-0.0307604 -1.66727e-05 -0.0252713" rpy="0 0 0"/>
+      <mass value="0.100006"/>
+      <inertia ixx="8.3759e-05" ixy="7.55525e-08" ixz="-1.16342e-06" iyy="8.10403e-05" iyz="1.54663e-07" izz="2.39783e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_2 -->
+    <visual>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_v1_2_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part motor_holder_so101_base_v1 -->
+    <visual>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+      <material name="motor_holder_so101_base_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part rotation_pitch_so101_v1 -->
+    <visual>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+      <material name="rotation_pitch_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="package://assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link upper_arm -->
+  <link name="upper_arm">
+    <inertial>
+      <origin xyz="-0.00838224 0.0898471 0.0184089" rpy="0 0 0"/>
+      <mass value="0.103"/>
+      <inertia ixx="0.000147318" ixy="1.97819e-05" ixz="8.97326e-09" iyy="4.08002e-05" iyz="4.03016e-08" izz="0.000142487"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_3 -->
+    <visual>
+      <origin xyz="-0.0155 0.11257 0.0187" rpy="-3.14159 -3.22659e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_v1_3_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.0155 0.11257 0.0187" rpy="-3.14159 -3.22659e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part upper_arm_so101_v1 -->
+    <visual>
+      <origin xyz="0.012 0.065085 0.0182" rpy="3.14159 -1.22125e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="upper_arm_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.012 0.065085 0.0182" rpy="3.14159 -1.22125e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link lower_arm -->
+  <link name="lower_arm">
+    <inertial>
+      <origin xyz="-0.00324376 -0.0980701 0.0182831" rpy="0 0 0"/>
+      <mass value="0.104"/>
+      <inertia ixx="0.000159844" ixy="-7.41152e-06" ixz="4.90188e-08" iyy="2.87438e-05" iyz="1.26409e-06" izz="0.00014529"/>
+    </inertial>
+    <!-- Part under_arm_so101_v1 -->
+    <visual>
+      <origin xyz="0.032 -0.0648499 0.0182" rpy="3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/under_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="under_arm_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.032 -0.0648499 0.0182" rpy="3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/under_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part motor_holder_so101_wrist_v1 -->
+    <visual>
+      <origin xyz="0.032 -0.0648499 0.018" rpy="-3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+      <material name="motor_holder_so101_wrist_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.032 -0.0648499 0.018" rpy="-3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1_4 -->
+    <visual>
+      <origin xyz="-0.0052 -0.1224 0.0187" rpy="3.14159 -8.24341e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_v1_4_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-0.0052 -0.1224 0.0187" rpy="3.14159 -8.24341e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link wrist -->
+  <link name="wrist">
+    <inertial>
+      <origin xyz="-0.000103312 -0.0386143 0.0281156" rpy="0 0 0"/>
+      <mass value="0.079"/>
+      <inertia ixx="3.68263e-05" ixy="1.7893e-08" ixz="-5.28128e-08" iyy="2.5391e-05" iyz="3.6412e-06" izz="2.1e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_no_horn_v1 -->
+    <visual>
+      <origin xyz="0 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_no_horn_v1_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_pitch_so101_v2 -->
+    <visual>
+      <origin xyz="-2.77556e-17 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+      <material name="wrist_roll_pitch_so101_v2_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="-2.77556e-17 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="package://assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Link gripper -->
+  <link name="gripper">
+    <inertial>
+      <origin xyz="0.000213627 0.000245138 -0.025187" rpy="0 0 0"/>
+      <mass value="0.087"/>
+      <inertia ixx="2.75087e-05" ixy="-3.35241e-07" ixz="-5.7352e-06" iyy="4.33657e-05" iyz="-5.17847e-08" izz="3.45059e-05"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_5 -->
+    <visual>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.55112e-17 -1.38213e-14"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215_03a_v1_5_material">
+        <color rgba="0.627451 0.627451 0.627451 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.55112e-17 -1.38213e-14"/>
+      <geometry>
+        <mesh filename="package://assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_follower_so101_v1 -->
+    <visual>
+      <origin xyz="1.11022e-16 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 -6.22813e-24"/>
+      <geometry>
+        <mesh filename="package://assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+      <material name="wrist_roll_follower_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="1.11022e-16 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 -6.22813e-24"/>
+      <geometry>
+        <mesh filename="package://assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Frame gripper (dummy link + fixed joint) -->
+  <link name="gripper">
+    <origin xyz="0 0 0" rpy="0 -0 0"/>
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="1e-9"/>
+      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+  </link>
+  <joint name="gripper_frame" type="fixed">
+    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="-1.5708 4.93038e-32 -1.5708"/>
+    <parent link="gripper"/>
+    <child link="gripper"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+  <!-- Link moving_jaw_so101_v1 -->
+  <link name="moving_jaw_so101_v1">
+    <inertial>
+      <origin xyz="-0.00157495 -0.0300244 0.0192755" rpy="0 0 0"/>
+      <mass value="0.012"/>
+      <inertia ixx="6.61427e-06" ixy="-3.19807e-07" ixz="-5.90717e-09" iyy="1.89032e-06" iyz="-1.09945e-07" izz="5.28738e-06"/>
+    </inertial>
+    <!-- Part moving_jaw_so101_v1 -->
+    <visual>
+      <origin xyz="0 1.89351e-17 0.0189" rpy="9.53145e-17 -1.20743e-23 0"/>
+      <geometry>
+        <mesh filename="package://assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+      <material name="moving_jaw_so101_v1_material">
+        <color rgba="0.964706 0.964706 0.952941 1.0"/>
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 1.89351e-17 0.0189" rpy="9.53145e-17 -1.20743e-23 0"/>
+      <geometry>
+        <mesh filename="package://assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+  <joint name="6" type="revolute">
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -7.91661e-17 -1.0396e-16"/>
+    <parent link="gripper"/>
+    <child link="moving_jaw_so101_v1"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
+  </joint>
+  <!-- Joint from wrist to gripper -->
+  <joint name="5" type="revolute">
+    <origin xyz="2.77556e-17 -0.0611 0.0181" rpy="1.5708 -9.38083e-08 3.14159"/>
+    <parent link="wrist"/>
+    <child link="gripper"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-2.79253" upper="2.79253"/>
+  </joint>
+  <!-- Joint from lower_arm to wrist -->
+  <joint name="4" type="revolute">
+    <origin xyz="-0.0052 -0.1349 3.40439e-16" rpy="-1.41553e-15 4.91611e-15 7.56157e-09"/>
+    <parent link="lower_arm"/>
+    <child link="wrist"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.65806" upper="1.65806"/>
+  </joint>
+  <!-- Joint from upper_arm to lower_arm -->
+  <joint name="3" type="revolute">
+    <origin xyz="-0.028 0.11257 1.24466e-16" rpy="1.89432e-15 -3.22659e-15 -3.14159"/>
+    <parent link="upper_arm"/>
+    <child link="lower_arm"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="3.14159"/>
+  </joint>
+  <!-- Joint from shoulder to upper_arm -->
+  <joint name="2" type="revolute">
+    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="-1.5708 1.5708 0"/>
+    <parent link="shoulder"/>
+    <child link="upper_arm"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-3.31613" upper="0.174533"/>
+  </joint>
+  <!-- Joint from base to shoulder -->
+  <joint name="1" type="revolute">
+    <origin xyz="0.0207909 -0.0230745 0.0948817" rpy="-3.14159 6.03684e-16 1.5708"/>
+    <parent link="base"/>
+    <child link="shoulder"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.91986" upper="1.91986"/>
+  </joint>
+  <!-- Additional joints_properties.xml -->
+  <default>
+    <default class="sts3215">
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.60" frictionloss="0.052" armature="0.028"/>
+      <position kp="17.8" kv="0.0" forcerange="-3.35 3.35"/>
+    </default>
+    <default class="backlash">
+      <!-- +/- 0.5° of backlash -->
+      <joint damping="0.01" frictionloss="0" armature="0.01" limited="true" range="-0.008726646259971648 0.008726646259971648"/>
+    </default>
+  </default>
+</robot>
diff --git a/Simulation/SO101/so101_old_calib.xml b/Simulation/SO101/so101_old_calib.xml
new file mode 100644
index 0000000..e11d693
--- /dev/null
+++ b/Simulation/SO101/so101_old_calib.xml
@@ -0,0 +1,160 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/bbd5da878b987933c7940f7d -->
+<mujoco model="so101_old_calib">
+  <compiler angle="radian" meshdir="assets" autolimits="true"/>
+  <default>
+    <default class="so101_old_calib">
+      <joint frictionloss="0.1" armature="0.005"/>
+      <position kp="50" dampratio="1"/>
+      <default class="visual">
+        <geom type="mesh" contype="0" conaffinity="0" group="2"/>
+      </default>
+      <default class="collision">
+        <geom group="3"/>
+      </default>
+    </default>
+  </default>
+  <!-- Additional joints_properties.xml -->
+  <default>
+    <default class="sts3215">
+      <geom contype="0" conaffinity="0"/>
+      <joint damping="0.60" frictionloss="0.052" armature="0.028"/>
+      <position kp="17.8" kv="0.0" forcerange="-3.35 3.35"/>
+    </default>
+    <default class="backlash">
+      <!-- +/- 0.5° of backlash -->
+      <joint damping="0.01" frictionloss="0" armature="0.01" limited="true" range="-0.008726646259971648 0.008726646259971648"/>
+    </default>
+  </default>
+  <worldbody>
+    <!-- Link base -->
+    <body name="base" pos="0 0 0" quat="1 0 0 0" childclass="so101_old_calib">
+      <inertial pos="0.020739 0.00204287 0.065966" mass="0.147" fullinertia="0.000136117 0.000114686 0.000130364 4.59787e-07 9.75275e-08 -4.97151e-06"/>
+      <!-- Part base_motor_holder_so101_v1 -->
+      <geom type="mesh" class="visual" pos="0.0206915 0.0221255 0.0300817" quat="0.707107 0.707107 7.85046e-16 8.68107e-16" mesh="base_motor_holder_so101_v1" material="base_motor_holder_so101_v1_material"/>
+      <!-- Part base_so101_v2 -->
+      <geom type="mesh" class="visual" pos="0.0207909 0.0221255 0.0300817" quat="0.707107 0.707107 -0 -0" mesh="base_so101_v2" material="base_so101_v2_material"/>
+      <!-- Part sts3215_03a_v1 -->
+      <geom type="mesh" class="visual" pos="0.0207909 -0.0105745 0.0761817" quat="0.707107 -7.69919e-16 8.95976e-16 -0.707107" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+      <!-- Part waveshare_mounting_plate_so101_v2 -->
+      <geom type="mesh" class="visual" pos="0.0205915 0.0467435 0.0798817" quat="0.707107 0.707107 -3.34318e-15 5.1276e-15" mesh="waveshare_mounting_plate_so101_v2" material="waveshare_mounting_plate_so101_v2_material"/>
+      <!-- Frame base -->
+      <site group="3" name="base" pos="0.020791 0.0157608 0.0324817" quat="0 1 0 0"/>
+      <!-- Link shoulder -->
+      <body name="shoulder" pos="0.0207909 -0.0230745 0.0948817" quat="7.88629e-16 -0.707107 -0.707107 7.69003e-16">
+        <!-- Joint from base to shoulder -->
+        <joint axis="0 0 1" name="1" type="hinge" range="-1.9198621771937616 1.9198621771937634" class="sts3215"/>
+        <inertial pos="-0.0307604 -1.66727e-05 -0.0252713" mass="0.100006" fullinertia="8.3759e-05 8.10403e-05 2.39783e-05 7.55525e-08 -1.16342e-06 1.54663e-07"/>
+        <!-- Part sts3215_03a_v1_2 -->
+        <geom type="mesh" class="visual" pos="-0.0303992 0.000422241 -0.0417" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+        <geom type="mesh" class="collision" pos="-0.0303992 0.000422241 -0.0417" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+        <!-- Part motor_holder_so101_base_v1 -->
+        <geom type="mesh" class="visual" pos="-0.0675992 -0.000177759 0.0158499" quat="0.5 0.5 -0.5 0.5" mesh="motor_holder_so101_base_v1" material="motor_holder_so101_base_v1_material"/>
+        <geom type="mesh" class="collision" pos="-0.0675992 -0.000177759 0.0158499" quat="0.5 0.5 -0.5 0.5" mesh="motor_holder_so101_base_v1" material="motor_holder_so101_base_v1_material"/>
+        <!-- Part rotation_pitch_so101_v1 -->
+        <geom type="mesh" class="visual" pos="0.0122008 2.22413e-05 0.0464" quat="0.707107 -0.707107 0 0" mesh="rotation_pitch_so101_v1" material="rotation_pitch_so101_v1_material"/>
+        <geom type="mesh" class="collision" pos="0.0122008 2.22413e-05 0.0464" quat="0.707107 -0.707107 0 0" mesh="rotation_pitch_so101_v1" material="rotation_pitch_so101_v1_material"/>
+        <!-- Link upper_arm -->
+        <body name="upper_arm" pos="-0.0303992 -0.0182778 -0.0542" quat="0.5 -0.5 0.5 0.5">
+          <!-- Joint from shoulder to upper_arm -->
+          <joint axis="0 0 1" name="2" type="hinge" range="-3.31612557878923 0.1745329251994294" class="sts3215"/>
+          <inertial pos="-0.00838224 0.0898471 0.0184089" mass="0.103" fullinertia="0.000147318 4.08002e-05 0.000142487 1.97819e-05 8.97326e-09 4.03016e-08"/>
+          <!-- Part sts3215_03a_v1_3 -->
+          <geom type="mesh" class="visual" pos="-0.0155 0.11257 0.0187" quat="4.91381e-16 3.1572e-16 -1 6.21545e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+          <geom type="mesh" class="collision" pos="-0.0155 0.11257 0.0187" quat="4.91381e-16 3.1572e-16 -1 6.21545e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+          <!-- Part upper_arm_so101_v1 -->
+          <geom type="mesh" class="visual" pos="0.012 0.065085 0.0182" quat="5.77339e-30 0.707107 -0.707107 -1.0267e-29" mesh="upper_arm_so101_v1" material="upper_arm_so101_v1_material"/>
+          <geom type="mesh" class="collision" pos="0.012 0.065085 0.0182" quat="5.77339e-30 0.707107 -0.707107 -1.0267e-29" mesh="upper_arm_so101_v1" material="upper_arm_so101_v1_material"/>
+          <!-- Link lower_arm -->
+          <body name="lower_arm" pos="-0.028 0.11257 1.24466e-16" quat="2.34133e-16 -3.33458e-16 -3.62568e-16 -1">
+            <!-- Joint from upper_arm to lower_arm -->
+            <joint axis="0 0 1" name="3" type="hinge" range="-0.17453292519941607 3.14159265358981" class="sts3215"/>
+            <inertial pos="-0.00324376 -0.0980701 0.0182831" mass="0.104" fullinertia="0.000159844 2.87438e-05 0.00014529 -7.41152e-06 4.90188e-08 1.26409e-06"/>
+            <!-- Part under_arm_so101_v1 -->
+            <geom type="mesh" class="visual" pos="0.032 -0.0648499 0.0182" quat="7.0875e-26 0.707107 0.707107 -7.0875e-26" mesh="under_arm_so101_v1" material="under_arm_so101_v1_material"/>
+            <geom type="mesh" class="collision" pos="0.032 -0.0648499 0.0182" quat="7.0875e-26 0.707107 0.707107 -7.0875e-26" mesh="under_arm_so101_v1" material="under_arm_so101_v1_material"/>
+            <!-- Part motor_holder_so101_wrist_v1 -->
+            <geom type="mesh" class="visual" pos="0.032 -0.0648499 0.018" quat="1.01856e-15 -0.707107 -0.707107 2.26153e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
+            <geom type="mesh" class="collision" pos="0.032 -0.0648499 0.018" quat="1.01856e-15 -0.707107 -0.707107 2.26153e-16" mesh="motor_holder_so101_wrist_v1" material="motor_holder_so101_wrist_v1_material"/>
+            <!-- Part sts3215_03a_v1_4 -->
+            <geom type="mesh" class="visual" pos="-0.0052 -0.1224 0.0187" quat="1.64704e-15 0.707107 -0.707107 1.29806e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <geom type="mesh" class="collision" pos="-0.0052 -0.1224 0.0187" quat="1.64704e-15 0.707107 -0.707107 1.29806e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <!-- Link wrist -->
+            <body name="wrist" pos="-0.0052 -0.1349 3.40439e-16" quat="1 -3.94784e-16 1.66357e-15 3.78078e-09">
+              <!-- Joint from lower_arm to wrist -->
+              <joint axis="0 0 1" name="4" type="hinge" range="-1.6580627969561947 1.6580627818330316" class="sts3215"/>
+              <inertial pos="-0.000103312 -0.0386143 0.0281156" mass="0.079" fullinertia="3.68263e-05 2.5391e-05 2.1e-05 1.7893e-08 -5.28128e-08 3.6412e-06"/>
+              <!-- Part sts3215_03a_no_horn_v1 -->
+              <geom type="mesh" class="visual" pos="0 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
+              <geom type="mesh" class="collision" pos="0 -0.0424 0.0306" quat="0.5 0.5 0.5 -0.5" mesh="sts3215_03a_no_horn_v1" material="sts3215_03a_no_horn_v1_material"/>
+              <!-- Part wrist_roll_pitch_so101_v2 -->
+              <geom type="mesh" class="visual" pos="-2.77556e-17 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
+              <geom type="mesh" class="collision" pos="-2.77556e-17 -0.028 0.0181" quat="0.5 -0.5 -0.5 -0.5" mesh="wrist_roll_pitch_so101_v2" material="wrist_roll_pitch_so101_v2_material"/>
+              <!-- Link gripper -->
+              <body name="gripper" pos="2.77556e-17 -0.0611 0.0181" quat="3.31663e-08 -3.31663e-08 -0.707107 -0.707107">
+                <!-- Joint from wrist to gripper -->
+                <joint axis="0 0 1" name="5" type="hinge" range="-2.7925268969992394 2.792526709382615" class="sts3215"/>
+                <inertial pos="0.000213627 0.000245138 -0.025187" mass="0.087" fullinertia="2.75087e-05 4.33657e-05 3.45059e-05 -3.35241e-07 -5.7352e-06 -5.17847e-08"/>
+                <!-- Part sts3215_03a_v1_5 -->
+                <geom type="mesh" class="visual" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.48145e-15 1.60424e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+                <geom type="mesh" class="collision" pos="0.0077 0.0001 -0.0234" quat="0.707107 -0.707107 6.48145e-15 1.60424e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+                <!-- Part wrist_roll_follower_so101_v1 -->
+                <geom type="mesh" class="visual" pos="1.11022e-16 -0.000218214 0.000949706" quat="0 1 0 0" mesh="wrist_roll_follower_so101_v1" material="wrist_roll_follower_so101_v1_material"/>
+                <geom type="mesh" class="collision" pos="1.11022e-16 -0.000218214 0.000949706" quat="0 1 0 0" mesh="wrist_roll_follower_so101_v1" material="wrist_roll_follower_so101_v1_material"/>
+                <!-- Frame gripper -->
+                <site group="3" name="gripper" pos="-0.0079 -0.000218121 -0.0981274" quat="0.5 -0.5 0.5 -0.5"/>
+                <!-- Link moving_jaw_so101_v1 -->
+                <body name="moving_jaw_so101_v1" pos="0.0202 0.0188 -0.0234" quat="0.707107 0.707107 0 -4.88629e-17">
+                  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+                  <joint axis="0 0 1" name="6" type="hinge" range="-0.17453292519941963 1.7453292519943429" class="sts3215"/>
+                  <inertial pos="-0.00157495 -0.0300244 0.0192755" mass="0.012" fullinertia="6.61427e-06 1.89032e-06 5.28738e-06 -3.19807e-07 -5.90717e-09 -1.09945e-07"/>
+                  <!-- Part moving_jaw_so101_v1 -->
+                  <geom type="mesh" class="visual" pos="0 1.89351e-17 0.0189" quat="1 0 1.11022e-16 8.14512e-24" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                  <geom type="mesh" class="collision" pos="0 1.89351e-17 0.0189" quat="1 0 1.11022e-16 8.14512e-24" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="motor_holder_so101_wrist_v1.stl"/>
+    <mesh file="moving_jaw_so101_v1.stl"/>
+    <mesh file="wrist_roll_follower_so101_v1.stl"/>
+    <mesh file="rotation_pitch_so101_v1.stl"/>
+    <mesh file="base_so101_v2.stl"/>
+    <mesh file="motor_holder_so101_base_v1.stl"/>
+    <mesh file="upper_arm_so101_v1.stl"/>
+    <mesh file="sts3215_03a_v1.stl"/>
+    <mesh file="under_arm_so101_v1.stl"/>
+    <mesh file="waveshare_mounting_plate_so101_v2.stl"/>
+    <mesh file="base_motor_holder_so101_v1.stl"/>
+    <mesh file="wrist_roll_pitch_so101_v2.stl"/>
+    <mesh file="sts3215_03a_no_horn_v1.stl"/>
+    <material name="base_motor_holder_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="base_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="sts3215_03a_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
+    <material name="waveshare_mounting_plate_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="motor_holder_so101_base_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="rotation_pitch_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="upper_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="under_arm_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="motor_holder_so101_wrist_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="sts3215_03a_no_horn_v1_material" rgba="0.627451 0.627451 0.627451 1"/>
+    <material name="wrist_roll_pitch_so101_v2_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="wrist_roll_follower_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+    <material name="moving_jaw_so101_v1_material" rgba="0.964706 0.964706 0.952941 1"/>
+  </asset>
+  <actuator>
+    <position class="sts3215" name="1" joint="1" inheritrange="1"/>
+    <position class="sts3215" name="2" joint="2" inheritrange="1"/>
+    <position class="sts3215" name="3" joint="3" inheritrange="1"/>
+    <position class="sts3215" name="4" joint="4" inheritrange="1"/>
+    <position class="sts3215" name="5" joint="5" inheritrange="1"/>
+    <position class="sts3215" name="6" joint="6" inheritrange="1"/>
+  </actuator>
+  <equality/>
+</mujoco>