diff --git a/dimos/hardware/manipulators/so101/__init__.py b/dimos/hardware/manipulators/so101/__init__.py
new file mode 100644
index 0000000000..e69de29bb2
diff --git a/dimos/hardware/manipulators/so101/calibration/so101_arm.json b/dimos/hardware/manipulators/so101/calibration/so101_arm.json
new file mode 100644
index 0000000000..da957f0a1e
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/calibration/so101_arm.json
@@ -0,0 +1,44 @@
+{
+  "shoulder_pan": {
+    "id": 1,
+    "drive_mode": 0,
+    "homing_offset": -2020,
+    "range_min": 744,
+    "range_max": 3362
+  },
+  "shoulder_lift": {
+    "id": 2,
+    "drive_mode": 0,
+    "homing_offset": -1618,
+    "range_min": 782,
+    "range_max": 3154
+  },
+  "elbow_flex": {
+    "id": 3,
+    "drive_mode": 0,
+    "homing_offset": 1436,
+    "range_min": 897,
+    "range_max": 3126
+  },
+  "wrist_flex": {
+    "id": 4,
+    "drive_mode": 0,
+    "homing_offset": -1499,
+    "range_min": 903,
+    "range_max": 3258
+  },
+  "wrist_roll": {
+    "id": 5,
+    "drive_mode": 0,
+    "homing_offset": -601,
+    "range_min": 160,
+    "range_max": 3981
+  },
+  "gripper": {
+    "id": 6,
+    "drive_mode": 0,
+    "homing_offset": 1365,
+    "range_min": 2037,
+    "range_max": 3508
+  }
+}
diff --git a/dimos/hardware/manipulators/so101/lerobot_kinematics.py b/dimos/hardware/manipulators/so101/lerobot_kinematics.py
new file mode 100644
index 0000000000..253e167b8b
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/lerobot_kinematics.py
@@ -0,0 +1,275 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from __future__ import annotations
+
+from pathlib import Path
+from typing import TYPE_CHECKING
+
+from lerobot.model.kinematics import RobotKinematics  # type: ignore[import-not-found]
+import numpy as np
+from scipy.spatial.transform import Rotation as R  # type: ignore[import-untyped]
+
+if TYPE_CHECKING:
+    from numpy.typing import NDArray
+
+
+class LerobotKinematics:
+    """LeRobot-based kinematics wrapper.
+
+    - Uses lerobot.model.kinematics.RobotKinematics (placo-based) internally.
+    - Provides:
+        * FK:    q -> (pos, quat_wxyz)  [q in degrees]
+        * IK:    (q_init, target_pos, target_quat_wxyz) -> q_sol  [degrees]
+        * J:     jacobian(q)  [q in radians, uses numerical differentiation]
+
+    Note: FK/IK work in degrees (matching lerobot convention), while
+    jacobian/joint_velocity work in radians for compatibility with existing code.
+    """
+
+    def __init__(
+        self,
+        urdf_path: str | Path,
+        ee_link_name: str,
+        *,
+        max_iters: int = 100,
+        tol: float = 1e-4,
+        damping: float = 1e-3,
+        joint_names: list[str] | None = None,
+    ) -> None:
+        """
+        Parameters
+        ----------
+        urdf_path:
+            Path to the URDF file.
+        ee_link_name:
+            Name of the end-effector link as defined in the URDF.
+        max_iters:
+            Maximum number of iterations for iterative IK (not used by lerobot, kept for compatibility).
+        tol:
+            Pose error tolerance (not used by lerobot, kept for compatibility).
+        damping:
+            Damping λ for damped least-squares IK / velocity control.
+        joint_names:
+            Optional list of joint names. If None, will be inferred from URDF.
+        """
+        urdf_path = Path(urdf_path)
+
+        self._lerobot_kin = RobotKinematics(
+            urdf_path=str(urdf_path),
+            target_frame_name=ee_link_name,
+            joint_names=joint_names,
+        )
+
+        self.joint_names: list[str] = self._lerobot_kin.joint_names
+        self.motor_names: list[str] = self.joint_names
+        self.dof: int = len(self.joint_names)
+
+        self._max_iters = int(max_iters)
+        self._tol = float(tol)
+        self._damping = float(damping)
+
+        self._jacobian_eps = 1e-6
+
+    # ------------------------------------------------------------------
+    # Forward Kinematics
+    # ------------------------------------------------------------------
+    def fk(self, positions: list[float]) -> tuple[NDArray[np.float64], NDArray[np.float64]]:
+        """
+        Forward kinematics: joint vector -> (position, quaternion_wxyz).
+
+        Parameters
+        ----------
+        positions:
+            Joint configuration in degrees, shape (dof,).
+
+        Returns
+        -------
+        pos:
+            End-effector position in meters, shape (3,).
+        quat_wxyz:
+            End-effector orientation quaternion (w, x, y, z), shape (4,).
+        """
+        q = np.asarray(positions, dtype=float).reshape(-1)
+        if q.shape[0] != self.dof:
+            raise ValueError(f"Expected {self.dof} DoF, got {q.shape[0]}")
+        T = self._lerobot_kin.forward_kinematics(q)
+
+        pos = T[:3, 3]
+        R_matrix = T[:3, :3]
+        rot = R.from_matrix(R_matrix)
+        quat_xyzw = rot.as_quat()
+        quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]], dtype=float)
+        return pos, quat_wxyz
+
+    # ------------------------------------------------------------------
+    # Inverse Kinematics
+    # ------------------------------------------------------------------
+    def ik(
+        self,
+        positions: list[float],
+        target_position: list[float],
+        target_quaternion_wxyz: list[float],
+        position_weight: float = 1.0,
+        orientation_weight: float = 1.0,
+    ) -> NDArray[np.float64]:
+        """
+        Inverse kinematics using Jacobian-based iterative solver.
+
+        This uses damped least-squares (DLS) iteration to find joint angles
+        that achieve the target pose.
+
+        Parameters
+        ----------
+        positions:
+            Initial joint configuration in degrees, shape (dof,).
+        target_position:
+            Target position in world frame (meters), shape (3,).
+        target_quaternion_wxyz:
+            Target orientation quaternion (w, x, y, z), shape (4,).
+        position_weight:
+            Weight for position error (default 1.0).
+        orientation_weight:
+            Weight for orientation error (default 1.0).
+
+        Returns
+        -------
+        q_sol:
+            Joint vector in degrees, shape (dof,), in the same order as `joint_names`.
+        """
+        q_init = np.asarray(positions, dtype=float).reshape(-1)
+        if q_init.shape[0] != self.dof:
+            raise ValueError(f"Expected {self.dof} DoF, got {q_init.shape[0]}")
+        target_pos = np.asarray(target_position, dtype=float).reshape(3)
+        target_quat_wxyz = np.asarray(target_quaternion_wxyz, dtype=float).reshape(4)
+
+        target_quat_xyzw = np.array(
+            [target_quat_wxyz[1], target_quat_wxyz[2], target_quat_wxyz[3], target_quat_wxyz[0]],
+            dtype=float,
+        )
+        target_rot = R.from_quat(target_quat_xyzw)
+
+        q = np.radians(q_init.copy())
+        q_deg = np.degrees(q)
+
+        # Iterative IK using Jacobian
+        step_size = 0.5  # Damping factor for stability
+
+        for _ in range(self._max_iters):
+            current_pos, current_quat_wxyz = self.fk(q_deg.tolist())
+
+            pos_error = target_pos - current_pos
+            pos_error_norm = np.linalg.norm(pos_error)
+
+            # Orientation error (axis-angle representation)
+            current_quat_xyzw = np.array(
+                [
+                    current_quat_wxyz[1],
+                    current_quat_wxyz[2],
+                    current_quat_wxyz[3],
+                    current_quat_wxyz[0],
+                ],
+                dtype=float,
+            )
+            current_rot = R.from_quat(current_quat_xyzw)
+
+            rot_error = target_rot * current_rot.inv()
+
+            # --- 5DoF orientation projection ---
+            w_world = rot_error.as_rotvec()
+            w_tool = current_rot.inv().apply(w_world)
+            w_tool[0] = 0.0  # block tool-x; keep tool-y + tool-z
+            orientation_error = current_rot.apply(w_tool)
+
+            orientation_error_norm = np.linalg.norm(orientation_error)
+
+            if pos_error_norm < self._tol and orientation_error_norm < self._tol:
+                break
+
+            twist = np.concatenate(
+                [position_weight * pos_error, orientation_weight * orientation_error]
+            )
+            J = self.jacobian(q.tolist())
+
+            JJt = J @ J.T
+            A = JJt + (self._damping**2) * np.eye(6, dtype=float)
+            dq_active = J.T @ np.linalg.solve(A, twist)
+
+            q = q + step_size * dq_active
+
+        # Safety check: verify the solution accuracy
+        q_sol_deg = np.degrees(q)
+        final_pos, _ = self.fk(q_sol_deg.tolist())
+        final_pos_error = np.linalg.norm(target_pos - final_pos)
+
+        if final_pos_error > 0.1:  # 10cm threshold
+            raise ValueError(
+                "IK solver unable to find satisfactory solution. "
+                f"Final position error: {final_pos_error * 100:.2f}cm (threshold: 10cm). "
+                f"Target position: {target_pos}, Final position: {final_pos}"
+            )
+
+        return np.array(q_sol_deg, dtype=np.float64)
+
+    # ------------------------------------------------------------------
+    # Jacobian & velocity-level control
+    # ------------------------------------------------------------------
+    def jacobian(self, positions: list[float]) -> NDArray[np.float64]:
+        """
+        Return the 6 x dof geometric Jacobian at configuration q.
+
+        Uses numerical differentiation since lerobot doesn't provide analytical jacobian.
+
+        Parameters
+        ----------
+        positions:
+            Joint configuration in radians, shape (dof,).
+
+        Returns
+        -------
+        J:
+            Geometric Jacobian matrix, shape (6, dof).
+            First 3 rows: linear velocity (vx, vy, vz)
+            Last 3 rows: angular velocity (wx, wy, wz)
+        """
+        q = np.asarray(positions, dtype=float).reshape(-1)
+        if q.shape[0] != self.dof:
+            raise ValueError(f"Expected {self.dof} DoF, got {q.shape[0]}")
+        q_deg = np.degrees(q)
+        pos0, quat0_wxyz = self.fk(q_deg.tolist())
+        quat0_xyzw = np.array([quat0_wxyz[1], quat0_wxyz[2], quat0_wxyz[3], quat0_wxyz[0]])
+        rot0 = R.from_quat(quat0_xyzw)
+        R0 = rot0.as_matrix()
+
+        J = np.zeros((6, self.dof), dtype=float)
+        eps_rad = self._jacobian_eps
+
+        for i in range(self.dof):
+            q_pert_deg = q_deg.copy()
+            q_pert_deg[i] += np.degrees(eps_rad)
+
+            pos1, quat1_wxyz = self.fk(q_pert_deg.tolist())
+            J[:3, i] = (pos1 - pos0) / eps_rad
+
+            quat1_xyzw = np.array([quat1_wxyz[1], quat1_wxyz[2], quat1_wxyz[3], quat1_wxyz[0]])
+            rot1 = R.from_quat(quat1_xyzw)
+            R1 = rot1.as_matrix()
+
+            dR = R1 @ R0.T
+            skew = (dR - dR.T) / (2.0 * eps_rad)
+            J[3, i] = skew[2, 1]
+            J[4, i] = skew[0, 2]
+            J[5, i] = skew[1, 0]
+
+        return J
diff --git a/dimos/hardware/manipulators/so101/so101_driver.py b/dimos/hardware/manipulators/so101/so101_driver.py
new file mode 100644
index 0000000000..3f525a16f9
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/so101_driver.py
@@ -0,0 +1,162 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""SO101 driver using the generalized component-based architecture."""
+
+import logging
+import time
+from typing import Any
+
+from dimos.hardware.manipulators.base import (
+    BaseManipulatorDriver,
+    StandardMotionComponent,
+    StandardServoComponent,
+    StandardStatusComponent,
+)
+
+from .so101_wrapper import SO101SDKWrapper
+
+logger = logging.getLogger(__name__)
+
+
+class SO101Driver(BaseManipulatorDriver):
+    """SO101 driver using component-based architecture.
+
+    This driver supports the SO101 LeRobot manipulator.
+    All the complex logic is handled by the base class and standard components.
+    This file just assembles the pieces.
+    """
+
+    def __init__(self, **kwargs: Any) -> None:
+        """Initialize the SO101 driver.
+
+        Args:
+            **kwargs: Arguments for Module initialization.
+                Driver configuration can be passed via 'config' keyword arg:
+                - port: USB port id (e.g., '/dev/ttyUSB0')
+                - has_gripper: Whether gripper is attached
+                - calibration_path: Path of SO101 calibration file
+                - enable_on_start: Whether to enable servos on start
+        """
+        config: dict[str, Any] = kwargs.pop("config", {})
+
+        # Extract driver-specific params that might be passed directly
+        driver_params = [
+            "port",
+            "has_gripper",
+            "calibration_path",
+            "enable_on_start",
+            "control_rate",
+            "monitor_rate",
+        ]
+        for param in driver_params:
+            if param in kwargs:
+                config[param] = kwargs.pop(param)
+
+        logger.info(f"Initializing SO101Driver with config: {config}")
+
+        # Create SDK wrapper
+        sdk = SO101SDKWrapper()
+
+        # Create standard components
+        components = [
+            StandardMotionComponent(sdk),
+            StandardServoComponent(sdk),
+            StandardStatusComponent(sdk),
+        ]
+
+        # Optional: Add gripper component if configured
+        # if config.get('has_gripper', False):
+        #     from dimos.hardware.manipulators.base.components import StandardGripperComponent
+        #     components.append(StandardGripperComponent(sdk))
+
+        # Remove any kwargs that would conflict with explicit arguments
+        kwargs.pop("sdk", None)
+        kwargs.pop("components", None)
+        kwargs.pop("name", None)
+
+        # Initialize base driver with SDK and components
+        super().__init__(
+            sdk=sdk, components=components, config=config, name="SO101Driver", **kwargs
+        )
+
+        # Initialize position target for velocity integration
+        self._position_target: list[float] | None = None
+        self._last_velocity_time: float = 0.0
+
+        # Enable on start if configured
+        if config.get("enable_on_start", False):
+            logger.info("Enabling SO101 servos on start...")
+            servo_component = self.get_component(StandardServoComponent)
+            if servo_component:
+                result = servo_component.enable_servo()
+                if result["success"]:
+                    logger.info("SO101 servos enabled successfully")
+                else:
+                    logger.warning(f"Failed to enable servos: {result.get('error')}")
+
+    def _process_command(self, command: Any) -> None:
+        """Override to implement velocity control via position integration.
+
+        Args:
+            command: Command to process
+        """
+        # Handle velocity commands specially for SO101
+        if command.type == "velocity":
+            # SO101 doesn't have native velocity control - integrate to position
+            current_time = time.time()
+
+            # Initialize position target from current state on first velocity command
+            if self._position_target is None:
+                positions = self.shared_state.joint_positions
+                if positions:
+                    self._position_target = list(positions)
+                    logger.info(
+                        f"Velocity control: Initialized position target from current state: {self._position_target}"
+                    )
+                else:
+                    logger.warning("Cannot start velocity control - no current position available")
+                    return
+
+            # Calculate dt since last velocity command
+            if self._last_velocity_time > 0:
+                dt = current_time - self._last_velocity_time
+            else:
+                dt = 1.0 / self.control_rate  # Use nominal period for first command
+
+            self._last_velocity_time = current_time
+
+            # Integrate velocity to position: pos += vel * dt
+            velocities = command.data["velocities"]
+            for i in range(min(len(velocities), len(self._position_target))):
+                self._position_target[i] += velocities[i] * dt
+
+            # Send integrated position command
+            success = self.sdk.set_joint_positions(
+                self._position_target,
+                wait=False,
+                use_ptp=False,  # type: ignore[call-arg]
+            )
+
+            if success:
+                self.shared_state.target_positions = self._position_target
+                self.shared_state.target_velocities = velocities
+        else:
+            # Reset velocity integration when switching to position mode
+            if command.type == "position":
+                self._position_target = None
+                self._last_velocity_time = 0.0
+
+            # Use base implementation for other command types
+            super()._process_command(command)
diff --git a/dimos/hardware/manipulators/so101/so101_wrapper.py b/dimos/hardware/manipulators/so101/so101_wrapper.py
new file mode 100644
index 0000000000..871fbe2966
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/so101_wrapper.py
@@ -0,0 +1,703 @@
+# Copyright 2025-2026 Dimensional Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import json
+import logging
+import threading
+import time
+from typing import Any
+
+from lerobot.motors import Motor, MotorCalibration, MotorNormMode  # type: ignore[import-not-found]
+from lerobot.motors.feetech import FeetechMotorsBus, OperatingMode  # type: ignore[import-not-found]
+import numpy as np
+from scipy.spatial.transform import Rotation as R  # type: ignore[import-untyped]
+
+from ..base.sdk_interface import BaseManipulatorSDK, ManipulatorInfo
+from .lerobot_kinematics import LerobotKinematics
+
+
+class SO101SDKWrapper(BaseManipulatorSDK):
+    def __init__(
+        self,
+        port: str = "/dev/ttyUSB0",
+        urdf_path: str = "dimos/hardware/manipulators/so101/urdf/so101_new_calib.urdf",
+        ee_link_name: str = "gripper_frame_link",
+        calibration_path: str = "dimos/hardware/manipulators/so101/calibration/so101_arm.json",
+    ):
+        self.logger = logging.getLogger(self.__class__.__name__)
+        self.dof = 5  # SO101 is always 5-DOF
+        self._connected = False
+        self._enabled = False
+        self._lock = threading.Lock()
+        self.gripper_max_open_m = 0.1
+
+        self.port = port
+        self.urdf_path = urdf_path
+        self.ee_link_name = ee_link_name
+        self.calibration_path = calibration_path
+
+        self.bus: FeetechMotorsBus | None = None
+
+        # Motor configuration: 5 DOF arm + 1 gripper
+        self.motor_names = [
+            "shoulder_pan",
+            "shoulder_lift",
+            "elbow_flex",
+            "wrist_flex",
+            "wrist_roll",
+        ]
+        self.gripper_name = "gripper"
+        # Joint angle offsets in degrees
+        # Measured angles when the arm is supposed to be mechanically at zero.
+        self.joint_offsets_deg = np.array(
+            [0, 0.26373626, 2.59340659, 0.65934066, 0.21978022], dtype=float
+        )
+
+        self.motor_ids: dict[str, int] = {
+            "shoulder_pan": 1,
+            "shoulder_lift": 2,
+            "elbow_flex": 3,
+            "wrist_flex": 4,
+            "wrist_roll": 5,
+            "gripper": 6,
+        }
+
+        self.min_speed = 0.2
+        self.max_speed = 1.0
+
+        # Initialize kinematics
+        self.kinematics: LerobotKinematics | None = None
+        try:
+            self.kinematics = LerobotKinematics(
+                self.urdf_path,
+                self.ee_link_name,
+                joint_names=self.motor_names,
+            )
+            self.logger.info(
+                "Initialized LerobotKinematics with URDF %s, EE link %s",
+                self.urdf_path,
+                self.ee_link_name,
+            )
+        except Exception as e:
+            self.logger.warning(f"Failed to initialize kinematics: {e}")
+
+    # ============= Connection Management =============
+
+    def _load_calibration(self, calibration_path: str = "") -> dict[str, MotorCalibration]:
+        """Load motor calibration from JSON file."""
+        try:
+            with open(calibration_path) as f:
+                calib_data = json.load(f)
+            calibration: dict[str, MotorCalibration] = {}
+            for name, data in calib_data.items():
+                calibration[name] = MotorCalibration(**data)
+            return calibration
+        except Exception as e:
+            self.logger.warning(f"Failed to load calibration from {calibration_path}: {e}")
+            return {}
+
+    def configure_motors(self) -> None:
+        """
+        Configure motors: operating mode + PID.
+
+        - Puts all motors (joints + gripper) into POSITION mode.
+        - Sets PID coefficients tuned to reduce shakiness.
+        """
+        if not self.bus:
+            raise RuntimeError("Bus not connected.")
+
+        self.logger.info("Configuring motors (OperatingMode + PID gains)")
+
+        # Disable torque while tweaking settings
+        with self._lock:
+            with self.bus.torque_disabled():
+                # Let LeRobot configure registers (limits, accel, etc.)
+                self.bus.configure_motors()
+
+                for motor in self.bus.motors:
+                    # Position control for all motors
+                    self.bus.write("Operating_Mode", motor, OperatingMode.POSITION.value)
+
+                    # PID gains – tuned for smoother motion
+                    self.bus.write("P_Coefficient", motor, 16)
+                    self.bus.write("I_Coefficient", motor, 0)
+                    self.bus.write("D_Coefficient", motor, 32)
+
+        self.logger.info("Motor configuration complete")
+
+    def connect(self, config: dict[str, Any]) -> bool:
+        """Connect to SO101 via bus.
+
+        Args:
+            config: Configuration with 'port' (e.g., 'dev/ttyUSB0') and config path
+
+        Returns:
+            True if connection successful
+        """
+        try:
+            port = config.get("port", "/dev/ttyUSB0")
+            calibration_path = config.get(
+                "calibration_path", "dimos/hardware/manipulators/so101/calibration/so101_arm.json"
+            )
+            self.control_rate = config.get("control_rate", 50)
+            self.control_dt = 1.0 / self.control_rate
+            self.logger.info("Connecting to SO-101 arm on port %s", port)
+
+            norm_mode_body = MotorNormMode.DEGREES
+            motors: dict[str, Motor] = {}
+
+            # Arm joints: normalized in degrees
+            for name in self.motor_names:
+                motors[name] = Motor(self.motor_ids[name], "sts3215", norm_mode_body)
+
+            # Gripper: normalized in [0, 100]
+            motors[self.gripper_name] = Motor(
+                self.motor_ids[self.gripper_name],
+                "sts3215",
+                MotorNormMode.RANGE_0_100,
+            )
+
+            calibration = self._load_calibration(calibration_path)
+
+            self.bus = FeetechMotorsBus(
+                port=port,
+                motors=motors,
+                calibration=calibration,
+            )
+            self.bus.connect()
+            self.logger.info("FeetechMotorsBus connected")
+
+            # Configure modes, PID, etc.
+            self.configure_motors()
+            self._connected = True
+            return True
+        except Exception as e:
+            self.logger.error(f"Failed to connect to SO-101 arm: {e}")
+            return False
+
+    def disconnect(self) -> None:
+        """Disconnect from the arm."""
+        if self.bus:
+            with self._lock:
+                self.bus.disconnect()
+            self.logger.info("SO-101 bus disconnected")
+            self._connected = False
+
+    def is_connected(self) -> bool:
+        return self._connected
+
+    # ============= Joint State Query =============
+
+    def get_joint_positions(self, degree: bool = False) -> list[float]:
+        """
+        Get current joint angles with joint offsets applied.
+
+        Returns:
+            Joint positions in RADIANS or DEGREES (default: RADIANS)
+        """
+        if not self.bus:
+            return [0.0] * len(self.motor_names)
+
+        with self._lock:
+            values = self.bus.sync_read("Present_Position")
+        q_deg_raw = np.array([values[name] for name in self.motor_names], dtype=float)
+        q_deg = q_deg_raw - self.joint_offsets_deg
+        return q_deg.tolist() if degree else np.radians(q_deg).tolist()  # type: ignore[no-any-return]
+
+    def get_joint_velocities(self) -> list[float]:
+        """Get current joint velocities.
+
+        Returns:
+            Joint velocities in RAD/S (0-indexed)
+        """
+        try:
+            if not self.bus:
+                return [0.0] * len(self.motor_names)
+
+            with self._lock:
+                values = self.bus.sync_read("Present_Velocity")
+            vel_deg = np.array([values[name] for name in self.motor_names], dtype=float)
+            return np.radians(vel_deg).tolist() # type: ignore[no-any-return]
+        except Exception as e:
+            self.logger.debug(f"Error when reading Present_Velocity: {e}")
+            return [0.0] * len(self.motor_names)
+
+    def get_joint_efforts(self) -> list[float]:
+        """Get current joint efforts/torques.
+
+        Returns:
+            Joint efforts in Nm (0-indexed)
+        """
+        # Not possible with Feetech motors so returning zeros
+        return [0.0] * len(self.motor_names)
+
+    # ============= Joint Motion Control =============
+
+    def move_joint_ptp(
+        self, positions: list[float], velocity: float = 1.0, duration: float | None = None
+    ) -> None:
+        """
+        Joint-space PTP interpolation.
+
+        Args:
+            q_target: target joint angles [rad], shape (5,)
+            velocity: Max velocity fraction (0-1)
+            duration: total motion time (s). If None, derived from velocity.
+        """
+        if not self.bus:
+            raise RuntimeError("Bus not connected.")
+
+        q_target = np.asarray(positions, dtype=float)
+        if q_target.shape[0] != len(self.motor_names):
+            raise ValueError(f"Expected {len(self.motor_names)} joints, got {q_target.shape[0]}")
+
+        q_start = np.array(self.get_joint_positions(degree=False))
+        dq = q_target - q_start
+        max_delta = float(np.max(np.abs(dq)))
+
+        if max_delta < 1e-6:
+            q_deg = np.degrees(q_target)
+            cmd = {name: float(q_deg[i]) for i, name in enumerate(self.motor_names)}
+            with self._lock:
+                self.bus.sync_write("Goal_Position", cmd)
+            return
+
+        if duration is None:
+            # Derive from velocity: 0..1 → 0.2..1.0 rad/s (tunable)
+            joint_speed = self.min_speed + (self.max_speed - self.min_speed) * velocity
+            if joint_speed < 1e-3:
+                joint_speed = self.min_speed
+
+            duration = max_delta / joint_speed
+            if duration < self.control_dt:
+                duration = self.control_dt
+
+        steps = max(int(duration / self.control_dt), 1)
+
+        for i in range(1, steps + 1):
+            alpha = i / steps
+            q_interp = q_start + alpha * dq
+            q_deg = np.degrees(q_interp)
+            cmd = {name: float(q_deg[j]) for j, name in enumerate(self.motor_names)}
+            with self._lock:
+                self.bus.sync_write("Goal_Position", cmd)
+            time.sleep(self.control_dt)
+
+    def set_joint_positions(
+        self,
+        positions: list[float],
+        velocity: float = 1.0,
+        _acceleration: float = 1.0,
+        wait: bool = False,
+        use_ptp: bool = True,
+    ) -> bool:
+        """Move joints to target positions.
+
+        Args:
+            positions: Target positions in RADIANS (0-indexed)
+            velocity: Max velocity fraction (0-1)
+            _acceleration: [UNUSED] Kept for interface compatibility
+            wait: If True, block until motion completes
+            use_ptp: If True, use PTP interpolation.
+                     If False, send command directly (assumes trajectory generated externally).
+
+        Returns:
+            True if command accepted
+        """
+
+        if not self.bus:
+            return False
+
+        if use_ptp:
+            self.move_joint_ptp(positions, velocity=velocity)
+            result = True
+        else:
+            q_target = np.asarray(positions, dtype=float)
+            q_deg = np.degrees(q_target)
+            cmd = {name: float(q_deg[i]) for i, name in enumerate(self.motor_names)}
+            with self._lock:
+                self.bus.sync_write("Goal_Position", cmd)
+            result = True
+
+        if wait and result:
+            start_time = time.time()
+            timeout = 30.0  # 30 second timeout
+
+            while time.time() - start_time < timeout:
+                try:
+                    # Check if reached target (within tolerance)
+                    current = self.get_joint_positions()
+                    tolerance = 0.05  # radians
+                    if all(
+                        abs(current[i] - positions[i]) < tolerance
+                        for i in range(len(self.motor_names))
+                    ):
+                        break
+                except Exception:
+                    pass  # Continue waiting
+                time.sleep(0.01)
+
+        return result
+
+    def set_joint_velocities(self, velocities: list[float]) -> bool:
+        """Set joint velocity targets.
+
+        Note: Lerobot doesn't have native velocity control. The driver should
+        implement velocity control via position integration if needed.
+
+        Args:
+            velocities: Target velocities in RAD/S (0-indexed)
+
+        Returns:
+            False - velocity control not supported at SDK level
+        """
+        # Lerobot doesn't have native velocity control
+        # The driver layer should implement this via position integration
+        self.logger.debug("Velocity control not supported at SDK level - use position integration")
+        return False
+
+    def set_joint_efforts(self, efforts: list[float]) -> bool:
+        """Set joint effort/torque targets.
+
+        Note: Lerobot doesn't have native torque control. The driver should
+        implement torque control via position integration if needed.
+        Args:
+            efforts: Target efforts in Nm (0-indexed)
+
+        Returns:
+            False - torque control not supported at SDK level
+        """
+        self.logger.debug("Torque control not supported at SDK level - use position integration")
+        return False
+
+    def stop_motion(self) -> bool:
+        """Stop all ongoing motion.
+
+        Returns:
+            True if stop successful
+        """
+        # SO101 emergency stop
+        try:
+            self.emergency_stop()
+        except Exception:
+            curr_pos = self.get_joint_positions()
+            self.set_joint_positions(curr_pos)
+        return True
+
+    # ============= Servo Control =============
+    def enable_servos(self) -> bool:
+        """Enable motor control.
+
+        Returns:
+            True if servos enabled
+        """
+        if not self.bus:
+            return False
+        with self._lock:
+            self.bus.enable_torque()
+        self._enabled = True
+        return True
+
+
+    def disable_servos(self) -> bool:
+        """Disable motor control.
+
+        Returns:
+            True if servos disabled
+        """
+        if not self.bus:
+            return False
+        with self._lock:
+            self.bus.disable_torque()
+        self._enabled = False
+        return True
+
+    def are_servos_enabled(self) -> bool:
+        """Check if servos are enabled.
+
+        Returns:
+            True if enabled
+        """
+        return self._enabled
+
+    # ============= System State =============
+    def get_robot_state(self) -> dict[str, Any]:
+        """Get current robot state.
+
+        Returns:
+            State dictionary
+        """
+        if not self.bus:
+            return {
+                "state": 2,  # Error if can't get status
+                "mode": 0,
+                "error_code": 999,
+                "warn_code": 0,
+                "is_moving": False,
+                "cmd_num": 0,
+            }
+
+        # Default state mapping
+        state = 0  # idle
+        mode = 0  # position mode
+        error_code = 0
+
+        error_code = self.get_error_code()
+        state = 2 if error_code != 0 else 0
+
+        return {
+            "state": state,
+            "mode": mode,
+            "error_code": error_code,
+            "warn_code": 0,
+            "is_moving": False,
+            "cmd_num": 0,
+        }
+
+    def get_error_code(self) -> int:
+        """Get current error code.
+
+        Returns:
+            Error code (0 = no error)
+        """
+        if not self.bus:
+            return 0
+
+        try:
+            with self._lock:
+                err_codes = {k: int(v) for k, v in self.bus.sync_read("Status").items()}
+        except Exception:
+            self.logger.exception("sync_read(Status) failed")
+            return 0
+        agg = 0
+        for _, code in err_codes.items():
+            agg |= int(code)
+        return agg
+
+    def _decode_hw_error_bits(self, err_code: int) -> list[str]:
+        error_map = {
+            0: "Voltage error",
+            1: "Sensor/encoder error",
+            2: "Overtemperature",
+            3: "Overcurrent",
+            4: "Angle/position error",
+            5: "Overload",
+        }
+
+        msgs = []
+        for bit, label in error_map.items():
+            if err_code & (1 << bit):
+                msgs.append(label)
+        return msgs
+
+    def get_error_message(self) -> str:
+        """Get human-readable error message.
+
+        Returns:
+            Error message string
+        """
+        error_code = self.get_error_code()
+        if error_code == 0:
+            return ""
+
+        msgs = self._decode_hw_error_bits(error_code)
+        if msgs:
+            return ", ".join(msgs)
+        return f"Unknown error code: {error_code}"
+
+    def clear_errors(self) -> bool:
+        """Clear error states.
+
+        Returns:
+            True if errors cleared
+        """
+        self.disable_servos()
+        time.sleep(0.1)
+        return self.enable_servos()
+
+    def emergency_stop(self) -> bool:
+        """Execute emergency stop.
+
+        Returns:
+            True if e-stop executed
+        """
+        return self.disable_servos()
+
+    # ============= Information =============
+    def get_info(self) -> ManipulatorInfo:
+        """Get manipulator information.
+
+        Returns:
+            ManipulatorInfo object
+        """
+        return ManipulatorInfo(
+            vendor="LeRobot",
+            model="SO101",
+            dof=self.dof,
+            firmware_version=None,  # Lerobot does not expose firmware version
+            serial_number=None,  # Lerobot does not expose serial number
+        )
+
+    def get_joint_limits(self) -> tuple[list[float], list[float]]:
+        """Get joint position limits.
+
+        Returns:
+            Tuple of (lower_limits, upper_limits) in RADIANS
+        """
+        lower_limits = [-1.919, -1.74, -1.69, -1.65, -2.74]
+        upper_limits = [1.919, 1.74, 1.69, 1.65, 2.84]
+
+        return (lower_limits, upper_limits)
+
+    def get_velocity_limits(self) -> list[float]:
+        """Get joint velocity limits.
+
+        Returns:
+            Maximum velocities in RAD/S
+        """
+        # SO101 max velocities (approximate)
+        max_vel = 2.0  # rad/s
+        return [max_vel] * self.dof
+
+    def get_acceleration_limits(self) -> list[float]:
+        """Get joint acceleration limits.
+
+        Returns:
+            Maximum accelerations in RAD/S²
+        """
+        # SO101 max accelerations (approximate)
+        max_acc = 8.0  # rad/s²
+        return [max_acc] * self.dof
+
+    # ============= Optional Methods =============
+    def set_gripper_position(self, position: float) -> bool:
+        """
+        Move gripper to target position.
+
+        Args:
+            position: 0.0 (closed) to 0.1 (open) in meters.
+        """
+        if not self.bus:
+            self.logger.warning("Robot not connected")
+            return False
+
+        # Map 0–0.1 m → 0–100 normalized range
+        val = (position / 0.1) * 100.0
+        val = max(0.0, min(100.0, val))
+        with self._lock:
+            self.bus.write("Goal_Position", self.gripper_name, val)
+        return True
+
+    def get_gripper_position(self) -> float | None:
+        """
+        Get gripper position.
+
+        Returns:
+            Position in meters ( 0.0 [closed] to 0.1 [open]) or None
+        """
+        if not self.bus:
+            return None
+
+        with self._lock:
+            raw = float(self.bus.read("Present_Position", self.gripper_name))
+            pos_m = (raw / 100.0) * self.gripper_max_open_m
+            return max(0.0, min(self.gripper_max_open_m, pos_m))
+
+    def get_cartesian_position(self) -> dict[str, float] | None:
+        """Get current end-effector pose.
+
+        Returns:
+            Pose dict or None if not supported
+        """
+        if self.kinematics:
+            q = self.get_joint_positions(degree=True)
+            pos, quat_wxyz = self.kinematics.fk(q)
+
+            # Convert quaternion (w, x, y, z) to (x, y, z, w) for scipy
+            quat_xyzw = [quat_wxyz[1], quat_wxyz[2], quat_wxyz[3], quat_wxyz[0]]
+            rot = R.from_quat(quat_xyzw)
+            roll, pitch, yaw = rot.as_euler("xyz", degrees=False)
+
+            return {
+                "x": float(pos[0]),
+                "y": float(pos[1]),
+                "z": float(pos[2]),
+                "roll": float(roll),
+                "pitch": float(pitch),
+                "yaw": float(yaw),
+            }
+        return None
+
+    def set_cartesian_position(
+        self,
+        pose: dict[str, float],
+        velocity: float = 1.0,
+        acceleration: float = 1.0,
+        wait: bool = False,
+    ) -> bool:
+        """Move end-effector to target pose.
+
+        Args:
+            pose: Target pose dict
+            velocity: Max velocity fraction (0-1)
+            acceleration: Max acceleration fraction (0-1)
+            wait: Block until complete
+
+        Returns:
+            True if command accepted
+        """
+        if not self.kinematics:
+            self.logger.warning("Cartesian control not available")
+            return False
+
+        curr_joint_angles = self.get_joint_positions(degree=True)
+        target_pos = [pose["x"], pose["y"], pose["z"]]
+
+        # Convert RPY to Quaternion (wxyz)
+        r = R.from_euler("xyz", [pose["roll"], pose["pitch"], pose["yaw"]], degrees=False)
+        quat_xyzw = r.as_quat()
+        target_quat_wxyz = [quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]
+        try:
+            q_target_kin_deg = self.kinematics.ik(curr_joint_angles, target_pos, target_quat_wxyz)
+            q_target = np.radians(q_target_kin_deg)
+            self.set_joint_positions(q_target.tolist())
+        except ValueError as e:
+            self.logger.error(f"Value Error Raised: {e}")
+            return False
+
+        # Wait if requested
+        if wait:
+            start_time = time.time()
+            timeout = 30.0
+
+            while time.time() - start_time < timeout:
+                current = self.get_cartesian_position()
+                if current:
+                    # Check if reached target (within tolerance)
+                    tol_pos = 0.01  # 1cm
+                    tol_rot = 0.1  # ~3 degrees
+
+                    if (
+                        abs(current["x"] - pose["x"]) < tol_pos
+                        and abs(current["y"] - pose["y"]) < tol_pos
+                        and abs(current["z"] - pose["z"]) < tol_pos
+                        and abs(current["roll"] - pose["roll"]) < tol_rot
+                        and abs(current["pitch"] - pose["pitch"]) < tol_rot
+                        and abs(current["yaw"] - pose["yaw"]) < tol_rot
+                    ):
+                        break
+
+                time.sleep(0.01)
+
+        return True
diff --git a/dimos/hardware/manipulators/so101/urdf/README.md b/dimos/hardware/manipulators/so101/urdf/README.md
new file mode 100644
index 0000000000..c50dc327be
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/README.md
@@ -0,0 +1,35 @@
+# 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.
+- The generated URDFs were modified to allow meshes with relative paths instead of `package://...`.
+- 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/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.part
new file mode 100644
index 0000000000..6d3ac50049
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "84d8ae1881704ebae1ffb70a",
+    "documentMicroversion": "0eea3500852bdb2f58b1cb79",
+    "documentVersion": "a5c3b0dfaa52ddd6829011cd",
+    "elementId": "22efbe4e0bef24fcd20f96e5",
+    "fullConfiguration": "default",
+    "id": "MCOhripg0ry51VlsC",
+    "isStandardContent": false,
+    "name": "Base_motor_holder_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.stl
new file mode 100644
index 0000000000..cac2b8dd98
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/base_motor_holder_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.part b/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.part
new file mode 100644
index 0000000000..f41563fa8b
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "bf61a6bc85b1d1a8bf9ea51b",
+    "documentMicroversion": "20484d37162a32a8a41a37f2",
+    "documentVersion": "25801b070e5b360715de8a30",
+    "elementId": "312f32f0073fa6e8e36fba7a",
+    "fullConfiguration": "default",
+    "id": "MY69cJlqvSzIiODdH",
+    "isStandardContent": false,
+    "name": "Base_SO101 v2 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.stl b/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.stl
new file mode 100644
index 0000000000..ed10aa61d1
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/base_so101_v2.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.part
new file mode 100644
index 0000000000..bf899dd0c0
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "652d5731024e57367badfda6",
+    "documentMicroversion": "56a8b8013480c176fd87df8d",
+    "documentVersion": "984ac31c92cac3664c8effb3",
+    "elementId": "6fb7b7f9315511b548d670ff",
+    "fullConfiguration": "default",
+    "id": "Mf4ZebMr4BkShucFj",
+    "isStandardContent": false,
+    "name": "Motor_holder_SO101_Base v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.stl
new file mode 100644
index 0000000000..9eaf67243a
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_base_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.part
new file mode 100644
index 0000000000..dd57728f58
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "4bd66da73cacb4d946d43e44",
+    "documentMicroversion": "2bf56247e58b70e90806e318",
+    "documentVersion": "df78bb7089f1de7d5588d238",
+    "elementId": "d7dfe76e402c21bbd8124e43",
+    "fullConfiguration": "default",
+    "id": "MN9BZ1p69dQQtKTjq",
+    "isStandardContent": false,
+    "name": "Motor_holder_SO101_Wrist v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.stl
new file mode 100644
index 0000000000..d0aa18b15a
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/motor_holder_so101_wrist_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.part
new file mode 100644
index 0000000000..959a7eb0c6
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "46218c02ef80d36172edbb35",
+    "documentMicroversion": "68b7d387e2500c451586ae59",
+    "documentVersion": "79c101d1a0207b77362b561a",
+    "elementId": "d4b1411d5d7333298f6e2458",
+    "fullConfiguration": "default",
+    "id": "MrHPLr9hZkrXwcSA4",
+    "isStandardContent": false,
+    "name": "Moving_Jaw_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.stl
new file mode 100644
index 0000000000..7e87366865
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/moving_jaw_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.part
new file mode 100644
index 0000000000..6870d9d55b
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "14078aa6723c502d07d6902e",
+    "documentMicroversion": "c0fca717407275159bcc6ed7",
+    "documentVersion": "3d9a887ff68fa477d98162b8",
+    "elementId": "43d24b3857ff686b275578bf",
+    "fullConfiguration": "default",
+    "id": "MrQ6Kmk9QDZlwbp95",
+    "isStandardContent": false,
+    "name": "Rotation_Pitch_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.stl
new file mode 100644
index 0000000000..5c05dee570
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/rotation_pitch_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.part
new file mode 100644
index 0000000000..0b420437d2
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "56e5f3702dad85e17841d2e2",
+    "documentMicroversion": "7958a6acbc8e0d0a0a611746",
+    "documentVersion": "29a4c51b8bf277a22743a333",
+    "elementId": "8c14fb13a6557ec89ff5d227",
+    "fullConfiguration": "default",
+    "id": "MOcaIFg8XgL+Ybg9z",
+    "isStandardContent": false,
+    "name": "STS3215_03a_no_horn v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.stl
new file mode 100644
index 0000000000..2a89597702
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_no_horn_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.part
new file mode 100644
index 0000000000..e57e2dd6c5
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "d2941bdba816affebdc6d6f0",
+    "documentMicroversion": "5904ef3cea04a0d0bc88b698",
+    "documentVersion": "dd4f7470101215836a4ae8c9",
+    "elementId": "e670b72d49b06f88fad5dbd8",
+    "fullConfiguration": "default",
+    "id": "M5vQNpe0onRFueych",
+    "isStandardContent": false,
+    "name": "STS3215_03a v1 <5>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.stl
new file mode 100644
index 0000000000..fe05403dbf
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/sts3215_03a_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.part
new file mode 100644
index 0000000000..bd97af90b7
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "9f5d6db47eb112442b9f130f",
+    "documentMicroversion": "e99cf45162e34789bd99512b",
+    "documentVersion": "817ebf29c5663d412edc0753",
+    "elementId": "2813aaffe3c8a342616d3527",
+    "fullConfiguration": "default",
+    "id": "M9yAEiX02J3c4HqXa",
+    "isStandardContent": false,
+    "name": "Under_arm_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.stl
new file mode 100644
index 0000000000..480e0583c8
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/under_arm_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.part
new file mode 100644
index 0000000000..0ef64e9a29
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "57f3eae43434311c28ac752b",
+    "documentMicroversion": "33eeab136e831427f0f0ca74",
+    "documentVersion": "435d47b71ef26075bf82672c",
+    "elementId": "a8e0c02dc43f7ccb373c52e4",
+    "fullConfiguration": "default",
+    "id": "Ml3rwO4kV53jDRgcs",
+    "isStandardContent": false,
+    "name": "Upper_arm_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.stl
new file mode 100644
index 0000000000..9c0f9b275a
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/upper_arm_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.part b/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.part
new file mode 100644
index 0000000000..3189de5738
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "066f8b5064455ec46759cd8c",
+    "documentMicroversion": "04c5790374bf3edfbbb7e818",
+    "documentVersion": "408440a116f7d8700bbb11c2",
+    "elementId": "dc35e56269e36de39738b34d",
+    "fullConfiguration": "default",
+    "id": "MjhXxhyF1+iAgCtUh",
+    "isStandardContent": false,
+    "name": "WaveShare_Mounting_Plate_SO101 v2 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.stl b/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.stl
new file mode 100644
index 0000000000..4dba1f22b0
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/waveshare_mounting_plate_so101_v2.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.part b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.part
new file mode 100644
index 0000000000..77c7ef5eee
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "e02f1e1d3fdd766a19a55890",
+    "documentMicroversion": "03f1dfc090db6bbecdb14475",
+    "documentVersion": "8a15327cfbde0344e0951076",
+    "elementId": "2317bd70c68862eeebd64492",
+    "fullConfiguration": "default",
+    "id": "MpI0voU28BOAZ6D9x",
+    "isStandardContent": false,
+    "name": "Wrist_Roll_Follower_SO101 v1 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.stl b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.stl
new file mode 100644
index 0000000000..123ce374ad
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_follower_so101_v1.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.part b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.part
new file mode 100644
index 0000000000..388f5efa44
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.part
@@ -0,0 +1,14 @@
+{
+    "configuration": "default",
+    "documentId": "eb144d215e733b8dbbb50b81",
+    "documentMicroversion": "4fef760722dee3a9b5ff19b1",
+    "documentVersion": "5880c1e9413206cac10772d0",
+    "elementId": "3c22c2c23cb0ce545b9df2ba",
+    "fullConfiguration": "default",
+    "id": "Ma99J59HxnSe2TArb",
+    "isStandardContent": false,
+    "name": "Wrist_Roll_Pitch_SO101 v2 <1>",
+    "partId": "JFD",
+    "suppressed": false,
+    "type": "Part"
+}
diff --git a/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.stl b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.stl
new file mode 100644
index 0000000000..ac5172a35e
Binary files /dev/null and b/dimos/hardware/manipulators/so101/urdf/assets/wrist_roll_pitch_so101_v2.stl differ
diff --git a/dimos/hardware/manipulators/so101/urdf/joints_properties.xml b/dimos/hardware/manipulators/so101/urdf/joints_properties.xml
new file mode 100644
index 0000000000..684ffd4576
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/joints_properties.xml
@@ -0,0 +1,12 @@
+<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>
diff --git a/dimos/hardware/manipulators/so101/urdf/robot.urdf b/dimos/hardware/manipulators/so101/urdf/robot.urdf
new file mode 100644
index 0000000000..120b5eef24
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/robot.urdf
@@ -0,0 +1,450 @@
+<robot name="so101_new_calib">
+
+  <!-- Materials -->
+  <material name="3d_printed">
+    <color rgba="1.0 0.82 0.12 1.0"/>
+  </material>
+  <material name="sts3215">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+
+  <!-- Link base -->
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0137179 -5.19711e-05 0.0334843" rpy="0 0 0"/>
+      <mass value="0.147"/>
+      <inertia ixx="0.000114686" ixy="-4.59787e-07" ixz="4.97151e-06" iyy="0.000136117" iyz="9.75275e-08" izz="0.000130364"/>
+    </inertial>
+    <!-- Part base_motor_holder_so101_v1 -->
+    <visual>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part base_so101_v2 -->
+    <visual>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1 -->
+    <visual>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part waveshare_mounting_plate_so101_v2 -->
+    <visual>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link shoulder -->
+  <link name="shoulder_link">
+    <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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="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 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link upper_arm -->
+  <link name="upper_arm_link">
+    <inertial>
+      <origin xyz="-0.0898471 -0.00838224 0.0184089" rpy="0 0 0"/>
+      <mass value="0.103"/>
+      <inertia ixx="4.08002e-05" ixy="-1.97819e-05" ixz="-4.03016e-08" iyy="0.000147318" iyz="8.97326e-09" izz="0.000142487"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_3 -->
+    <visual>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="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 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -0 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link lower_arm -->
+  <link name="lower_arm_link">
+    <inertial>
+      <origin xyz="-0.0980701 0.00324376 0.0182831" rpy="0 0 0"/>
+      <mass value="0.104"/>
+      <inertia ixx="2.87438e-05" ixy="7.41152e-06" ixz="1.26409e-06" iyy="0.000159844" iyz="-4.90188e-08" izz="0.00014529"/>
+    </inertial>
+    <!-- Part under_arm_so101_v1 -->
+    <visual>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="assets/under_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="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 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="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 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link wrist -->
+  <link name="wrist_link">
+    <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="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_pitch_so101_v2 -->
+    <visual>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link gripper -->
+  <link name="gripper_link">
+    <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.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_follower_so101_v1 -->
+    <visual>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Gripper frame (dummy link + fixed joint) -->
+  <link name="gripper_frame_link">
+    <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_joint" type="fixed">
+    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="0 3.14159 0"/>
+    <parent link="gripper_link"/>
+    <child link="gripper_frame_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <!-- Link moving_jaw_so101_v1 -->
+  <link name="moving_jaw_so101_v1_link">
+    <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="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+  <joint name="gripper" type="revolute">
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -5.24284e-08 -1.41553e-15"/>
+    <parent link="gripper_link"/>
+    <child link="moving_jaw_so101_v1_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="gripper_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="gripper">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from wrist to gripper -->
+  <joint name="wrist_roll" type="revolute">
+    <origin xyz="5.55112e-17 -0.0611 0.0181" rpy="1.5708 0.0486795 3.14159"/>
+    <parent link="wrist_link"/>
+    <child link="gripper_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-2.74385" upper="2.84121"/>
+  </joint>
+
+  <transmission name="wrist_roll_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_roll">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from lower_arm to wrist -->
+  <joint name="wrist_flex" type="revolute">
+    <origin xyz="-0.1349 0.0052 3.62355e-17" rpy="4.02456e-15 8.67362e-16 -1.5708"/>
+    <parent link="lower_arm_link"/>
+    <child link="wrist_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.65806" upper="1.65806"/>
+  </joint>
+
+  <transmission name="wrist_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from upper_arm to lower_arm -->
+  <!-- Note: 5-degree calibration offset applied to joint limits -->
+  <joint name="elbow_flex" type="revolute">
+    <origin xyz="-0.11257 -0.028 1.73763e-16" rpy="-3.63608e-16 8.74301e-16 1.5708"/>
+    <parent link="upper_arm_link"/>
+    <child link="lower_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.69" upper="1.69"/>
+  </joint>
+
+  <transmission name="elbow_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from shoulder to upper_arm -->
+  <joint name="shoulder_lift" type="revolute">
+    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="-1.5708 -1.5708 0"/>
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.74533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from base to shoulder -->
+  <joint name="shoulder_pan" type="revolute">
+    <origin xyz="0.0388353 -8.97657e-09 0.0624" rpy="3.14159 4.18253e-17 -3.14159"/>
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.91986" upper="1.91986"/>
+  </joint>
+
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+</robot>
diff --git a/dimos/hardware/manipulators/so101/urdf/scene.xml b/dimos/hardware/manipulators/so101/urdf/scene.xml
new file mode 100644
index 0000000000..cc166be2c4
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/scene.xml
@@ -0,0 +1,26 @@
+<mujoco model="scene">
+    <include file="so101_new_calib.xml" />
+
+    <option timestep="0.0005" iterations="100" solver="Newton" tolerance="1e-8"/>
+
+    <visual>
+        <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+        <rgba haze="0.15 0.25 0.35 1" />
+        <global azimuth="160" elevation="-20" />
+    </visual>
+
+    <asset>
+        <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
+            height="3072" />
+        <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4"
+            rgb2="0.1 0.2 0.3"
+            markrgb="0.8 0.8 0.8" width="300" height="300" />
+        <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"
+            reflectance="0.2" />
+    </asset>
+
+    <worldbody>
+        <light pos="0 0 3.5" dir="0 0 -1" directional="true" />
+        <geom name="floor" size="0 0 0.05" pos="0 0 0" type="plane" material="groundplane" />
+    </worldbody>
+</mujoco>
diff --git a/dimos/hardware/manipulators/so101/urdf/so101_new_calib.urdf b/dimos/hardware/manipulators/so101/urdf/so101_new_calib.urdf
new file mode 100644
index 0000000000..120b5eef24
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/so101_new_calib.urdf
@@ -0,0 +1,450 @@
+<robot name="so101_new_calib">
+
+  <!-- Materials -->
+  <material name="3d_printed">
+    <color rgba="1.0 0.82 0.12 1.0"/>
+  </material>
+  <material name="sts3215">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+
+  <!-- Link base -->
+  <link name="base_link">
+    <inertial>
+      <origin xyz="0.0137179 -5.19711e-05 0.0334843" rpy="0 0 0"/>
+      <mass value="0.147"/>
+      <inertia ixx="0.000114686" ixy="-4.59787e-07" ixz="4.97151e-06" iyy="0.000136117" iyz="9.75275e-08" izz="0.000130364"/>
+    </inertial>
+    <!-- Part base_motor_holder_so101_v1 -->
+    <visual>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -9.94414e-05 -0.0024" rpy="1.5708 -1.67685e-15 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part base_so101_v2 -->
+    <visual>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.00636471 -8.97657e-09 -0.0024" rpy="1.5708 -2.78073e-29 1.5708"/>
+      <geometry>
+        <mesh filename="assets/base_so101_v2.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part sts3215_03a_v1 -->
+    <visual>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0263353 -8.97657e-09 0.0437" rpy="-8.21148e-16 7.84513e-18 1.249e-15"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part waveshare_mounting_plate_so101_v2 -->
+    <visual>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0309827 -0.000199441 0.0474" rpy="1.5708 -1.35493e-14 1.5708"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link shoulder -->
+  <link name="shoulder_link">
+    <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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="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 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 2.35221e-33 0"/>
+      <geometry>
+        <mesh filename="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link upper_arm -->
+  <link name="upper_arm_link">
+    <inertial>
+      <origin xyz="-0.0898471 -0.00838224 0.0184089" rpy="0 0 0"/>
+      <mass value="0.103"/>
+      <inertia ixx="4.08002e-05" ixy="-1.97819e-05" ixz="-4.03016e-08" iyy="0.000147318" iyz="8.97326e-09" izz="0.000142487"/>
+    </inertial>
+    <!-- Part sts3215_03a_v1_3 -->
+    <visual>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.11257 -0.0155 0.0187" rpy="-3.14159 -5.27356e-16 -1.5708"/>
+      <geometry>
+        <mesh filename="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 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.065085 0.012 0.0182" rpy="3.14159 -0 -1.30911e-30"/>
+      <geometry>
+        <mesh filename="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link lower_arm -->
+  <link name="lower_arm_link">
+    <inertial>
+      <origin xyz="-0.0980701 0.00324376 0.0182831" rpy="0 0 0"/>
+      <mass value="0.104"/>
+      <inertia ixx="2.87438e-05" ixy="7.41152e-06" ixz="1.26409e-06" iyy="0.000159844" iyz="-4.90188e-08" izz="0.00014529"/>
+    </inertial>
+    <!-- Part under_arm_so101_v1 -->
+    <visual>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="assets/under_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.0182" rpy="3.14159 -0 6.67202e-31"/>
+      <geometry>
+        <mesh filename="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 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0648499 -0.032 0.018" rpy="-3.14159 -2.55351e-15 -1.83387e-30"/>
+      <geometry>
+        <mesh filename="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 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.1224 0.0052 0.0187" rpy="-3.14159 -7.88861e-31 -3.14159"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link wrist -->
+  <link name="wrist_link">
+    <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="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_pitch_so101_v2 -->
+    <visual>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link gripper -->
+  <link name="gripper_link">
+    <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.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.19179e-17 -1.66533e-16"/>
+      <geometry>
+        <mesh filename="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+    </collision>
+    <!-- Part wrist_roll_follower_so101_v1 -->
+    <visual>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="8.32667e-17 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 0"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Gripper frame (dummy link + fixed joint) -->
+  <link name="gripper_frame_link">
+    <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_joint" type="fixed">
+    <origin xyz="-0.0079 -0.000218121 -0.0981274" rpy="0 3.14159 0"/>
+    <parent link="gripper_link"/>
+    <child link="gripper_frame_link"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <!-- Link moving_jaw_so101_v1 -->
+  <link name="moving_jaw_so101_v1_link">
+    <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="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-5.55112e-17 -5.55112e-17 0.0189" rpy="9.53145e-17 6.93889e-18 1.24077e-24"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+  <joint name="gripper" type="revolute">
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -5.24284e-08 -1.41553e-15"/>
+    <parent link="gripper_link"/>
+    <child link="moving_jaw_so101_v1_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="gripper_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="gripper">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from wrist to gripper -->
+  <joint name="wrist_roll" type="revolute">
+    <origin xyz="5.55112e-17 -0.0611 0.0181" rpy="1.5708 0.0486795 3.14159"/>
+    <parent link="wrist_link"/>
+    <child link="gripper_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-2.74385" upper="2.84121"/>
+  </joint>
+
+  <transmission name="wrist_roll_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_roll">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from lower_arm to wrist -->
+  <joint name="wrist_flex" type="revolute">
+    <origin xyz="-0.1349 0.0052 3.62355e-17" rpy="4.02456e-15 8.67362e-16 -1.5708"/>
+    <parent link="lower_arm_link"/>
+    <child link="wrist_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.65806" upper="1.65806"/>
+  </joint>
+
+  <transmission name="wrist_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from upper_arm to lower_arm -->
+  <!-- Note: 5-degree calibration offset applied to joint limits -->
+  <joint name="elbow_flex" type="revolute">
+    <origin xyz="-0.11257 -0.028 1.73763e-16" rpy="-3.63608e-16 8.74301e-16 1.5708"/>
+    <parent link="upper_arm_link"/>
+    <child link="lower_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.69" upper="1.69"/>
+  </joint>
+
+  <transmission name="elbow_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from shoulder to upper_arm -->
+  <joint name="shoulder_lift" type="revolute">
+    <origin xyz="-0.0303992 -0.0182778 -0.0542" rpy="-1.5708 -1.5708 0"/>
+    <parent link="shoulder_link"/>
+    <child link="upper_arm_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.74533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from base to shoulder -->
+  <joint name="shoulder_pan" type="revolute">
+    <origin xyz="0.0388353 -8.97657e-09 0.0624" rpy="3.14159 4.18253e-17 -3.14159"/>
+    <parent link="base_link"/>
+    <child link="shoulder_link"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-1.91986" upper="1.91986"/>
+  </joint>
+
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+</robot>
diff --git a/dimos/hardware/manipulators/so101/urdf/so101_new_calib.xml b/dimos/hardware/manipulators/so101/urdf/so101_new_calib.xml
new file mode 100644
index 0000000000..e24ff77732
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/so101_new_calib.xml
@@ -0,0 +1,162 @@
+<?xml version="1.0" ?>
+<!-- Generated using onshape-to-robot -->
+<!-- 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_new_calib">
+      <joint damping="1" frictionloss="0.1" armature="0.005"/>
+      <position kp="50"/>
+      <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"/>
+      <!-- For lerobot this are not exactly the motor params as the Kp and Kd not map 1-to-1 thus motor idendification with lerobot Kp=16 and Kd=32 should actually be done -->
+      <position kp="17.8"/>
+    </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_new_calib">
+      <inertial pos="0.0137179 -5.19711e-05 0.0334843" mass="0.147" fullinertia="0.000114686 0.000136117 0.000130364 -4.59787e-07 4.97151e-06 9.75275e-08"/>
+      <!-- Part base_motor_holder_so101_v1 -->
+      <geom type="mesh" class="visual" pos="-0.00636471 -9.94414e-05 -0.0024" quat="0.5 0.5 0.5 0.5" mesh="base_motor_holder_so101_v1" material="base_motor_holder_so101_v1_material"/>
+      <!-- Part base_so101_v2 -->
+      <geom type="mesh" class="visual" pos="-0.00636471 -8.97657e-09 -0.0024" quat="0.5 0.5 0.5 0.5" mesh="base_so101_v2" material="base_so101_v2_material"/>
+      <!-- Part sts3215_03a_v1 -->
+      <geom type="mesh" class="visual" pos="0.0263353 -8.97657e-09 0.0437" quat="1 -2.85511e-16 -9.64433e-17 6.12908e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+      <!-- Part waveshare_mounting_plate_so101_v2 -->
+      <geom type="mesh" class="visual" pos="-0.0309827 -0.000199441 0.0474" quat="0.5 0.5 0.5 0.5" mesh="waveshare_mounting_plate_so101_v2" material="waveshare_mounting_plate_so101_v2_material"/>
+      <!-- Frame baseframe -->
+      <site group="3" name="baseframe" pos="8.67362e-19 9.55596e-18 3.46945e-18" quat="1 -8.17396e-19 3.78392e-17 2.22045e-16"/>
+      <!-- Link shoulder -->
+      <body name="shoulder" pos="0.0388353 -8.97657e-09 0.0624" quat="3.56167e-16 1.22818e-15 -1 -4.14635e-16">
+        <!-- Joint from base to shoulder -->
+        <joint axis="0 0 1" name="shoulder_pan" 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 8.3163e-34" 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 8.3163e-34" 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="shoulder_lift" type="hinge" range="-1.7453292519943224 1.7453292519943366" 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="4.56308e-16 -0.707107 0.707107 -1.37383e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+          <geom type="mesh" class="collision" pos="-0.11257 -0.0155 0.0187" quat="4.56308e-16 -0.707107 0.707107 -1.37383e-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 1.73763e-16" quat="0.707107 -5.98613e-17 -2.58051e-17 0.707107">
+            <!-- Joint from upper_arm to lower_arm -->
+            <!-- Note: 5-degree calibration offset applied to joint range -->
+            <joint axis="0 0 1" name="elbow_flex" type="hinge" range="-1.69 1.69" 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="3.92687e-16 -1 -1.9186e-15 -6.38378e-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.92687e-16 -1 -1.9186e-15 -6.38378e-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="7.21645e-16 1.56949e-15 1 -3.33067e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <geom type="mesh" class="collision" pos="-0.1224 0.0052 0.0187" quat="7.21645e-16 1.56949e-15 1 -3.33067e-16" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+            <!-- Link wrist -->
+            <body name="wrist" pos="-0.1349 0.0052 3.62355e-17" quat="0.707107 9.58722e-16 -7.51313e-16 -0.707107">
+              <!-- Joint from lower_arm to wrist -->
+              <joint axis="0 0 1" name="wrist_flex" type="hinge" range="-1.6580628494556928 1.6580627293335335" 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="8.32667e-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="8.32667e-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="5.55112e-17 -0.0611 0.0181" quat="0.0172091 -0.0172091 0.706897 0.706897">
+                <!-- Joint from wrist to gripper -->
+                <joint axis="0 0 1" name="wrist_roll" type="hinge" range="-2.7438472969992493 2.841206309382605" 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 1.66015e-15 6.45094e-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 1.66015e-15 6.45094e-15" mesh="sts3215_03a_v1" material="sts3215_03a_v1_material"/>
+                <!-- Part wrist_roll_follower_so101_v1 -->
+                <geom type="mesh" class="visual" pos="8.32667e-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="8.32667e-17 -0.000218214 0.000949706" quat="0 1 0 0" mesh="wrist_roll_follower_so101_v1" material="wrist_roll_follower_so101_v1_material"/>
+                <!-- Frame gripperframe -->
+                <site group="3" name="gripperframe" pos="-0.0079 -0.000218121 -0.0981274" quat="0.707107 -0 0.707107 -2.37788e-17"/>
+                <!-- Link moving_jaw_so101_v1 -->
+                <body name="moving_jaw_so101_v1" pos="0.0202 0.0188 -0.0234" quat="0.707107 0.707107 -1.85362e-08 1.85362e-08">
+                  <!-- Joint from gripper to moving_jaw_so101_v1 -->
+                  <joint axis="0 0 1" name="gripper" type="hinge" range="-0.17453297762778586 1.7453291995659765" 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="-5.55112e-17 -5.55112e-17 0.0189" quat="1 -0 3.00524e-16 -2.00834e-17" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                  <geom type="mesh" class="collision" pos="-5.55112e-17 -5.55112e-17 0.0189" quat="1 -0 3.00524e-16 -2.00834e-17" mesh="moving_jaw_so101_v1" material="moving_jaw_so101_v1_material"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+  <asset>
+    <mesh file="waveshare_mounting_plate_so101_v2.stl"/>
+    <mesh file="sts3215_03a_v1.stl"/>
+    <mesh file="motor_holder_so101_base_v1.stl"/>
+    <mesh file="wrist_roll_follower_so101_v1.stl"/>
+    <mesh file="moving_jaw_so101_v1.stl"/>
+    <mesh file="base_motor_holder_so101_v1.stl"/>
+    <mesh file="upper_arm_so101_v1.stl"/>
+    <mesh file="wrist_roll_pitch_so101_v2.stl"/>
+    <mesh file="under_arm_so101_v1.stl"/>
+    <mesh file="rotation_pitch_so101_v1.stl"/>
+    <mesh file="motor_holder_so101_wrist_v1.stl"/>
+    <mesh file="sts3215_03a_no_horn_v1.stl"/>
+    <mesh file="base_so101_v2.stl"/>
+    <material name="base_motor_holder_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="base_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="sts3215_03a_v1_material" rgba="0.1 0.1 0.1 1"/>
+    <material name="waveshare_mounting_plate_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="motor_holder_so101_base_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="rotation_pitch_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="upper_arm_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="under_arm_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="motor_holder_so101_wrist_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="sts3215_03a_no_horn_v1_material" rgba="0.1 0.1 0.1 1.0"/>
+    <material name="wrist_roll_pitch_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="wrist_roll_follower_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="moving_jaw_so101_v1_material" rgba="1 0.82 0.12 1"/>
+  </asset>
+  <actuator>
+    <position class="sts3215" name="shoulder_pan" joint="shoulder_pan" forcerange="-3.35 3.35" ctrlrange="-1.91986 1.91986"/>
+    <position class="sts3215" name="shoulder_lift" joint="shoulder_lift" forcerange="-3.35 3.35" ctrlrange="-1.74533 1.74533"/>
+    <position class="sts3215" name="elbow_flex" joint="elbow_flex" forcerange="-3.35 3.35" ctrlrange="-1.69 1.69"/>
+    <position class="sts3215" name="wrist_flex" joint="wrist_flex" forcerange="-3.35 3.35" ctrlrange="-1.65806 1.65806"/>
+    <position class="sts3215" name="wrist_roll" joint="wrist_roll" forcerange="-3.35 3.35" ctrlrange="-2.74385 2.84121"/>
+    <position class="sts3215" name="gripper" joint="gripper" forcerange="-3.35 3.35" ctrlrange="-0.17453 1.74533"/>
+  </actuator>
+  <equality/>
+</mujoco>
diff --git a/dimos/hardware/manipulators/so101/urdf/so101_old_calib.urdf b/dimos/hardware/manipulators/so101/urdf/so101_old_calib.urdf
new file mode 100644
index 0000000000..c75bb38ea2
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/so101_old_calib.urdf
@@ -0,0 +1,435 @@
+<?xml version='1.0' encoding='utf-8'?>
+<!-- Generated using onshape-to-robot -->
+<!-- Onshape https://cad.onshape.com/documents/7715cc284bb430fe6dab4ffd/w/4fd0791b683777b02f8d975a/e/826c553ede3b7592eb9ca800 -->
+<robot name="so101_old_calib">
+
+  <!-- Materials -->
+  <material name="3d_printed">
+    <color rgba="1.0 0.82 0.12 1.0"/>
+  </material>
+  <material name="sts3215">
+    <color rgba="0.1 0.1 0.1 1.0"/>
+  </material>
+
+  <!-- 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="assets/base_motor_holder_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0206915 0.0221255 0.0300817" rpy="1.5708 -1.23909e-16 2.33147e-15"/>
+      <geometry>
+        <mesh filename="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="assets/base_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0207909 0.0221255 0.0300817" rpy="1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0207909 -0.0105745 0.0761817" rpy="-2.20282e-15 2.77556e-17 -1.5708"/>
+      <geometry>
+        <mesh filename="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="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0205915 0.0467435 0.0798817" rpy="1.5708 -1.21716e-14 2.33147e-15"/>
+      <geometry>
+        <mesh filename="assets/waveshare_mounting_plate_so101_v2.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- 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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0303992 0.000422241 -0.0417" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/motor_holder_so101_base_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0675992 -0.000177759 0.0158499" rpy="1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/rotation_pitch_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0122008 2.22413e-05 0.0464" rpy="-1.5708 -0 0"/>
+      <geometry>
+        <mesh filename="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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0155 0.11257 0.0187" rpy="-3.14159 -3.22659e-15 -3.14159"/>
+      <geometry>
+        <mesh filename="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="assets/upper_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.012 0.065085 0.0182" rpy="3.14159 -1.22125e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="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="assets/under_arm_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.032 -0.0648499 0.0182" rpy="3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="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="assets/motor_holder_so101_wrist_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0.032 -0.0648499 0.018" rpy="-3.14159 -1.11022e-16 1.5708"/>
+      <geometry>
+        <mesh filename="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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="-0.0052 -0.1224 0.0187" rpy="3.14159 -8.24341e-15 -1.5708"/>
+      <geometry>
+        <mesh filename="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="assets/sts3215_03a_no_horn_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.0424 0.0306" rpy="1.5708 1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/wrist_roll_pitch_so101_v2.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="-2.77556e-17 -0.028 0.0181" rpy="-1.5708 -1.5708 0"/>
+      <geometry>
+        <mesh filename="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="assets/sts3215_03a_v1.stl"/>
+      </geometry>
+      <material name="sts3215"/>
+    </visual>
+    <collision>
+      <origin xyz="0.0077 0.0001 -0.0234" rpy="-1.5708 -5.55112e-17 -1.38213e-14"/>
+      <geometry>
+        <mesh filename="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="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="1.11022e-16 -0.000218214 0.000949706" rpy="-3.14159 -5.55112e-17 -6.22813e-24"/>
+      <geometry>
+        <mesh filename="assets/wrist_roll_follower_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Link jaw -->
+  <link name="jaw">
+    <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="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+      <material name="3d_printed"/>
+    </visual>
+    <collision>
+      <origin xyz="0 1.89351e-17 0.0189" rpy="9.53145e-17 -1.20743e-23 0"/>
+      <geometry>
+        <mesh filename="assets/moving_jaw_so101_v1.stl"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <!-- Joint from gripper to jaw -->
+  <joint name="gripper" type="revolute">
+    <origin xyz="0.0202 0.0188 -0.0234" rpy="1.5708 -7.91661e-17 -1.0396e-16"/>
+    <parent link="gripper"/>
+    <child link="jaw"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="10" velocity="10" lower="-0.174533" upper="1.74533"/>
+  </joint>
+
+  <transmission name="gripper_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="gripper">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor6">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from wrist to gripper -->
+  <joint name="wrist_roll" 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>
+
+  <transmission name="wrist_roll_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_roll">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor5">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from lower_arm to wrist -->
+  <joint name="wrist_flex" 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>
+
+  <transmission name="wrist_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="wrist_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor4">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from upper_arm to lower_arm -->
+  <joint name="elbow_flex" 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>
+
+  <transmission name="elbow_flex_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="elbow_flex">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor3">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from shoulder to upper_arm -->
+  <joint name="shoulder_lift" 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>
+
+  <transmission name="shoulder_lift_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_lift">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor2">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+  <!-- Joint from base to shoulder -->
+  <joint name="shoulder_pan" 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>
+
+  <transmission name="shoulder_pan_trans">
+    <type>transmission_interface/SimpleTransmission</type>
+    <joint name="shoulder_pan">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+    </joint>
+    <actuator name="motor1">
+      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
+      <mechanicalReduction>1</mechanicalReduction>
+    </actuator>
+  </transmission>
+
+</robot>
diff --git a/dimos/hardware/manipulators/so101/urdf/so101_old_calib.xml b/dimos/hardware/manipulators/so101/urdf/so101_old_calib.xml
new file mode 100644
index 0000000000..7bc79e982c
--- /dev/null
+++ b/dimos/hardware/manipulators/so101/urdf/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 damping="1" frictionloss="0.1" armature="0.005"/>
+      <position kp="50"/>
+      <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"/>
+    </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="shoulder_pan" 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="shoulder_lift" 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="elbow_flex" 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="wrist_flex" 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="wrist_roll" 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="gripper" 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="1 0.82 0.12 1"/>
+    <material name="base_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="sts3215_03a_v1_material" rgba="0.1 0.1 0.1 1"/>
+    <material name="waveshare_mounting_plate_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="motor_holder_so101_base_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="rotation_pitch_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="upper_arm_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="under_arm_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="motor_holder_so101_wrist_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="sts3215_03a_no_horn_v1_material" rgba="0.1 0.1 0.1 1"/>
+    <material name="wrist_roll_pitch_so101_v2_material" rgba="1 0.82 0.12 1"/>
+    <material name="wrist_roll_follower_so101_v1_material" rgba="1 0.82 0.12 1"/>
+    <material name="moving_jaw_so101_v1_material" rgba="1 0.82 0.12 1"/>
+  </asset>
+  <actuator>
+    <position class="sts3215" name="shoulder_pan" joint="shoulder_pan" forcerange="-3.35 3.35" ctrlrange="-1.91986 1.91986"/>
+    <position class="sts3215" name="shoulder_lift" joint="shoulder_lift" forcerange="-3.35 3.35" ctrlrange="-3.31612 0.17453"/>
+    <position class="sts3215" name="elbow_flex" joint="elbow_flex" forcerange="-3.35 3.35" ctrlrange="-0.17453 3.14159"/>
+    <position class="sts3215" name="wrist_flex" joint="wrist_flex" forcerange="-3.35 3.35" ctrlrange="-1.65806 1.65806"/>
+    <position class="sts3215" name="wrist_roll" joint="wrist_roll" forcerange="-3.35 3.35" ctrlrange="-2.79252 2.79252"/>
+    <position class="sts3215" name="gripper" joint="gripper" forcerange="-3.35 3.35" ctrlrange="-0.17453 1.74533"/>
+  </actuator>
+  <equality/>
+</mujoco>
diff --git a/pyproject.toml b/pyproject.toml
index 3faa358b4b..5132d3b2bd 100644
--- a/pyproject.toml
+++ b/pyproject.toml
@@ -185,6 +185,11 @@ manipulation = [
     # piper arm
     "piper-sdk",
 
+    # lerobot so101 arm
+    "lerobot",
+    "lerobot[feetech]",
+    "lerobot[kinematics]",
+
     # Visualization (Optional)
     "kaleido>=0.2.1",
     "plotly>=5.9.0",