Added calibration visualization

lerobot/common/robot_devices/motors/feetech.py

@@ -16,13 +16,6 @@ TIMEOUT_MS = 1000
 
 MAX_ID_RANGE = 252
 
-# The following bounds define the lower and upper joints range (after calibration).
-# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
-# which corresponds to a half rotation on the left and half rotation on the right.
-# Some joints might require higher range, so we allow up to [-270, 270] degrees until
-# an error is raised.
-LOWER_BOUND_DEGREE = -180
-UPPER_BOUND_DEGREE = 180
 # For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
 # their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
 # closed, and 100% is fully open. To account for slight calibration issue, we allow up to
@@ -32,7 +25,6 @@ UPPER_BOUND_LINEAR = 110
 
 HALF_TURN_DEGREE = 180
 
-
 # See this link for STS3215 Memory Table:
 # https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
 # data_name: (address, size_byte)
@@ -119,25 +111,39 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: list[str]) -> np.ndarray:
+def convert_degrees_to_steps(
-    """This function converts the degree range to the step range for indicating motors rotation.
+    degrees: float | np.ndarray, models: list[str], multi_turn_index: int
-    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
+) -> np.ndarray:
-    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
     """
+    Converts degrees to motor steps with multi-turn tracking.
+    - Each full rotation (360°) corresponds to an additional 4096 steps.
+    """
+
     resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
-    steps = degrees / 180 * np.array(resolutions) / 2
+
-    steps = steps.astype(int)
+    # Remove full rotations from degrees
-    return steps
+    base_degrees = degrees - (multi_turn_index * 360.0)
+
+    # Convert degrees to motor steps
+    steps = base_degrees / 180.0 * (resolutions / 2)
+
+    # Add back multi-turn steps
+    steps += multi_turn_index * resolutions
+
+    return steps.astype(int)
 
 
 def convert_steps_to_degrees(steps: int | np.ndarray, models: list[str]) -> np.ndarray:
-    """This function converts the step range to the degrees range for indicating motors rotation.
+    """
-    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
+    Converts motor steps to degrees while accounting for multi-turn motion.
-    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
+    - Each full rotation (4096 steps) adds ±360°.
     """
     resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = np.array(steps, dtype=float)
+
+    # Convert steps to degrees
     degrees = steps * (360.0 / resolutions)
+
     return degrees
 
 
@@ -145,38 +151,36 @@ def adjusted_to_homing_ticks(
     raw_motor_ticks: int, encoder_offset: int, model: str, motorbus, motor_id: int
 ) -> int:
     """
-    Converts raw motor ticks [0..4095] to homed servo ticks in [-2048..2047].
+    Converts raw motor ticks [0..4095] to homed servo ticks with multi-turn indexing.
-    Tracks multi-turn rotations by adjusting a multi-turn index.
+    - Uses a rolling index to track full rotations (4096 steps each)
     """
 
     resolutions = MODEL_RESOLUTION[model]
-    # Retrieve "previous shifted" and the multi-turn index
+
+    # Retrieve previous values for tracking
     prev_value = motorbus.previous_value[motor_id - 1]
     multi_turn_index = motorbus.multi_turn_index[motor_id - 1]
 
-    # Compute the new shifted value in the range [-2048..2047]
+    # Compute new shifted position
     shifted = (raw_motor_ticks + encoder_offset) % resolutions
-    if shifted > 2047:
+    if shifted > resolutions // 2:
-        shifted -= 4096
+        shifted -= resolutions  # Normalize to [-2048, 2047]
 
-    # If we have a valid previous value, detect big jumps
     if prev_value is not None:
         delta = shifted - prev_value
 
-        # If we jump forward by more than half a turn, we must have wrapped negatively
+        # If jump forward > 180° (2048 steps), assume full rotation
-        if delta > 2048:
+        if delta > resolutions // 2:
             multi_turn_index -= 1
-
+        elif delta < -resolutions // 2:
-        # If we jump backward by more than half a turn, we must have wrapped positively
-        elif delta < -2048:
             multi_turn_index += 1
 
-    # Store the new shifted value and updated multi-turn index
+    # Update stored values
     motorbus.previous_value[motor_id - 1] = shifted
     motorbus.multi_turn_index[motor_id - 1] = multi_turn_index
 
-    # Return the final multi-turn adjusted position
+    # Return final adjusted ticks with multi-turn indexing
-    return shifted + multi_turn_index * 4096
+    return shifted + (multi_turn_index * resolutions)
 
 
 def adjusted_to_motor_ticks(
@@ -485,22 +489,10 @@ class FeetechMotorsBus:
                 if drive_mode:
                     values[i] *= -1
 
-                if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
-                        f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
-                        f"but present value is {values[i]} degree. "
-                        "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
             elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
                 start_pos = self.calibration["start_pos"][calib_idx]
                 end_pos = self.calibration["end_pos"][calib_idx]
 
-                # TODO(pepijn): Check if a homing (something similar to homing) should be applied to linear joint (think so)
-
                 # Rescale the present position to a nominal range [0, 100] %,
                 # useful for joints with linear motions like Aloha gripper
                 values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
@@ -532,7 +524,7 @@ class FeetechMotorsBus:
                 motor_idx, model = self.motors[name]
 
                 # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_steps(values[i], [model])
+                values[i] = convert_degrees_to_steps(values[i], [model], self.multi_turn_index[motor_idx - 1])
                 values[i] = adjusted_to_motor_ticks(values[i], homing_offset, model, self, motor_idx)
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to

lerobot/common/robot_devices/robots/feetech_calibration.py

@@ -121,7 +121,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     print("\nMove arm to homing position")
     print(
         "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
-    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center)
+    )  # TODO(pepijn): replace with new instruction homing pos (all motors in center) in tutorial
     input("Press Enter to continue...")
 
     start_positions = np.zeros(len(arm.motor_indices))
@@ -158,7 +158,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
 
 
 def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """TODO: Add this method later as extra
+    """TODO(pepijn): Add this method later as extra
     Example of usage:
     ```python
     run_full_auto_arm_calibration(arm, "so100", "left", "follower")

lerobot/scripts/calibration_visualization.py

@@ -0,0 +1,121 @@
+import argparse
+import json
+import time
+from pathlib import Path
+
+from lerobot.common.robot_devices.motors.feetech import (
+    FeetechMotorsBus,
+    adjusted_to_homing_ticks,
+    adjusted_to_motor_ticks,
+    convert_steps_to_degrees,
+)
+
+# Replace this import with your real config class import
+# (e.g., So100RobotConfig or any other)
+from lerobot.common.robot_devices.robots.configs import So100RobotConfig
+from lerobot.common.robot_devices.robots.utils import make_robot_from_config
+
+
+def debug_feetech_positions(cfg, arm_arg: str):
+    """
+    Reads each joint’s (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
+
+    :param cfg: A config object (e.g. So100RobotConfig).
+    :param arm_arg: One of "main_leader" or "main_follower".
+    """
+    # 1) Make the Robot from your config
+    #    e.g. `So100RobotConfig` might already be the "robot object",
+    #    or if it is purely a config structure, do: robot = make_robot_from_config(cfg).
+    robot = make_robot_from_config(cfg)
+
+    # 2) Parse which arm we want: 'main_leader' or 'main_follower'
+    if arm_arg not in ("main_leader", "main_follower"):
+        raise ValueError("Please specify --arm=main_leader or --arm=main_follower")
+
+    bus_config = robot.leader_arms["main"] if arm_arg == "main_leader" else robot.follower_arms["main"]
+
+    # 3) Create the Feetech bus from that config
+    bus = FeetechMotorsBus(bus_config)
+
+    # 4) (Optional) Load calibration if it exists
+    calib_file = Path(robot.calibration_dir) / f"{arm_arg}.json"
+    if calib_file.exists():
+        with open(calib_file) as f:
+            calibration_dict = json.load(f)
+        bus.set_calibration(calibration_dict)
+        print(f"Loaded calibration from {calib_file}")
+    else:
+        print(f"No calibration file found at {calib_file}, skipping calibration set.")
+
+    # 5) Connect to the bus
+    bus.connect()
+    print(f"Connected to Feetech bus on port: {bus.port}")
+
+    # 6) Disable torque on all motors so you can move them freely by hand
+    bus.write("Torque_Enable", 0, motor_names=bus.motor_names)
+    print("Torque disabled on all joints.")
+
+    try:
+        print("\nPress Ctrl+C to quit.\n")
+        while True:
+            # (a) Read *raw* positions (no calibration)
+            raw_positions = bus.read_with_motor_ids(
+                bus.motor_models, bus.motor_indices, data_name="Present_Position"
+            )
+
+            # (b) Read *already-homed* positions (calibration is applied automatically)
+            homed_positions = bus.read("Present_Position")
+
+            # Print them side by side
+            for i, name in enumerate(bus.motor_names):
+                motor_idx, model = bus.motors[name]
+
+                raw_ticks = raw_positions[i]  # 0..4095
+                homed_val = homed_positions[i]  # degrees or % if linear
+
+                # If you want to see how offset is used inside bus.read(), do it manually:
+                offset = 0
+                if bus.calibration and name in bus.calibration["motor_names"]:
+                    offset_idx = bus.calibration["motor_names"].index(name)
+                    offset = bus.calibration["homing_offset"][offset_idx]
+
+                # Manually compute "adjusted ticks" from raw ticks
+                manual_adjusted = adjusted_to_homing_ticks(raw_ticks, offset, model, bus, motor_idx)
+                # Convert to degrees
+                manual_degs = convert_steps_to_degrees(manual_adjusted, [model])[0]
+                # Invert
+                inv_ticks = adjusted_to_motor_ticks(manual_adjusted, offset, model, bus, motor_idx)
+
+                print(
+                    f"{name:15s} | "
+                    f"RAW={raw_ticks:4d} | "
+                    f"HOMED={homed_val:7.2f} | "
+                    f"MANUAL_ADJ={manual_adjusted:6d} | "
+                    f"DEG={manual_degs:7.2f} | "
+                    f"INV={inv_ticks:4d}"
+                )
+            print("----------------------------------------------------")
+            time.sleep(0.25)  # slow down loop
+    except KeyboardInterrupt:
+        pass
+    finally:
+        print("\nExiting. Disconnecting bus...")
+        bus.disconnect()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser(description="Debug Feetech positions.")
+    parser.add_argument(
+        "--arm",
+        type=str,
+        choices=["main_leader", "main_follower"],
+        default="main_leader",
+        help="Which arm to debug: 'main_leader' or 'main_follower'.",
+    )
+    args = parser.parse_args()
+
+    # 1) Instantiate the config you want (So100RobotConfig, or whichever your project uses).
+    cfg = So100RobotConfig()
+
+    # 2) Call the function with (cfg, args.arm)
+    debug_feetech_positions(cfg, arm_arg=args.arm)

lerobot/scripts/configure_motor.py

@@ -141,10 +141,9 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
             motor_bus.write("Lock", 1)
             print("Offset", motor_bus.read("Offset"))
 
-            # TODO(pepijn):
+            # TODO(pepijn): Add when doing a motor configuration, you instantainesly home the position (while assembling)
             # single_calibration = single_motor_calibration(motor_bus, motor_idx_des)
-
+            # Then store single_calibration in yaml via dict (not overwrite but add the single calibration each time for motor id)
-            # TODO(pepijn): store single_calibration
 
     except Exception as e:
         print(f"Error occurred during motor configuration: {e}")

lerobot/scripts/test_homing.py

@@ -1,112 +0,0 @@
-from lerobot.common.robot_devices.motors.feetech import (
-    adjusted_to_homing_ticks,
-    adjusted_to_motor_ticks,
-    convert_steps_to_degrees,
-)
-
-# TODO(pepijn): remove this file!
-
-MODEL_RESOLUTION = {
-    "scs_series": 4096,
-    "sts3215": 4096,
-}
-
-
-def calibrate_homing_motor(motor_id, motor_bus):
-    """
-    1) Reads servo ticks.
-    2) Calculates the offset so that 'home_ticks' becomes 0.
-    3) Returns the offset
-    """
-
-    home_ticks = motor_bus.read("Present_Position")[0]  # Read index starts at 0
-
-    # Calculate how many ticks to shift so that 'home_ticks' becomes 0
-    raw_offset = -home_ticks  # negative of home_ticks
-    encoder_offset = raw_offset % 4096  # wrap to [0..4095]
-
-    # Convert to a signed range [-2048..2047]
-    if encoder_offset > 2047:
-        encoder_offset -= 4096
-
-    print(f"Encoder offset: {encoder_offset}")
-
-    return encoder_offset
-
-
-def configure_and_calibrate():
-    from lerobot.common.robot_devices.motors.feetech import FeetechMotorsBus as MotorsBusClass
-
-    motor_idx_des = 2  # index of motor
-
-    # Setup motor names, indices, and models
-    motor_name = "motor"
-    motor_index_arbitrary = motor_idx_des  # Use the motor ID passed via argument
-    motor_model = "sts3215"  # Use the motor model passed via argument
-
-    # Initialize the MotorBus with the correct port and motor configurations
-    motor_bus = MotorsBusClass(
-        port="/dev/tty.usbmodem58760431631", motors={motor_name: (motor_index_arbitrary, motor_model)}
-    )
-
-    # Try to connect to the motor bus and handle any connection-specific errors
-    try:
-        motor_bus.connect()
-        print(f"Connected on port {motor_bus.port}")
-    except OSError as e:
-        print(f"Error occurred when connecting to the motor bus: {e}")
-        return
-
-    present_idx = motor_bus.read_with_motor_ids(motor_bus.motor_models, motor_idx_des, "ID", num_retry=2)
-    if present_idx != motor_idx_des:
-        raise OSError("Failed to write index.")
-
-    # Set Maximum_Acceleration to 254 to speedup acceleration and deceleration of
-    # the motors. Note: this configuration is not in the official STS3215 Memory Table
-    motor_bus.write("Lock", 0)
-    motor_bus.write("Maximum_Acceleration", 254)
-
-    motor_bus.write("Max_Angle_Limit", 4095)  # default 4095
-    motor_bus.write("Min_Angle_Limit", 0)  # default 0
-    motor_bus.write("Offset", 0)
-    motor_bus.write("Mode", 0)
-    motor_bus.write("Goal_Position", 0)
-    motor_bus.write("Torque_Enable", 0)
-
-    motor_bus.write("Lock", 1)
-
-    # Calibration
-    print("Offset", motor_bus.read("Offset"))
-    print("Max_Angle_Limit", motor_bus.read("Max_Angle_Limit"))
-    print("Min_Angle_Limit", motor_bus.read("Min_Angle_Limit"))
-    print("Goal_Position", motor_bus.read("Goal_Position"))
-    print("Present Position", motor_bus.read("Present_Position"))
-
-    input("Move the motor to middle (home) position, then press Enter...")
-    encoder_offset = calibrate_homing_motor(motor_idx_des, motor_bus)
-
-    try:
-        while True:
-            name = motor_bus.motor_names[0]
-            _, model = motor_bus.motors[name]
-            raw_motor_ticks = motor_bus.read("Present_Position")[0]
-            adjusted_ticks = adjusted_to_homing_ticks(raw_motor_ticks, encoder_offset, model, motor_bus, 1)
-            inverted_ticks = adjusted_to_motor_ticks(adjusted_ticks, encoder_offset, model, motor_bus, 1)
-            adjusted_degrees = convert_steps_to_degrees([adjusted_ticks], [model])
-            print(
-                f"Raw Motor ticks: {raw_motor_ticks} | Adjusted ticks: {adjusted_ticks} | Adjusted degrees: {adjusted_degrees} | Invert adjusted ticks: {inverted_ticks}"
-            )
-            # time.sleep(0.3)
-    except KeyboardInterrupt:
-        print("Stopped reading positions.")
-
-    except Exception as e:
-        print(f"Error occurred during motor configuration: {e}")
-
-    finally:
-        motor_bus.disconnect()
-        print("Disconnected from motor bus.")
-
-
-if __name__ == "__main__":
-    configure_and_calibrate()