Small fixes

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

pyproject.toml

@@ -116,7 +116,7 @@ packages = [
 [tool.ruff]
 line-length = 110
 target-version = "py310"
-exclude = ["tests/artifacts/**/*.safetensors", "*_pb2.py", "*_pb2_grpc.py"]
+exclude = ["tests/artifacts/**/*.safetensors", "*_pb2.py", "*_pb2_grpc.py", "*.part", "*.stl"]
 
 [tool.ruff.lint]
 select = ["E4", "E7", "E9", "F", "I", "N", "B", "C4", "SIM"]
@@ -148,6 +148,12 @@ default.extend-ignore-identifiers-re = [
     "ein",
 ]
 
+[tool.typos.files]
+extend-exclude = [
+  "*.stl",
+  "*.part",
+]
+
 [build-system]
 requires = ["poetry-core"]
 build-backend = "poetry.core.masonry.api"

src/lerobot/bi_teleoperate.py

@@ -0,0 +1,175 @@
+import math
+import sys
+import time
+
+from lerobot.robots.so101_follower_torque.config_so101_follower_t import SO101FollowerTConfig
+from lerobot.robots.so101_follower_torque.so101_follower_t import SO101FollowerT
+from lerobot.utils.robot_utils import busy_wait
+from lerobot.utils.visualization_utils import _init_rerun, log_rerun_data
+
+FRQ = 100
+PRINT_HZ = 10
+RERUN_HZ = 100
+ESC_CLR_EOL = "\x1b[K"
+CURSOR_UP = "\x1b[F"
+
+follower_cfg = SO101FollowerTConfig(
+    port="/dev/tty.usbmodem58760432961",
+    id="follower_arm_torque",
+)
+
+leader_cfg = SO101FollowerTConfig(
+    port="/dev/tty.usbmodem58760432571",
+    id="leader_arm_torque",
+)
+
+follower = SO101FollowerT(follower_cfg)
+leader = SO101FollowerT(leader_cfg)
+follower.connect()
+leader.connect()
+
+_init_rerun("bilateral_teleoperation")
+
+print("Starting 4-channel bilateral teleoperation")
+first_print = True
+loop_count = 0
+tic_prev = time.perf_counter()
+
+while True:
+    tic = time.perf_counter()
+
+    obs_l, obs_f = leader.get_observation(), follower.get_observation()
+
+    dt = tic - tic_prev
+    tic_prev = tic
+    if dt <= 0.0:
+        dt = 0.01  # avoid div-by-zero
+
+    tau_cmd_f, tau_cmd_l = [], []
+    debug_info_f, debug_info_l = {}, {}
+
+    pos_f = {j: obs_f[f"{j}.pos"] for j in follower.bus.motors}
+    vel_f = {j: obs_f[f"{j}.vel"] for j in follower.bus.motors}
+    tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in follower.bus.motors}
+
+    pos_l = {j: obs_l[f"{j}.pos"] for j in leader.bus.motors}
+    vel_l = {j: obs_l[f"{j}.vel"] for j in leader.bus.motors}
+    tau_reaction_l = {j: obs_l[f"{j}.effort"] for j in leader.bus.motors}
+
+    # Joint-specific control gains
+    kp_gains = follower.kp_gains
+    kd_gains = follower.kd_gains
+    kf_gains = follower.kf_gains
+
+    # Compute torque commands
+    tau_cmd_f = [
+        kp_gains[j] * (pos_l[j] - pos_f[j])  # Position tracking
+        + kd_gains[j] * (vel_l[j] - vel_f[j])  # Velocity damping
+        + kf_gains[j] * (-tau_reaction_l[j] - tau_reaction_f[j])  # Force reflection
+        for j in follower.bus.motors
+    ]
+
+    tau_cmd_l = [
+        kp_gains[j] * (pos_f[j] - pos_l[j])  # Position tracking
+        + kd_gains[j] * (vel_f[j] - vel_l[j])  # Velocity damping
+        + kf_gains[j] * (-tau_reaction_f[j] - tau_reaction_l[j])  # Force reflection
+        for j in leader.bus.motors
+    ]
+
+    # Store debug info
+    for i, j in enumerate(follower.bus.motors):
+        debug_info_f[j] = {
+            "τ_reaction": tau_reaction_f[j],
+            "τ_ref": tau_cmd_f[i],
+            "θ_err": pos_l[j] - pos_f[j],
+            "ω_err": vel_l[j] - vel_f[j],
+            "τ_err": -tau_reaction_l[j] - tau_reaction_f[j],
+        }
+        debug_info_l[j] = {
+            "τ_reaction": tau_reaction_l[j],
+            "τ_ref": tau_cmd_l[i],
+            "θ_err": pos_f[j] - pos_l[j],
+            "ω_err": vel_f[j] - vel_l[j],
+            "τ_err": -tau_reaction_f[j] - tau_reaction_l[j],
+        }
+
+    # Send torques to both arms
+    follower.send_action({f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(follower.bus.motors)})
+    leader.send_action({f"{m}.effort": tau_cmd_l[i] for i, m in enumerate(leader.bus.motors)})
+
+    observation = {
+        "follower_joint_angles": pos_f,  # θ_f: current angles
+        "follower_angular_velocities": vel_f,  # ω_f: current velocities
+        "follower_external_torques": tau_reaction_f,  # τ_ext: measured minus deterministic components
+    }
+
+    action = {
+        "leader_target_angles": pos_l,  # θ_leader[τ]: absolute target angles
+        "leader_target_velocities": vel_l,  # ω_leader[τ]: absolute target velocities
+        "leader_interaction_torques": tau_reaction_l,  # τ_leader[τ]: cmd minus deterministic components
+    }
+
+    if loop_count % (FRQ // RERUN_HZ) == 0:
+        log_rerun_data(observation, action)
+
+    loop_count += 1
+    if loop_count % (FRQ // PRINT_HZ) == 0:
+        hz = 1.0 / dt
+
+        lines = [f"Loop {hz:6.1f} Hz    Δt {dt * 1e3:5.2f} ms"]
+        lines.append("=" * 106)
+        lines.append("LEADER ARM TORQUE ANALYSIS:")
+        lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Cmd':>6}")
+        lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}")
+        lines.append("-" * 86)
+
+        for i, j in enumerate(leader.bus.motors):
+            debug_l = debug_info_l[j]
+
+            lines.append(
+                f"{j:<13s}{math.degrees(pos_l[j]):+8.1f}{debug_l['τ_reaction']:+6.2f}{tau_cmd_l[i]:+6.2f}"
+            )
+
+        lines.append("")
+        lines.append("FOLLOWER ARM TORQUE ANALYSIS:")
+        lines.append(f"{'Joint':<13}{'Pos':>8}{'React':>6}{'Cmd':>6}")
+        lines.append(f"{'':13}{'(deg)':>8}{'(Nm)':>6}{'(Nm)':>6}")
+        lines.append("-" * 86)
+
+        for i, j in enumerate(follower.bus.motors):
+            debug_f = debug_info_f[j]
+
+            lines.append(
+                f"{j:<13s}{math.degrees(pos_f[j]):+8.1f}{debug_f['τ_reaction']:+6.2f}{tau_cmd_f[i]:+6.2f}"
+            )
+
+        lines.append("")
+        lines.append("=" * 86)
+        lines.append("TORQUE COMPONENT EXPLANATIONS:")
+        lines.append("• Pos   (joint pos)  = Joint position in degrees")
+        lines.append("• React (reaction)  = External forces (human interaction, contact)")
+        lines.append("• Meas  (measured)  = Raw torque from motor current sensor")
+        lines.append("• Cmd   (command)   = Final torque sent to motor")
+        lines.append("-" * 86)
+        lines.append(
+            "Cmd = Track + Vel + Force + (Added as feedforward in send_action: Grav + Inert + Frict)"
+        )
+        lines.append("React = Meas - Grav - Inert - Frict  (external forces)")
+        lines.append("Force = Kf × (reflect_other_robot - React)  (telepresence)")
+        lines.append("Frict = b_visc×ω + f_coulomb×sign(ω)  (transparency)")
+        lines.append(
+            f"Joint Gains: shoulder_pan Kp={kp_gains['shoulder_pan']:.1f} | shoulder_pan Kd={kd_gains['shoulder_pan']:.1f} | shoulder_pan Kf={kf_gains['shoulder_pan']:.1f}"
+        )
+        lines.append(
+            f"Friction Comp, Viscous: {follower.friction_viscous['shoulder_pan']:.3f} | Coulomb: {follower.friction_coulomb['shoulder_pan']:.3f} (robot-class)"
+        )
+
+        block = "\n".join(lines)
+        if first_print:
+            sys.stdout.write(block + "\n")
+            first_print = False
+        else:
+            sys.stdout.write(CURSOR_UP * len(lines) + ESC_CLR_EOL + block + "\n")
+        sys.stdout.flush()
+
+    busy_wait(max(0.0, 1.0 / FRQ - (time.perf_counter() - tic)))

src/lerobot/cameras/opencv/camera_opencv.py

@@ -112,7 +112,8 @@ class OpenCVCamera(Camera):
         self.config = config
         self.index_or_path = config.index_or_path
 
-        self.fps = config.fps
+        self.wanted_fps = config.fps
+        self.camera_fps = None
         self.color_mode = config.color_mode
         self.warmup_s = config.warmup_s
 
@@ -200,10 +201,9 @@ class OpenCVCamera(Camera):
         if not self.is_connected:
             raise DeviceNotConnectedError(f"Cannot configure settings for {self} as it is not connected.")
 
-        if self.fps is None:
-            self.fps = self.videocapture.get(cv2.CAP_PROP_FPS)
-        else:
-            self._validate_fps()
+        # We don't set the FPS. We GET the actual (max) FPS from the camera.
+        self.camera_fps = self.videocapture.get(cv2.CAP_PROP_FPS)
+        logger.info(f"{self} is running at its default/max FPS: {self.camera_fps:.2f}")
 
         default_width = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_WIDTH)))
         default_height = int(round(self.videocapture.get(cv2.CAP_PROP_FRAME_HEIGHT)))
@@ -316,19 +316,23 @@ class OpenCVCamera(Camera):
         if not self.is_connected:
             raise DeviceNotConnectedError(f"{self} is not connected.")
 
-        start_time = time.perf_counter()
+        # Start the background capture thread if it's not running
+        if self.thread is None or not self.thread.is_alive():
+            # Perform an initial blocking read to populate the first frame
+            ret, frame = self.videocapture.read()
+            if not ret or frame is None:
+                raise RuntimeError(f"{self} failed to read initial frame.")
 
-        ret, frame = self.videocapture.read()
+            self.latest_frame = self._postprocess_image(frame)
+            self._start_read_thread()
 
-        if not ret or frame is None:
-            raise RuntimeError(f"{self} read failed (status={ret}).")
+        with self.frame_lock:
+            frame = self.latest_frame
 
-        processed_frame = self._postprocess_image(frame, color_mode)
+        if frame is None:
+            raise RuntimeError(f"Internal error: Read thread started but no frame is available for {self}.")
 
-        read_duration_ms = (time.perf_counter() - start_time) * 1e3
-        logger.debug(f"{self} read took: {read_duration_ms:.1f}ms")
-
-        return processed_frame
+        return frame.copy()
 
     def _postprocess_image(self, image: np.ndarray, color_mode: ColorMode | None = None) -> np.ndarray:
         """
@@ -386,16 +390,23 @@ class OpenCVCamera(Camera):
         """
         while not self.stop_event.is_set():
             try:
-                color_image = self.read()
+                ret, frame = self.videocapture.read()
+                if not ret or frame is None:
+                    logger.warning(f"Failed to read frame in background for {self}.")
+                    time.sleep(0.01)
+                    continue
+
+                processed_frame = self._postprocess_image(frame)
 
                 with self.frame_lock:
-                    self.latest_frame = color_image
+                    self.latest_frame = processed_frame
+
                 self.new_frame_event.set()
 
-            except DeviceNotConnectedError:
-                break
             except Exception as e:
                 logger.warning(f"Error reading frame in background thread for {self}: {e}")
+                if not self.is_connected:
+                    break
 
     def _start_read_thread(self) -> None:
         """Starts or restarts the background read thread if it's not running."""

src/lerobot/constants.py

@@ -28,6 +28,9 @@ ROBOTS = "robots"
 ROBOT_TYPE = "robot_type"
 TELEOPERATORS = "teleoperators"
 
+ROBOTS = "robots"
+TELEOPERATORS = "teleoperators"
+
 # files & directories
 CHECKPOINTS_DIR = "checkpoints"
 LAST_CHECKPOINT_LINK = "last"

src/lerobot/motors/feetech/feetech.py

@@ -164,8 +164,9 @@ class FeetechMotorsBus(MotorsBus):
             )
 
     def _handshake(self) -> None:
-        self._assert_motors_exist()
-        self._assert_same_firmware()
+        # self._assert_motors_exist()
+        # self._assert_same_firmware()
+        return
 
     def _find_single_motor(self, motor: str, initial_baudrate: int | None = None) -> tuple[int, int]:
         if self.protocol_version == 0:
@@ -223,7 +224,7 @@ class FeetechMotorsBus(MotorsBus):
         for motor in self.motors:
             # By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on
             # the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0).
-            self.write("Return_Delay_Time", motor, return_delay_time)
+            # self.write("Return_Delay_Time", motor, 0) # THIS DOES NOT WORK FOR HLS3625
             # Set 'Maximum_Acceleration' to 254 to speedup acceleration and deceleration of the motors.
             if self.protocol_version == 0:
                 self.write("Maximum_Acceleration", motor, maximum_acceleration)
@@ -231,83 +232,58 @@ class FeetechMotorsBus(MotorsBus):
 
     @property
     def is_calibrated(self) -> bool:
-        motors_calibration = self.read_calibration()
-        if set(motors_calibration) != set(self.calibration):
-            return False
-
-        same_ranges = all(
-            self.calibration[motor].range_min == cal.range_min
-            and self.calibration[motor].range_max == cal.range_max
-            for motor, cal in motors_calibration.items()
-        )
-        if self.protocol_version == 1:
-            return same_ranges
-
-        same_offsets = all(
-            self.calibration[motor].homing_offset == cal.homing_offset
-            for motor, cal in motors_calibration.items()
-        )
-        return same_ranges and same_offsets
+        # Check if calibration data has been loaded from file
+        return bool(self.calibration)
 
     def read_calibration(self) -> dict[str, MotorCalibration]:
-        offsets, mins, maxes = {}, {}, {}
-        for motor in self.motors:
-            mins[motor] = self.read("Min_Position_Limit", motor, normalize=False)
-            maxes[motor] = self.read("Max_Position_Limit", motor, normalize=False)
-            offsets[motor] = (
-                self.read("Homing_Offset", motor, normalize=False) if self.protocol_version == 0 else 0
-            )
-
+        # Return empty calibration - we don't read from motors anymore
         calibration = {}
         for motor, m in self.motors.items():
             calibration[motor] = MotorCalibration(
                 id=m.id,
                 drive_mode=0,
-                homing_offset=offsets[motor],
-                range_min=mins[motor],
-                range_max=maxes[motor],
+                homing_offset=0,
+                range_min=0,
+                range_max=4095,  # Default max resolution
             )
 
         return calibration
 
-    def write_calibration(self, calibration_dict: dict[str, MotorCalibration], cache: bool = True) -> None:
-        for motor, calibration in calibration_dict.items():
-            if self.protocol_version == 0:
-                self.write("Homing_Offset", motor, calibration.homing_offset)
-            self.write("Min_Position_Limit", motor, calibration.range_min)
-            self.write("Max_Position_Limit", motor, calibration.range_max)
-
-        if cache:
-            self.calibration = calibration_dict
+    def write_calibration(self, calibration_dict: dict[str, MotorCalibration]) -> None:
+        # Only update the in-memory calibration, don't write to motors
+        self.calibration = calibration_dict
 
     def _get_half_turn_homings(self, positions: dict[NameOrID, Value]) -> dict[NameOrID, Value]:
         """
-        On Feetech Motors:
-        Present_Position = Actual_Position - Homing_Offset
+        Calculate homing offsets such that the current position becomes 0 degrees.
+
+        For Feetech motors:
+        - The homing offset is subtracted from the raw position during normalization
+        - So to make current position = 0 degrees, homing_offset = current_raw_position
         """
         half_turn_homings = {}
         for motor, pos in positions.items():
-            model = self._get_motor_model(motor)
-            max_res = self.model_resolution_table[model] - 1
-            half_turn_homings[motor] = pos - int(max_res / 2)
+            # The homing offset should be the current position
+            # This way, when we normalize: (pos - homing_offset) = 0
+            half_turn_homings[motor] = pos
 
         return half_turn_homings
 
-    def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+    def disable_torque(self, motors: str | list[str] | None = None, num_retry: int = 5) -> None:
         for motor in self._get_motors_list(motors):
             self.write("Torque_Enable", motor, TorqueMode.DISABLED.value, num_retry=num_retry)
-            self.write("Lock", motor, 0, num_retry=num_retry)
+            # self.write("Lock", motor, 0, num_retry=num_retry)
 
-    def _disable_torque(self, motor_id: int, model: str, num_retry: int = 0) -> None:
+    def _disable_torque(self, motor_id: int, model: str, num_retry: int = 5) -> None:
         addr, length = get_address(self.model_ctrl_table, model, "Torque_Enable")
         self._write(addr, length, motor_id, TorqueMode.DISABLED.value, num_retry=num_retry)
-        addr, length = get_address(self.model_ctrl_table, model, "Lock")
-        self._write(addr, length, motor_id, 0, num_retry=num_retry)
+        # addr, length = get_address(self.model_ctrl_table, model, "Lock")
+        # self._write(addr, length, motor_id, 0, num_retry=num_retry)
 
-    def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 0) -> None:
+    def enable_torque(self, motors: str | list[str] | None = None, num_retry: int = 5) -> None:
         for motor in self._get_motors_list(motors):
             self.write("Torque_Enable", motor, TorqueMode.ENABLED.value, num_retry=num_retry)
-            self.write("Lock", motor, 1, num_retry=num_retry)
+            # self.write("Lock", motor, 1, num_retry=num_retry)
 
     def _encode_sign(self, data_name: str, ids_values: dict[int, int]) -> dict[int, int]:
         for id_ in ids_values:

src/lerobot/motors/feetech/tables.py

@@ -151,6 +151,95 @@ SCS_SERIES_CONTROL_TABLE = {
     "Acceleration_2": (83, 1),  # don't know what that is
 }
 
+# http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
+HLS_SERIES_CONTROL_TABLE = {
+    # Version Information (0-4) - read-only
+    "Firmware_Major_Version": FIRMWARE_MAJOR_VERSION,  # (0, 1) read-only
+    "Firmware_Minor_Version": FIRMWARE_MINOR_VERSION,  # (1, 1) read-only
+    "End_Type": (2, 1),  # read-only - 0 represents little-endian storage
+    "Model_Number": MODEL_NUMBER,  # (3, 2) read-only
+    # EPROM configuration (5-39)
+    "ID": (5, 1),  # Main ID - unique identifier on bus
+    "Baud_Rate": (6, 1),  # 0-7 for different baud rates
+    "Secondary_ID": (7, 1),  # Secondary ID for write instructions
+    "Response_Status_Level": (8, 1),  # 0: limited response, 1: full response
+    "Min_Position_Limit": (9, 2),  # 0-4094 (0.087 degrees per unit)
+    "Max_Position_Limit": (11, 2),  # 1-4095 (0.087 degrees per unit)
+    "Max_Temperature_Limit": (13, 1),  # 0-100 (°C)
+    "Max_Voltage_Limit": (14, 1),  # 0-254 (0.1V per unit)
+    "Min_Voltage_Limit": (15, 1),  # 0-254 (0.1V per unit)
+    "Max_Torque_Limit": (16, 2),  # 0-1000 (0.1% per unit)
+    "Phase": (18, 1),  # Special function byte for motor phase configuration
+    "Unloading_Condition": (19, 1),  # Bit flags for protection conditions
+    "LED_Alarm_Condition": (20, 1),  # Bit flags for LED alarm conditions
+    "P_Coefficient": (21, 1),  # Position ring P proportional coefficient
+    "D_Coefficient": (22, 1),  # Position ring D differential coefficient
+    "I_Coefficient": (23, 1),  # Position ring I integral coefficient
+    "Minimum_Startup_Force": (24, 1),  # 0-254 (0.1% per unit)
+    "Point_Limit_Value": (25, 1),  # 0-254 - maximum point value = point_limit * 4
+    "CW_Dead_Zone": (26, 1),  # 0-16 (0.087 degrees per unit)
+    "CCW_Dead_Zone": (27, 1),  # 0-16 (0.087 degrees per unit)
+    "Protection_Current": (28, 2),  # 0-2047 (6.5 mA per unit)
+    "Angle_Resolution": (30, 1),  # 1-128 - amplification coefficient
+    "Homing_Offset": (31, 2),  # -4095 to 4095 (0.087 degrees per unit)
+    "Operating_Mode": (33, 1),  # 0: position, 1: speed, 2: current, 3: PWM
+    "P_Coefficient_Curr": (34, 1),  # Current ring P proportional coefficient
+    "I_Coefficient_Curr": (35, 1),  # Current ring I integral coefficient
+    # Address 36 undefined
+    "Speed_P_Coefficient": (37, 1),  # Speed closed-loop P proportional coefficient
+    "Overcurrent_Protection_Time": (38, 1),  # 0-254 (10ms per unit)
+    "Speed_I_Coefficient": (39, 1),  # Speed closed-loop I integral coefficient
+    # SRAM control (40-55)
+    "Torque_Enable": (40, 1),  # 0: off, 1: on, 2: damping
+    "Acceleration": (41, 1),  # 0-254 (8.7 degrees/second² per unit)
+    "Goal_Position": (42, 2),  # -32767 to 32767 (0.087 degrees per unit)
+    "Target_Torque": (44, 2),  # -2047 to 2047 (6.5 mA per unit)
+    "Goal_Velocity": (46, 2),  # -32767 to 32767 (0.732 RPM per unit)
+    "Torque_Limit": (48, 2),  # 0-1000 (0.1% per unit)
+    "P_Coefficient_Ring": (50, 1),  # Motor position ring proportional coefficient
+    "D_Coefficient_Ring": (51, 1),  # Motor position ring differential coefficient
+    "I_Coefficient_Ring": (52, 1),  # Motor position ring integral coefficient
+    "km": (53, 1),  # 0: position+current dual loop, 1: position single loop
+    # Address 54 undefined
+    "Lock": (55, 1),  # 0: close write lock, 1: open write lock
+    # SRAM feedback (56-73) - read-only
+    "Present_Position": (56, 2),  # read-only - current absolute position
+    "Present_Velocity": (58, 2),  # read-only - current motor rotation speed
+    "Present_Load": (60, 2),  # read-only - current load (0.1% per unit)
+    "Present_Voltage": (62, 1),  # read-only - current voltage (0.1V per unit)
+    "Present_Temperature": (63, 1),  # read-only - current temperature (°C)
+    "Async_Write_Flag": (64, 1),  # read-only - async write instruction flag
+    "Status": (65, 1),  # read-only - servo status bit flags
+    "Moving": (66, 1),  # read-only - movement status flags
+    "Target_Position": (67, 2),  # read-only - current target position
+    "Present_Current": (69, 2),  # read-only - current motor phase current (6.5 mA per unit)
+    # Address 71 undefined
+    "Present_Bias": (73, 2),  # read-only - current 0-point offset value
+    # Factory parameters (77-86) - read-only
+    "VFk_x10": (77, 1),  # read-only - factory parameter
+    "vKgI": (78, 1),  # read-only - factory parameter
+    "PFk_x10": (79, 1),  # read-only - factory parameter
+    "Moving_Velocity_Threshold": (80, 1),  # read-only - factory parameter
+    "DTs_ms": (81, 1),  # read-only - factory parameter
+    "eFk_x10": (82, 1),  # read-only - factory parameter
+    "Vk_ms": (83, 1),  # read-only - factory parameter
+    "Maximum_Velocity_Limit": (84, 1),  # read-only - factory parameter
+    "Maximum_Acceleration": (85, 1),  # read-only - factory parameter
+    "Acceleration_Multiplier": (86, 1),  # read-only - factory parameter
+}
+
+# HLS series baud rate table (same as STS/SMS series)
+HLS_SERIES_BAUDRATE_TABLE = {
+    1_000_000: 0,
+    500_000: 1,
+    250_000: 2,
+    128_000: 3,
+    115_200: 4,
+    76_800: 5,  # Note: HLS documentation mentions 76800 instead of 57600
+    57_600: 6,
+    38_400: 7,
+}
+
 STS_SMS_SERIES_BAUDRATE_TABLE = {
     1_000_000: 0,
     500_000: 1,
@@ -181,6 +270,7 @@ MODEL_CONTROL_TABLE = {
     "sts3250": STS_SMS_SERIES_CONTROL_TABLE,
     "scs0009": SCS_SERIES_CONTROL_TABLE,
     "sm8512bl": STS_SMS_SERIES_CONTROL_TABLE,
+    "hls3625": HLS_SERIES_CONTROL_TABLE,
 }
 
 MODEL_RESOLUTION = {
@@ -191,6 +281,7 @@ MODEL_RESOLUTION = {
     "sts3250": 4096,
     "sm8512bl": 4096,
     "scs0009": 1024,
+    "hls3625": 4096,
 }
 
 MODEL_BAUDRATE_TABLE = {
@@ -201,6 +292,7 @@ MODEL_BAUDRATE_TABLE = {
     "sts3215": STS_SMS_SERIES_BAUDRATE_TABLE,
     "sts3250": STS_SMS_SERIES_BAUDRATE_TABLE,
     "scs0009": SCS_SERIES_BAUDRATE_TABLE,
+    "hls3625": HLS_SERIES_BAUDRATE_TABLE,
 }
 
 # Sign-Magnitude encoding bits
@@ -210,6 +302,18 @@ STS_SMS_SERIES_ENCODINGS_TABLE = {
     "Present_Velocity": 15,
 }
 
+# HLS series sign-magnitude encoding bits
+HLS_SERIES_ENCODINGS_TABLE = {
+    "Homing_Offset": 15,  # BIT15 represents positive/negative direction
+    "Goal_Position": 15,  # BIT15 represents positive/negative direction
+    "Target_Torque": 15,  # BIT15 represents positive/negative direction in constant current mode
+    "Goal_Velocity": 15,  # BIT15 represents positive/negative direction in constant speed mode
+    "Present_Position": 15,  # BIT15 represents positive/negative direction
+    "Present_Velocity": 15,  # BIT15 represents positive/negative direction
+    "Present_Current": 15,  # BIT15 represents positive/negative direction
+    "Present_Load": 10,  # BIT10 represents positive/negative direction
+}
+
 MODEL_ENCODING_TABLE = {
     "sts_series": STS_SMS_SERIES_ENCODINGS_TABLE,
     "sms_series": STS_SMS_SERIES_ENCODINGS_TABLE,
@@ -218,6 +322,7 @@ MODEL_ENCODING_TABLE = {
     "sts3250": STS_SMS_SERIES_ENCODINGS_TABLE,
     "sm8512bl": STS_SMS_SERIES_ENCODINGS_TABLE,
     "scs0009": {},
+    "hls3625": HLS_SERIES_ENCODINGS_TABLE,
 }
 
 SCAN_BAUDRATES = [
@@ -239,6 +344,7 @@ MODEL_NUMBER_TABLE = {
     "sts3250": 2825,
     "sm8512bl": 11272,
     "scs0009": 1284,
+    "hls3625": 3338,
 }
 
 MODEL_PROTOCOL = {
@@ -249,4 +355,5 @@ MODEL_PROTOCOL = {
     "sts3250": 0,
     "sm8512bl": 0,
     "scs0009": 1,
+    "hls3625": 0,  # Uses FT-SCS protocol
 }

src/lerobot/motors/motors_bus.py

@@ -83,6 +83,9 @@ class MotorNormMode(str, Enum):
     DEGREES = "degrees"
 
 
+COUNT_TO_DEG = 0.087  # 1 encoder count = 0.087 °
+
+
 @dataclass
 class MotorCalibration:
     id: int
@@ -441,8 +444,8 @@ class MotorsBus(abc.ABC):
         try:
             if not self.port_handler.openPort():
                 raise OSError(f"Failed to open port '{self.port}'.")
-            elif handshake:
-                self._handshake()
+            # elif handshake:
+            # self._handshake()
         except (FileNotFoundError, OSError, serial.SerialException) as e:
             raise ConnectionError(
                 f"\nCould not connect on port '{self.port}'. Make sure you are using the correct port."
@@ -711,9 +714,8 @@ class MotorsBus(abc.ABC):
         self.reset_calibration(motors)
         actual_positions = self.sync_read("Present_Position", motors, normalize=False)
         homing_offsets = self._get_half_turn_homings(actual_positions)
-        for motor, offset in homing_offsets.items():
-            self.write("Homing_Offset", motor, offset)
 
+        # Don't write to motors, just return the calculated offsets
         return homing_offsets
 
     @abc.abstractmethod
@@ -782,21 +784,32 @@ class MotorsBus(abc.ABC):
             motor = self._id_to_name(id_)
             min_ = self.calibration[motor].range_min
             max_ = self.calibration[motor].range_max
+            homing_offset = self.calibration[motor].homing_offset
             drive_mode = self.apply_drive_mode and self.calibration[motor].drive_mode
+
             if max_ == min_:
                 raise ValueError(f"Invalid calibration for motor '{motor}': min and max are equal.")
 
-            bounded_val = min(max_, max(min_, val))
             if self.motors[motor].norm_mode is MotorNormMode.RANGE_M100_100:
+                bounded_val = min(max_, max(min_, val))
                 norm = (((bounded_val - min_) / (max_ - min_)) * 200) - 100
                 normalized_values[id_] = -norm if drive_mode else norm
             elif self.motors[motor].norm_mode is MotorNormMode.RANGE_0_100:
+                bounded_val = min(max_, max(min_, val))
                 norm = ((bounded_val - min_) / (max_ - min_)) * 100
                 normalized_values[id_] = 100 - norm if drive_mode else norm
             elif self.motors[motor].norm_mode is MotorNormMode.DEGREES:
-                mid = (min_ + max_) / 2
-                max_res = self.model_resolution_table[self._id_to_model(id_)] - 1
-                normalized_values[id_] = (val - mid) * 360 / max_res
+                # For motors without wrap-around handling
+                # The homing offset becomes 0 degrees
+
+                # Calculate difference from homing position
+                diff = val - homing_offset
+
+                # Convert to degrees
+                deg = diff * COUNT_TO_DEG
+
+                # Apply drive mode if needed
+                normalized_values[id_] = -deg if drive_mode else deg
             else:
                 raise NotImplementedError
 
@@ -811,7 +824,9 @@ class MotorsBus(abc.ABC):
             motor = self._id_to_name(id_)
             min_ = self.calibration[motor].range_min
             max_ = self.calibration[motor].range_max
+            homing_offset = self.calibration[motor].homing_offset
             drive_mode = self.apply_drive_mode and self.calibration[motor].drive_mode
+
             if max_ == min_:
                 raise ValueError(f"Invalid calibration for motor '{motor}': min and max are equal.")
 
@@ -824,9 +839,22 @@ class MotorsBus(abc.ABC):
                 bounded_val = min(100.0, max(0.0, val))
                 unnormalized_values[id_] = int((bounded_val / 100) * (max_ - min_) + min_)
             elif self.motors[motor].norm_mode is MotorNormMode.DEGREES:
-                mid = (min_ + max_) / 2
-                max_res = self.model_resolution_table[self._id_to_model(id_)] - 1
-                unnormalized_values[id_] = int((val * max_res / 360) + mid)
+                # For motors without wrap-around, simple conversion back
+                # Apply drive mode if needed
+                val = -val if drive_mode else val
+
+                # Convert degrees to raw counts
+                raw_counts = int(round(val / COUNT_TO_DEG))
+
+                # Add back the homing offset
+                raw_counts_with_offset = raw_counts + homing_offset
+
+                # Ensure value stays within calibrated motor range
+                # Use the calibration min/max if available
+                if min_ is not None and max_ is not None:
+                    raw_counts_with_offset = max(min_, min(max_, raw_counts_with_offset))
+
+                unnormalized_values[id_] = raw_counts_with_offset
             else:
                 raise NotImplementedError
 

src/lerobot/record.py

@@ -21,7 +21,7 @@ Example:
 python -m lerobot.record \
     --robot.type=so100_follower \
     --robot.port=/dev/tty.usbmodem58760431541 \
-    --robot.cameras="{laptop: {type: opencv, camera_index: 0, width: 640, height: 480}}" \
+    --robot.cameras="{laptop: {type: opencv, index_or_path: 0, width: 640, height: 480}}" \
     --robot.id=black \
     --dataset.repo_id=aliberts/record-test \
     --dataset.num_episodes=2 \
@@ -33,6 +33,41 @@ python -m lerobot.record \
     # <- Policy optional if you want to record with a policy \
     # --policy.path=${HF_USER}/my_policy \
 ```
+
+Example with bilateral teleoperation:
+
+```shell
+python -m lerobot.record \
+    --robot.type=so101_follower_t \
+    --robot.port=/dev/tty.usbmodem58760432961 \
+    --robot.id=follower_arm_torque \
+    --dataset.repo_id=pepijn/bilateral-teleop-test \
+    --dataset.num_episodes=5 \
+    --dataset.single_task="Wipe the table" \
+    --biteleop=true \
+    --teleop.type=so101_follower_t \
+    --teleop.port=/dev/tty.usbmodem58760432571 \
+    --teleop.id=leader_arm_torque \
+    --dataset.fps=100 \
+    --robot.cameras="{side: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 100}}" \
+    --display_data=true
+```
+
+Example Eval with bilateral teleoperation:
+```
+python -m lerobot.record  \
+  --robot.type=so101_follower_t \
+  --robot.port=/dev/tty.usbmodem58760432961 \
+  --robot.id=follower_arm_torque \
+  --robot.cameras="{side: {type: opencv, index_or_path: 0, width: 640, height: 480, fps: 100}}" \
+  --display_data=true \
+  --dataset.repo_id=pepijn223/eval_bilateral-wipe-large \
+  --dataset.single_task="Wipe the table" \
+  --policy.path=pepijn223/bilateral-wipe-large-single \
+  --dataset.fps=100 \
+  --biteleop=true
+```
+
 """
 
 import logging
@@ -62,7 +97,9 @@ from lerobot.robots import (  # noqa: F401
     make_robot_from_config,
     so100_follower,
     so101_follower,
+    so101_follower_torque,
 )
+from lerobot.robots.so101_follower_torque import SO101FollowerT
 from lerobot.teleoperators import (  # noqa: F401
     Teleoperator,
     TeleoperatorConfig,
@@ -89,6 +126,24 @@ from lerobot.utils.utils import (
 from lerobot.utils.visualization_utils import _init_rerun, log_rerun_data
 
 
+def split_interleaved_action(vec, motors):
+    """
+    vec     : 1‑D tensor/array, length = 3*len(motors)
+    motors  : ['shoulder_pan', 'shoulder_lift', …]
+
+    returns : pos, vel, tau  (three dicts keyed by joint name)
+    """
+    pos = {}
+    vel = {}
+    tau = {}
+    for i, j in enumerate(motors):
+        base = 3 * i
+        pos[j] = float(vec[base + 0])
+        vel[j] = float(vec[base + 1])
+        tau[j] = float(vec[base + 2])
+    return pos, vel, tau
+
+
 @dataclass
 class DatasetRecordConfig:
     # Dataset identifier. By convention it should match '{hf_username}/{dataset_name}' (e.g. `lerobot/test`).
@@ -133,7 +188,7 @@ class RecordConfig:
     robot: RobotConfig
     dataset: DatasetRecordConfig
     # Whether to control the robot with a teleoperator
-    teleop: TeleoperatorConfig | None = None
+    teleop: TeleoperatorConfig | RobotConfig | None = None
     # Whether to control the robot with a policy
     policy: PreTrainedConfig | None = None
     # Display all cameras on screen
@@ -142,6 +197,8 @@ class RecordConfig:
     play_sounds: bool = True
     # Resume recording on an existing dataset.
     resume: bool = False
+    # Enable bilateral teleoperation with force feedback
+    biteleop: bool = False
 
     def __post_init__(self):
         # HACK: We parse again the cli args here to get the pretrained path if there was one.
@@ -166,15 +223,23 @@ def record_loop(
     events: dict,
     fps: int,
     dataset: LeRobotDataset | None = None,
-    teleop: Teleoperator | List[Teleoperator] | None = None,
+    teleop: Teleoperator | List[Teleoperator] | Robot | None = None,
     policy: PreTrainedPolicy | None = None,
     control_time_s: int | None = None,
     single_task: str | None = None,
     display_data: bool = False,
+    biteleop: bool = False,
 ):
     if dataset is not None and dataset.fps != fps:
         raise ValueError(f"The dataset fps should be equal to requested fps ({dataset.fps} != {fps}).")
 
+    if biteleop and policy is None:
+        if not isinstance(robot, SO101FollowerT):
+            raise ValueError(
+                "Bilateral teleoperation requires both robot and teleop to be of type SO101FollowerT"
+            )
+        logging.info("Bilateral teleoperation mode enabled")
+
     teleop_arm = teleop_keyboard = None
     if isinstance(teleop, list):
         teleop_keyboard = next((t for t in teleop if isinstance(t, KeyboardTeleop)), None)
@@ -198,7 +263,11 @@ def record_loop(
 
     timestamp = 0
     start_episode_t = time.perf_counter()
-    while timestamp < control_time_s:
+
+    loop_count = 0
+    rerun_log_freq = max(1, int(fps / 10))
+
+    while control_time_s is not None and timestamp < control_time_s:
         start_loop_t = time.perf_counter()
 
         if events["exit_early"]:
@@ -210,7 +279,67 @@ def record_loop(
         if policy is not None or dataset is not None:
             observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
 
-        if policy is not None:
+        if (
+            biteleop
+            and isinstance(robot, SO101FollowerT)
+            and isinstance(teleop, SO101FollowerT)
+            and policy is None
+        ):
+            obs_f = observation  # robot is the follower
+            obs_l = teleop.get_observation()
+
+            pos_f = {j: obs_f[f"{j}.pos"] for j in robot.bus.motors}
+            vel_f = {j: obs_f[f"{j}.vel"] for j in robot.bus.motors}
+            tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in robot.bus.motors}
+
+            pos_l = {j: obs_l[f"{j}.pos"] for j in teleop.bus.motors}
+            vel_l = {j: obs_l[f"{j}.vel"] for j in teleop.bus.motors}
+            acc_l = {j: obs_l[f"{j}.acc"] for j in teleop.bus.motors}
+            tau_reaction_l = {j: obs_l[f"{j}.effort"] for j in teleop.bus.motors}
+
+            # Get control gains from robot
+            kp_gains = robot.kp_gains
+            kd_gains = robot.kd_gains
+            kf_gains = robot.kf_gains
+
+            # Compute torque commands
+            tau_cmd_f = [
+                (
+                    kp_gains[j] * (pos_l[j] - pos_f[j])  # Position tracking
+                    + kd_gains[j] * (vel_l[j] - vel_f[j])  # Velocity damping
+                    + kf_gains[j] * (-tau_reaction_l[j] - tau_reaction_f[j])
+                )  # Force reflection
+                for j in robot.bus.motors
+            ]
+
+            tau_cmd_l = [
+                (
+                    kp_gains[j] * (pos_f[j] - pos_l[j])  # Position tracking
+                    + kd_gains[j] * (vel_f[j] - vel_l[j])  # Velocity damping
+                    + kf_gains[j] * (-tau_reaction_f[j] - tau_reaction_l[j])
+                )  # Force reflection
+                for j in teleop.bus.motors
+            ]
+
+            action = {f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(robot.bus.motors)}
+            teleop_action = {f"{m}.effort": tau_cmd_l[i] for i, m in enumerate(teleop.bus.motors)}
+            teleop.send_action(teleop_action)
+            robot.send_action(action)
+
+            # For bilateral teleoperation, create custom observation and action for dataset
+            bilateral_action = {}
+            for j in teleop.bus.motors:
+                bilateral_action[f"{j}.pos"] = pos_l[j]
+                bilateral_action[f"{j}.vel"] = vel_l[j]
+                bilateral_action[f"{j}.acc"] = acc_l[j]
+                bilateral_action[f"{j}.effort"] = -tau_reaction_l[j]
+
+            # Override the observation_frame and action for dataset recording
+            if dataset is not None:
+                observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
+                action = bilateral_action
+
+        elif policy is not None and biteleop and isinstance(robot, SO101FollowerT):
             action_values = predict_action(
                 observation_frame,
                 policy,
@@ -219,10 +348,57 @@ def record_loop(
                 task=single_task,
                 robot_type=robot.robot_type,
             )
+            pos_f = {j: observation[f"{j}.pos"] for j in robot.bus.motors}
+            vel_f = {j: observation[f"{j}.vel"] for j in robot.bus.motors}
+            tau_reaction_f = {j: observation[f"{j}.effort"] for j in robot.bus.motors}
+
+            # The model returns [pos1, pos2, …, vel1, vel2, …, tau1, tau2, …]
+            motors = robot.bus.motors  # 6 joints
+            pos_l, vel_l, neg_tau_reaction_l = split_interleaved_action(
+                action_values, motors
+            )  # The model is trained and returns the effort already as negative: -tau_reaction_l
+
+            kp, kd, kf = robot.kp_gains, robot.kd_gains, robot.kf_gains
+
+            # Compute torque command for the follower robot
+            tau_cmd_f = [
+                (
+                    kp[j] * (pos_l[j] - pos_f[j])  # Position tracking
+                    + kd[j] * (vel_l[j] - vel_f[j])  # Velocity damping
+                    + kf[j] * (neg_tau_reaction_l[j] - tau_reaction_f[j])  # Force reflection
+                )
+                for j in robot.bus.motors
+            ]
+
+            # Format action with calculated torques and send to robot
+            action = {f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(robot.bus.motors)}
+            robot.send_action(action)
+
+            bilateral_action = {}
+            for j in robot.bus.motors:
+                bilateral_action[f"{j}.pos"] = pos_l[j]
+                bilateral_action[f"{j}.vel"] = vel_l[j]
+                bilateral_action[f"{j}.effort"] = neg_tau_reaction_l[j]
+
+            # Override the observation_frame and action for dataset recording
+            if dataset is not None:
+                observation_frame = build_dataset_frame(dataset.features, observation, prefix="observation")
+                action = bilateral_action
+
+        elif policy is not None and not biteleop:
+            action_values = predict_action(
+                observation_frame,
+                policy,
+                get_safe_torch_device(policy.config.device),
+                policy.config.use_amp,
+                task=single_task,
+                robot_type=robot.robot_type,
+            )
+
             action = {key: action_values[i].item() for i, key in enumerate(robot.action_features)}
-        elif policy is None and isinstance(teleop, Teleoperator):
+        elif policy is None and isinstance(teleop, Teleoperator) and not biteleop:
             action = teleop.get_action()
-        elif policy is None and isinstance(teleop, list):
+        elif policy is None and isinstance(teleop, list) and not biteleop:
             # TODO(pepijn, steven): clean the record loop for use of multiple robots (possibly with pipeline)
             arm_action = teleop_arm.get_action()
             arm_action = {f"arm_{k}": v for k, v in arm_action.items()}
@@ -241,31 +417,62 @@ def record_loop(
 
         # Action can eventually be clipped using `max_relative_target`,
         # so action actually sent is saved in the dataset.
-        sent_action = robot.send_action(action)
+        if not biteleop:
+            sent_action = robot.send_action(action)
 
         if dataset is not None:
-            action_frame = build_dataset_frame(dataset.features, sent_action, prefix="action")
+            # For bilateral teleoperation, use the bilateral_action (leader pos & torque)
+            # For other modes, use sent_action as usual
+            if biteleop and isinstance(robot, SO101FollowerT):
+                action_frame = build_dataset_frame(dataset.features, action, prefix="action")
+            else:
+                action_frame = build_dataset_frame(dataset.features, sent_action, prefix="action")
             frame = {**observation_frame, **action_frame}
             dataset.add_frame(frame, task=single_task)
 
-        if display_data:
+        if display_data and loop_count % rerun_log_freq == 0:
             log_rerun_data(observation, action)
 
         dt_s = time.perf_counter() - start_loop_t
         busy_wait(1 / fps - dt_s)
 
         timestamp = time.perf_counter() - start_episode_t
+        loop_count += 1
 
 
 @parser.wrap()
 def record(cfg: RecordConfig) -> LeRobotDataset:
     init_logging()
     logging.info(pformat(asdict(cfg)))
+
     if cfg.display_data:
         _init_rerun(session_name="recording")
 
     robot = make_robot_from_config(cfg.robot)
-    teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
+
+    if cfg.biteleop and cfg.teleop is not None:
+        print("Bilateral teleoperation enabled")
+        # For bilateral teleoperation, both arms must be SO101FollowerT robots
+        from lerobot.robots.so101_follower_torque.config_so101_follower_t import SO101FollowerTConfig
+
+        # Check if teleop config has the right type
+        if cfg.teleop.type != "so101_follower_t":
+            raise ValueError("Bilateral teleoperation requires teleop.type to be 'so101_follower_t'")
+
+        port = getattr(cfg.teleop, "port", None)
+        if port is None:
+            raise ValueError("Bilateral teleoperation requires teleop.port to be specified")
+
+        teleop_robot_config = SO101FollowerTConfig(
+            port=port,
+            id=getattr(cfg.teleop, "id", "leader_arm_torque"),
+            cameras=getattr(cfg.teleop, "cameras", {}),
+            disable_torque_on_disconnect=getattr(cfg.teleop, "disable_torque_on_disconnect", True),
+        )
+
+        teleop = SO101FollowerT(teleop_robot_config)
+    else:
+        teleop = make_teleoperator_from_config(cfg.teleop) if cfg.teleop is not None else None
 
     action_features = hw_to_dataset_features(robot.action_features, "action", cfg.dataset.video)
     obs_features = hw_to_dataset_features(robot.observation_features, "observation", cfg.dataset.video)
@@ -319,6 +526,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
             control_time_s=cfg.dataset.episode_time_s,
             single_task=cfg.dataset.single_task,
             display_data=cfg.display_data,
+            biteleop=cfg.biteleop,
         )
 
         # Execute a few seconds without recording to give time to manually reset the environment
@@ -335,6 +543,7 @@ def record(cfg: RecordConfig) -> LeRobotDataset:
                 control_time_s=cfg.dataset.reset_time_s,
                 single_task=cfg.dataset.single_task,
                 display_data=cfg.display_data,
+                biteleop=cfg.biteleop,
             )
 
         if events["rerecord_episode"]:

src/lerobot/replay.py

@@ -25,6 +25,17 @@ python -m lerobot.replay \
     --dataset.repo_id=aliberts/record-test \
     --dataset.episode=2
 ```
+
+Biteleop example:
+```shell
+python -m lerobot.replay \
+    --robot.type=so101_follower_t \
+    --robot.port=/dev/tty.usbmodem58760432961 \
+    --robot.id=follower_arm_torque \
+    --dataset.repo_id=pepijn223/bilateral-wipe-large \
+    --dataset.episode=10 \
+    --biteleop=true
+```
 """
 
 import logging
@@ -44,7 +55,9 @@ from lerobot.robots import (  # noqa: F401
     make_robot_from_config,
     so100_follower,
     so101_follower,
+    so101_follower_torque,
 )
+from lerobot.robots.so101_follower_torque import SO101FollowerT
 from lerobot.utils.robot_utils import busy_wait
 from lerobot.utils.utils import (
     init_logging,
@@ -70,6 +83,8 @@ class ReplayConfig:
     dataset: DatasetReplayConfig
     # Use vocal synthesis to read events.
     play_sounds: bool = True
+    # Use biteleop to replay the dataset
+    biteleop: bool = False
 
 
 @draccus.wrap()
@@ -80,22 +95,70 @@ def replay(cfg: ReplayConfig):
     robot = make_robot_from_config(cfg.robot)
     dataset = LeRobotDataset(cfg.dataset.repo_id, root=cfg.dataset.root, episodes=[cfg.dataset.episode])
     actions = dataset.hf_dataset.select_columns("action")
+
+    if cfg.biteleop:
+        if not isinstance(robot, SO101FollowerT):
+            raise ValueError(
+                "Bilateral teleoperation replay requires the robot to be of type SO101FollowerT."
+            )
+        log_say("Bilateral teleoperation replay enabled.", cfg.play_sounds)
+
     robot.connect()
 
     log_say("Replaying episode", cfg.play_sounds, blocking=True)
+
+    start_time_all = time.perf_counter()
+
     for idx in range(dataset.num_frames):
-        start_episode_t = time.perf_counter()
+        start_loop_t = time.perf_counter()
 
-        action_array = actions[idx]["action"]
-        action = {}
+        action_from_ds_array = actions[idx]["action"]
+        action_from_ds = {}
         for i, name in enumerate(dataset.features["action"]["names"]):
-            action[name] = action_array[i]
+            action_from_ds[name] = action_from_ds_array[i]
 
-        robot.send_action(action)
+        # Bilateral teleoperation
+        if cfg.biteleop:
+            obs_f = robot.get_observation()
+            pos_f = {j: obs_f[f"{j}.pos"] for j in robot.bus.motors}
+            vel_f = {j: obs_f[f"{j}.vel"] for j in robot.bus.motors}
+            tau_reaction_f = {j: obs_f[f"{j}.effort"] for j in robot.bus.motors}
 
-        dt_s = time.perf_counter() - start_episode_t
+            pos_l = {j: action_from_ds[f"{j}.pos"] for j in robot.bus.motors}
+            vel_l = {j: action_from_ds[f"{j}.vel"] for j in robot.bus.motors}
+            # The saved effort in dataset is -tau_reaction_l
+            neg_tau_reaction_l = {j: action_from_ds[f"{j}.effort"] for j in robot.bus.motors}
+
+            # Get control gains from the robot instance
+            kp_gains = robot.kp_gains
+            kd_gains = robot.kd_gains
+            kf_gains = robot.kf_gains
+
+            # Compute torque command for the follower robot
+            tau_cmd_f = [
+                (
+                    kp_gains[j] * (pos_l[j] - pos_f[j])  # Position tracking
+                    + kd_gains[j] * (vel_l[j] - vel_f[j])  # Velocity damping
+                    + kf_gains[j] * (neg_tau_reaction_l[j] - tau_reaction_f[j])  # Force reflection
+                )
+                for j in robot.bus.motors
+            ]
+
+            # Format action with calculated torques and send to robot
+            action_to_send = {f"{m}.effort": tau_cmd_f[i] for i, m in enumerate(robot.bus.motors)}
+            robot.send_action(action_to_send)
+        else:
+            # Original logic for standard position-based replay
+            robot.send_action(action_from_ds)
+
+        dt_s = time.perf_counter() - start_loop_t
         busy_wait(1 / dataset.fps - dt_s)
 
+    total_time = time.perf_counter() - start_time_all
+    actual_fps = idx / total_time if total_time > 0 else float("inf")
+    logging.info(f"Average FPS achieved over episode: {actual_fps:.2f}")
+    log_say(f"Average FPS achieved: {actual_fps:.2f}", cfg.play_sounds)
+
     robot.disconnect()
 
 

src/lerobot/robots/so101_follower_torque/__init__.py

@@ -0,0 +1,2 @@
+from .config_so101_follower_t import SO101FollowerTConfig
+from .so101_follower_t import SO101FollowerT

src/lerobot/robots/so101_follower_torque/config_so101_follower_t.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# 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 dataclasses import dataclass, field
+
+from lerobot.cameras import CameraConfig
+
+from ..config import RobotConfig
+
+
+@RobotConfig.register_subclass("so101_follower_t")
+@dataclass
+class SO101FollowerTConfig(RobotConfig):
+    # Port to connect to the arm
+    port: str
+
+    disable_torque_on_disconnect: bool = True
+
+    # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes.
+    # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as
+    # the number of motors in your follower arms.
+    max_relative_target: int | None = None
+
+    # cameras
+    cameras: dict[str, CameraConfig] = field(default_factory=dict)

src/lerobot/robots/so101_follower_torque/so101_follower_t.py

@@ -0,0 +1,553 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# 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 collections
+import logging
+import time
+from functools import cached_property
+from typing import Any
+
+import numpy as np
+import pinocchio as pin
+from scipy.signal import butter, lfilter
+
+from lerobot.cameras.utils import make_cameras_from_configs
+from lerobot.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
+from lerobot.motors import Motor, MotorCalibration, MotorNormMode
+from lerobot.motors.feetech import (
+    FeetechMotorsBus,
+)
+
+from ..robot import Robot
+from .config_so101_follower_t import SO101FollowerTConfig
+
+logger = logging.getLogger(__name__)
+
+
+class SO101FollowerT(Robot):
+    """
+    SO-101 Arm with HLS3625 motors with current control.
+    """
+
+    config_class = SO101FollowerTConfig
+    name = "so101_follower_t"
+
+    _CURRENT_STEP_A: float = 6.5e-3  # 6.5 mA per register LSB #http://doc.feetech.cn/#/prodinfodownload?srcType=FT-SMS-STS-emanual-229f4476422d4059abfb1cb0
+    _KT_NM_PER_AMP: float = 0.814  # Torque constant Kt [N·m/A] #https://www.feetechrc.com/811177.html
+    _MAX_CURRENT_A: float = 4.0  # Safe driver limit
+
+    # Position gains
+    _KP_GAINS = {
+        "shoulder_pan": 5.0,
+        "shoulder_lift": 7.0,
+        "elbow_flex": 7.0,
+        "wrist_flex": 5.0,
+        "wrist_roll": 5.0,
+        "gripper": 5.0,
+    }
+
+    # Velocity gains
+    _KD_GAINS = {
+        "shoulder_pan": 0.4,
+        "shoulder_lift": 0.6,
+        "elbow_flex": 0.6,
+        "wrist_flex": 0.4,
+        "wrist_roll": 0.4,
+        "gripper": 0.4,
+    }
+
+    # Force gains
+    _KF_GAINS = {
+        "shoulder_pan": 0.05,
+        "shoulder_lift": 0.05,
+        "elbow_flex": 0.05,
+        "wrist_flex": 0.05,
+        "wrist_roll": 0.05,
+        "gripper": 0.05,
+    }
+
+    # Viscous friction coefficient
+    _FRICTION_VISCOUS = {
+        "shoulder_pan": 0.05,
+        "shoulder_lift": 0.08,
+        "elbow_flex": 0.05,
+        "wrist_flex": 0.05,
+        "wrist_roll": 0.05,
+        "gripper": 0.05,
+    }
+
+    # Coulomb/static friction
+    _FRICTION_COULOMB = {
+        "shoulder_pan": 0.15,
+        "shoulder_lift": 0.25,
+        "elbow_flex": 0.25,
+        "wrist_flex": 0.20,
+        "wrist_roll": 0.20,
+        "gripper": 0.20,
+    }
+
+    def __init__(self, config: SO101FollowerTConfig):
+        super().__init__(config)
+        self.config = config
+
+        if self.calibration_fpath.is_file() and not self.calibration:
+            self._load_calibration()
+
+        self.bus = FeetechMotorsBus(
+            port=self.config.port,
+            motors={
+                "shoulder_pan": Motor(1, "hls3625", MotorNormMode.DEGREES),
+                "shoulder_lift": Motor(2, "hls3625", MotorNormMode.DEGREES),
+                "elbow_flex": Motor(3, "hls3625", MotorNormMode.DEGREES),
+                "wrist_flex": Motor(4, "hls3625", MotorNormMode.DEGREES),
+                "wrist_roll": Motor(5, "hls3625", MotorNormMode.DEGREES),
+                "gripper": Motor(6, "hls3625", MotorNormMode.DEGREES),
+            },
+            calibration=self.calibration,
+        )
+        self.cameras = make_cameras_from_configs(config.cameras)
+
+        self.pin_robot = pin.RobotWrapper.BuildFromURDF("urdf/so101_new_calib.urdf", "urdf")
+
+        flip = {
+            "shoulder_pan": True,
+            "shoulder_lift": True,
+            "elbow_flex": True,
+            "wrist_flex": True,
+            "wrist_roll": True,
+            "gripper": True,
+        }
+        self.torque_sign = {n: (-1.0 if flip[n] else 1.0) for n in self.bus.motors}
+
+        self._prev_pos_rad: dict[str, float] | None = None
+        self._prev_vel_rad: dict[str, float] | None = None
+        self._prev_t: float | None = None
+
+        # Butterworth low-pass filter parameters
+        self._cutoff_freq = 10.0  # Hz, cutoff frequency for the filter
+        self._filter_order = 2  # Filter order
+        self._sampling_freq = 100.0  # Hz, (control loop frequency)
+
+        nyquist_freq = self._sampling_freq / 2
+        normalized_cutoff = self._cutoff_freq / nyquist_freq
+        self._b, self._a = butter(self._filter_order, normalized_cutoff, btype="low")
+
+        # History buffers
+        self._pos_history = {m: collections.deque(maxlen=20) for m in self.bus.motors}
+        self._vel_raw_history = {m: collections.deque(maxlen=20) for m in self.bus.motors}
+        self._time_history = collections.deque(maxlen=20)
+
+        self._last_observation = None
+
+    @property
+    def _motors_ft(self) -> dict[str, type]:
+        d: dict[str, type] = {}
+        for motor in self.bus.motors:
+            d[f"{motor}.pos"] = float
+            d[f"{motor}.vel"] = float
+            d[f"{motor}.effort"] = float
+        return d
+
+    @property
+    def _cameras_ft(self) -> dict[str, tuple]:
+        return {
+            cam: (self.config.cameras[cam].height, self.config.cameras[cam].width, 3) for cam in self.cameras
+        }
+
+    @cached_property
+    def observation_features(self) -> dict[str, type | tuple]:
+        return {**self._motors_ft, **self._cameras_ft}
+
+    @cached_property
+    def action_features(self) -> dict[str, type]:
+        d: dict[str, type] = {}
+        for motor in self.bus.motors:
+            d[f"{motor}.pos"] = float
+            d[f"{motor}.vel"] = float
+            d[f"{motor}.effort"] = float
+        return d
+
+    @property
+    def is_connected(self) -> bool:
+        return self.bus.is_connected and all(cam.is_connected for cam in self.cameras.values())
+
+    @property
+    def kp_gains(self) -> dict[str, float]:
+        """Position control gains [Nm/rad] for bilateral teleoperation"""
+        return self._KP_GAINS.copy()
+
+    @property
+    def kd_gains(self) -> dict[str, float]:
+        """Velocity control gains [Nm⋅s/rad] for bilateral teleoperation"""
+        return self._KD_GAINS.copy()
+
+    @property
+    def kf_gains(self) -> dict[str, float]:
+        """Force control gains for bilateral teleoperation"""
+        return self._KF_GAINS.copy()
+
+    @property
+    def friction_viscous(self) -> dict[str, float]:
+        """Viscous friction coefficients [Nm⋅s/rad] for friction compensation"""
+        return self._FRICTION_VISCOUS.copy()
+
+    @property
+    def friction_coulomb(self) -> dict[str, float]:
+        """Coulomb friction coefficients [Nm] for friction compensation"""
+        return self._FRICTION_COULOMB.copy()
+
+    def set_butterworth_params(self, cutoff_freq: float = 10.0, order: int = 2) -> None:
+        """Configure Butterworth low-pass filter parameters for velocity/acceleration estimation.
+
+        Args:
+            cutoff_freq: Cutoff frequency in Hz (default: 10 Hz)
+            order: Filter order (default: 2)
+        """
+        if cutoff_freq <= 0:
+            raise ValueError("Cutoff frequency must be positive")
+        if cutoff_freq >= self._sampling_freq / 2:
+            raise ValueError(
+                f"Cutoff frequency must be less than Nyquist frequency ({self._sampling_freq / 2} Hz)"
+            )
+        if order < 1:
+            raise ValueError("Filter order must be at least 1")
+
+        self._cutoff_freq = cutoff_freq
+        self._filter_order = order
+
+        nyquist_freq = self._sampling_freq / 2
+        normalized_cutoff = self._cutoff_freq / nyquist_freq
+        self._b, self._a = butter(self._filter_order, normalized_cutoff, btype="low")
+
+        # Clear buffers
+        for m in self.bus.motors:
+            self._pos_history[m].clear()
+            self._vel_raw_history[m].clear()
+        self._time_history.clear()
+
+        logger.info(f"Butterworth filter updated: cutoff_freq={cutoff_freq} Hz, order={order}")
+
+    def _current_to_torque_nm(self, raw: dict[str, Any]) -> dict[str, float]:
+        """Convert "Present_Current" register counts (±2047) → torque [Nm].
+        Values are clamped to ±3A before conversion for protection.
+        """
+        max_cnt = int(round(self._MAX_CURRENT_A / self._CURRENT_STEP_A))  # ≈ 462
+        coef = self._CURRENT_STEP_A * self._KT_NM_PER_AMP
+        return {k: self.torque_sign[k] * max(-max_cnt, min(max_cnt, v)) * coef for k, v in raw.items()}
+
+    def _torque_nm_to_current(self, torque: dict[str, float]) -> dict[str, int]:
+        """Convert torque [Nm] to register counts, clamped to ±3A (2.44 Nm)."""
+        inv_coef = 1.0 / (self._CURRENT_STEP_A * self._KT_NM_PER_AMP)
+        max_cnt = int(round(self._MAX_CURRENT_A / self._CURRENT_STEP_A))
+        counts = {}
+        for k, τ in torque.items():
+            cnt = τ * self.torque_sign[k] * inv_coef
+            cnt = max(-max_cnt, min(max_cnt, cnt))
+            counts[k] = int(round(cnt))
+        return counts
+
+    def _deg_to_rad(self, deg: dict[str, float | int]) -> dict[str, float]:
+        """Degrees to radians."""
+        return {m: np.deg2rad(float(v)) for m, v in deg.items()}
+
+    def _gravity_from_q(self, q_rad: dict[str, float]) -> dict[str, float]:
+        """
+        Compute g(q) [N m] for all joints in the robot.
+        The order of joints in the URDF matches self.bus.motors.
+        """
+        q = np.zeros(self.pin_robot.model.nq)
+        for i, motor_name in enumerate(self.bus.motors):
+            q[i] = q_rad[motor_name]
+
+        g = pin.computeGeneralizedGravity(self.pin_robot.model, self.pin_robot.data, q)
+
+        return {motor_name: float(g[i]) for i, motor_name in enumerate(self.bus.motors)}
+
+    def _inertia_from_q_dq(
+        self, q_rad: dict[str, float], dq_rad: dict[str, float], ddq_rad: dict[str, float]
+    ) -> dict[str, float]:
+        """
+        Compute inertia torques τ_inertia = M(q) * ddq directly from URDF model.
+        """
+        q = np.zeros(self.pin_robot.model.nq)
+        dq = np.zeros(self.pin_robot.model.nv)
+        ddq = np.zeros(self.pin_robot.model.nv)
+
+        for i, motor_name in enumerate(self.bus.motors):
+            q[i] = q_rad[motor_name]
+            dq[i] = dq_rad[motor_name]
+            ddq[i] = ddq_rad[motor_name]
+
+        # Compute mass matrix M(q)
+        mass_matrix = pin.crba(self.pin_robot.model, self.pin_robot.data, q)
+
+        # Compute inertia torques: τ_inertia = M(q) * ddq
+        tau_inertia = mass_matrix @ ddq
+
+        return {motor_name: float(tau_inertia[i]) for i, motor_name in enumerate(self.bus.motors)}
+
+    def _compute_model_based_disturbance(
+        self,
+        q_rad: dict[str, float],
+        dq_rad: dict[str, float],
+        ddq_rad: dict[str, float],
+        tau_measured: dict[str, float],
+    ) -> dict[str, float]:
+        """
+        Compute disturbance torques using direct model-based approach:
+        τ_disturbance = τ_measured - τ_gravity - τ_inertia - τ_friction
+
+        Args:
+            include_friction: If True, also removes friction from the disturbance calculation
+        """
+        tau_gravity = self._gravity_from_q(q_rad)
+        tau_inertia = self._inertia_from_q_dq(q_rad, dq_rad, ddq_rad)
+
+        # Compute disturbance
+        tau_disturbance = {}
+        tau_friction = {}
+        for motor_name in self.bus.motors:
+            tau_dist = tau_measured[motor_name] - tau_gravity[motor_name] - tau_inertia[motor_name]
+
+            # Calculate friction torque
+            omega = dq_rad[motor_name]
+            tau_friction_motor = self._FRICTION_VISCOUS[motor_name] * omega + self._FRICTION_COULOMB[
+                motor_name
+            ] * (1.0 if omega > 0.01 else -1.0 if omega < -0.01 else 0.0)
+            # Apply torque sign correction
+            tau_friction_motor = -tau_friction_motor
+            tau_friction[motor_name] = tau_friction_motor
+            tau_dist -= tau_friction_motor
+
+            tau_disturbance[motor_name] = tau_dist
+
+        return tau_disturbance
+
+    def connect(self, calibrate: bool = True) -> None:
+        """
+        We assume that at connection time, arm is in a rest position,
+        and torque can be safely disabled to run calibration.
+        """
+        if self.is_connected:
+            raise DeviceAlreadyConnectedError(f"{self} already connected")
+
+        # Ensure calibration is loaded from file if it exists
+        if self.calibration_fpath.is_file() and not self.calibration:
+            self._load_calibration()
+            # Update the bus with the loaded calibration
+            self.bus.calibration = self.calibration
+
+        self.bus.connect()
+        if not self.is_calibrated and calibrate:
+            self.calibrate()
+
+        for cam in self.cameras.values():
+            cam.connect()
+
+        self.configure()
+        logger.info(f"{self} connected.")
+
+    @property
+    def is_calibrated(self) -> bool:
+        # Check if calibration file exists and is loaded
+        return self.calibration_fpath.is_file() and bool(self.calibration)
+
+    def calibrate(self) -> None:
+        logger.info(f"\nRunning calibration of {self}")
+        self.bus.disable_torque()
+        for motor in self.bus.motors:
+            self.bus.write("Operating_Mode", motor, 2, num_retry=2)  # Set to current mode
+
+        input(f"Move {self} to the middle of its range of motion and press ENTER....")
+        homing_offsets = self.bus.set_half_turn_homings()
+
+        print(
+            "Move all joints sequentially through their entire ranges "
+            "of motion.\nRecording positions. Press ENTER to stop..."
+        )
+        range_mins, range_maxes = self.bus.record_ranges_of_motion()
+
+        self.calibration = {}
+        for motor, m in self.bus.motors.items():
+            self.calibration[motor] = MotorCalibration(
+                id=m.id,
+                drive_mode=0,
+                homing_offset=int(homing_offsets[motor]),
+                range_min=int(range_mins[motor]),
+                range_max=int(range_maxes[motor]),
+            )
+
+        # Update the bus calibration with the new values
+        self.bus.calibration = self.calibration
+        # Save calibration to file only
+        self._save_calibration()
+        print("Calibration saved to", self.calibration_fpath)
+
+    def configure(self) -> None:
+        self.bus.disable_torque()  # here was issue at startup previously
+        for motor in self.bus.motors:
+            self.bus.write("Operating_Mode", motor, 2, num_retry=2)  # Set to current mode
+            self.bus.write("Present_Current", motor, 0, normalize=False, num_retry=5)
+
+    def setup_motors(self) -> None:
+        for motor in reversed(self.bus.motors):
+            input(f"Connect the controller board to the '{motor}' motor only and press enter.")
+            self.bus.setup_motor(motor)
+            print(f"'{motor}' motor id set to {self.bus.motors[motor].id}")
+
+    def get_observation(self) -> dict[str, Any]:
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        t_now = time.perf_counter()
+
+        # Position
+        pos_deg = self.bus.sync_read("Present_Position", num_retry=5)
+        pos_rad = self._deg_to_rad(pos_deg)
+
+        # Store position and time history
+        for m in pos_rad:
+            self._pos_history[m].append(pos_rad[m])
+        self._time_history.append(t_now)
+
+        # Calculate raw velocity
+        vel_rad_raw = {}
+        if self._prev_pos_rad is None or self._prev_t is None:
+            vel_rad_raw = dict.fromkeys(pos_rad, 0.0)
+        else:
+            dt = t_now - self._prev_t
+            dt = max(dt, 1e-4)  # Avoid division by zero
+            vel_rad_raw = {m: (pos_rad[m] - self._prev_pos_rad[m]) / dt for m in pos_rad}
+
+        # Store raw velocity history
+        for m in vel_rad_raw:
+            self._vel_raw_history[m].append(vel_rad_raw[m])
+
+        # Apply Butterworth low-pass filter to velocity
+        vel_rad = {}
+        for m in pos_rad:
+            if len(self._vel_raw_history[m]) >= 10:
+                vel_raw_array = np.array(list(self._vel_raw_history[m]))
+
+                # Apply Butterworth filter
+                vel_filtered = lfilter(self._b, self._a, vel_raw_array)
+                vel_rad[m] = vel_filtered[-1]
+            else:
+                vel_rad[m] = vel_rad_raw[m]
+
+        # Calculate acceleration from filtered velocity
+        acc_rad = {}
+        if self._prev_vel_rad is None or self._prev_t is None:
+            acc_rad = dict.fromkeys(pos_rad, 0.0)
+        else:
+            dt = t_now - self._prev_t
+            dt = max(dt, 1e-4)  # Avoid division by zero
+            acc_rad = {m: (vel_rad[m] - self._prev_vel_rad[m]) / dt for m in vel_rad}
+
+        self._prev_pos_rad = pos_rad.copy()
+        self._prev_vel_rad = vel_rad.copy()
+        self._prev_t = t_now
+
+        # Measured torque (Nm)
+        cur_raw = self.bus.sync_read("Present_Current", normalize=False, num_retry=5)
+        tau_meas = self._current_to_torque_nm(cur_raw)
+
+        # Compute reaction torques using model-based approach
+        tau_reaction = self._compute_model_based_disturbance(pos_rad, vel_rad, acc_rad, tau_meas)
+
+        obs_dict = {}
+        obs_dict |= {f"{m}.pos": pos_rad[m] for m in self.bus.motors}
+        obs_dict |= {f"{m}.vel": vel_rad[m] for m in self.bus.motors}
+        obs_dict |= {f"{m}.acc": acc_rad[m] for m in self.bus.motors}
+        obs_dict |= {f"{m}.effort": tau_reaction[m] for m in self.bus.motors}
+
+        # Capture images from cameras
+        for cam_key, cam in self.cameras.items():
+            start = time.perf_counter()
+            obs_dict[cam_key] = cam.read()
+            dt_ms = (time.perf_counter() - start) * 1e3
+            logger.debug(f"{self} read {cam_key}: {dt_ms:.1f}ms")
+
+        # Store observation for feedforward compensation
+        self._last_observation = obs_dict.copy()
+
+        return obs_dict
+
+    def send_action(self, action: dict[str, Any]) -> dict[str, Any]:
+        """Command arm to move to a target torque for a joint.
+
+        Raises:
+            RobotDeviceNotConnectedError: if robot is not connected.
+
+        Returns:
+            the action sent to the motors.
+        """
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        # Extract torque commands
+        tau_cmd_nm = {k.removesuffix(".effort"): float(v) for k, v in action.items() if k.endswith(".effort")}
+        if not tau_cmd_nm:
+            return action
+
+        # Add feedforward compensation if we have a last observation
+        if self._last_observation is not None:
+            # Extract position, velocity, acceleration from last observation
+            pos_rad = {m: self._last_observation[f"{m}.pos"] for m in self.bus.motors}
+            vel_rad = {m: self._last_observation[f"{m}.vel"] for m in self.bus.motors}
+            acc_rad = {m: self._last_observation[f"{m}.acc"] for m in self.bus.motors}
+
+            # Compute feedforward terms
+            tau_gravity = self._gravity_from_q(pos_rad)
+            tau_inertia = self._inertia_from_q_dq(pos_rad, vel_rad, acc_rad)
+
+            # Add feedforward compensation to commanded torques
+            for motor in tau_cmd_nm:
+                # Add gravity compensation
+                tau_cmd_nm[motor] += tau_gravity[motor]
+
+                # Add inertia compensation
+                tau_cmd_nm[motor] += tau_inertia[motor]
+
+                # Add friction compensation
+                omega = vel_rad[motor]
+                tau_friction = self._FRICTION_VISCOUS[motor] * omega + self._FRICTION_COULOMB[motor] * (
+                    1.0 if omega > 0.01 else -1.0 if omega < -0.01 else 0.0
+                )
+                tau_friction = -tau_friction  # Apply torque sign correction
+                tau_cmd_nm[motor] += tau_friction
+
+        inv_coef = 1.0 / (self._CURRENT_STEP_A * self._KT_NM_PER_AMP)
+        max_cnt = int(round(self._MAX_CURRENT_A / self._CURRENT_STEP_A))
+        counts = {}
+        for joint, τ in tau_cmd_nm.items():
+            cnt = τ * self.torque_sign[joint] * inv_coef  # flip SIGN
+            cnt = max(-max_cnt, min(max_cnt, cnt))
+            counts[joint] = int(round(cnt))
+
+        self.bus.sync_write("Target_Torque", counts, normalize=False, num_retry=2)
+        self._last_cmd_nm = tau_cmd_nm
+        return action
+
+    def disconnect(self):
+        if not self.is_connected:
+            raise DeviceNotConnectedError(f"{self} is not connected.")
+
+        self.bus.disconnect(self.config.disable_torque_on_disconnect)
+        for cam in self.cameras.values():
+            cam.disconnect()
+
+        logger.info(f"{self} disconnected.")

src/lerobot/robots/utils.py

@@ -37,6 +37,10 @@ def make_robot_from_config(config: RobotConfig) -> Robot:
         from .so101_follower import SO101Follower
 
         return SO101Follower(config)
+    elif config.type == "so101_follower_t":
+        from .so101_follower_torque import SO101FollowerT
+
+        return SO101FollowerT(config)
     elif config.type == "lekiwi":
         from .lekiwi import LeKiwi
 

src/lerobot/setup_motors.py

@@ -35,6 +35,7 @@ from lerobot.robots import (  # noqa: F401
     make_robot_from_config,
     so100_follower,
     so101_follower,
+    so101_follower_torque,
 )
 from lerobot.teleoperators import (  # noqa: F401
     TeleoperatorConfig,
@@ -52,6 +53,7 @@ COMPATIBLE_DEVICES = [
     "so101_follower",
     "so101_leader",
     "lekiwi",
+    "so101_follower_t",
 ]
 
 

src/lerobot/teleoperators/utils.py

@@ -33,6 +33,12 @@ def make_teleoperator_from_config(config: TeleoperatorConfig) -> Teleoperator:
         from .so101_leader import SO101Leader
 
         return SO101Leader(config)
+    elif config.type == "so101_follower_t":
+        # For bilateral teleoperation, SO101FollowerT is used as a robot, not a teleoperator
+        # This should be handled in the record.py file instead
+        raise ValueError(
+            "so101_follower_t should be created as a robot instance for bilateral teleoperation, not as a teleoperator"
+        )
     elif config.type == "stretch3":
         from .stretch3_gamepad import Stretch3GamePad
 

src/lerobot/utils/visualization_utils.py

@@ -28,19 +28,35 @@ def _init_rerun(session_name: str = "lerobot_control_loop") -> None:
     rr.spawn(memory_limit=memory_limit)
 
 
-def log_rerun_data(observation: dict[str | Any], action: dict[str | Any]):
+def log_rerun_data(observation: dict[str, Any], action: dict[str, Any]):
     for obs, val in observation.items():
         if isinstance(val, float):
             rr.log(f"observation.{obs}", rr.Scalar(val))
+        elif isinstance(val, dict):
+            # Handle dictionary of joint values
+            for joint_name, joint_val in val.items():
+                if isinstance(joint_val, (float, int)):
+                    rr.log(f"observation.{obs}.{joint_name}", rr.Scalar(float(joint_val)))
         elif isinstance(val, np.ndarray):
             if val.ndim == 1:
                 for i, v in enumerate(val):
                     rr.log(f"observation.{obs}_{i}", rr.Scalar(float(v)))
             else:
                 rr.log(f"observation.{obs}", rr.Image(val), static=True)
+
     for act, val in action.items():
         if isinstance(val, float):
             rr.log(f"action.{act}", rr.Scalar(val))
+        elif isinstance(val, dict):
+            # Handle dictionary of joint values
+            for joint_name, joint_val in val.items():
+                if isinstance(joint_val, (float, int)):
+                    rr.log(f"action.{act}.{joint_name}", rr.Scalar(float(joint_val)))
         elif isinstance(val, np.ndarray):
             for i, v in enumerate(val):
                 rr.log(f"action.{act}_{i}", rr.Scalar(float(v)))
+        elif isinstance(val, list):
+            # Handle list of values
+            for i, v in enumerate(val):
+                if isinstance(v, (float, int)):
+                    rr.log(f"action.{act}_{i}", rr.Scalar(float(v)))

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"
+}

urdf/assets/base_motor_holder_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8cd2f241037ea377af1191fffe0dd9d9006beea6dcc48543660ed41647072424
+size 1877084

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"
+}

urdf/assets/base_so101_v2.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb12b7026575e1f70ccc7240051f9d943553bf34e5128537de6cd86fae33924d
+size 471584

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"
+}

urdf/assets/motor_holder_so101_base_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:31242ae6fb59d8b15c66617b88ad8e9bded62d57c35d11c0c43a70d2f4caa95b
+size 1129384

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"
+}

urdf/assets/motor_holder_so101_wrist_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:887f92e6013cb64ea3a1ab8675e92da1e0beacfd5e001f972523540545e08011
+size 1052184

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"
+}

urdf/assets/moving_jaw_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:785a9dded2f474bc1d869e0d3dae398a3dcd9c0c345640040472210d2861fa9d
+size 1413584

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"
+}

urdf/assets/rotation_pitch_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9be900cc2a2bf718102841ef82ef8d2873842427648092c8ed2ca1e2ef4ffa34
+size 883684

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"
+}

urdf/assets/sts3215_03a_no_horn_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:75ef3781b752e4065891aea855e34dc161a38a549549cd0970cedd07eae6f887
+size 865884

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"
+}

urdf/assets/sts3215_03a_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a37c871fb502483ab96c256baf457d36f2e97afc9205313d9c5ab275ef941cd0
+size 954084

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"
+}

urdf/assets/under_arm_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d01d1f2de365651dcad9d6669e94ff87ff7652b5bb2d10752a66a456a86dbc71
+size 1975884

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"
+}

urdf/assets/upper_arm_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:475056e03a17e71919b82fd88ab9a0b898ab50164f2a7943652a6b2941bb2d4f
+size 1303484

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"
+}

urdf/assets/waveshare_mounting_plate_so101_v2.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e197e24005a07d01bbc06a8c42311664eaeda415bf859f68fa247884d0f1a6e9
+size 62784

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"
+}

urdf/assets/wrist_roll_follower_so101_v1.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4b17b410a12d64ec39554abc3e8054d8a97384b2dc4a8d95a5ecb2a93670f5f4
+size 1439884

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"
+}

urdf/assets/wrist_roll_pitch_so101_v2.stl

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6c7ec5525b4d8b9e397a30ab4bb0037156a5d5f38a4adf2c7d943d6c56eda5ae
+size 2699784

urdf/so101_new_calib.urdf

@@ -0,0 +1,453 @@
+<?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_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>

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>