change manual motor calib

.cache/calibration/so100/main_follower.json

@@ -0,0 +1 @@
+{"homing_offset": [-1985, 1915, 1984, 2008, 1765, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1007], "end_pos": [0, 0, 0, 0, 0, 2448], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}

.cache/calibration/so100/main_leader.json

@@ -0,0 +1 @@
+{"homing_offset": [1433, 4, 1917, -263, 972, 0], "drive_mode": [0, 0, 0, 0, 0, 0], "start_pos": [0, 0, 0, 0, 0, 1534], "end_pos": [0, 0, 0, 0, 0, 2590], "calib_mode": ["DEGREE", "DEGREE", "DEGREE", "DEGREE", "DEGREE", "LINEAR"], "motor_names": ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]}

lerobot/common/robot_devices/motors/feetech.py

@@ -20,8 +20,8 @@ MAX_ID_RANGE = 252
 # 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 = -270
+LOWER_BOUND_DEGREE = -180
-UPPER_BOUND_DEGREE = 270
+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
@@ -116,28 +116,59 @@ NUM_READ_RETRY = 20
 NUM_WRITE_RETRY = 20
 
 
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
+def convert_degrees_to_steps(degrees: float | np.ndarray, models: list[str]) -> np.ndarray:
     """This function converts the degree range to the step range for indicating motors rotation.
     It assumes a motor achieves a full rotation by going from -180 degree position to +180.
     The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
     """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = degrees / 180 * np.array(resolutions) / 2
     steps = steps.astype(int)
     return steps
 
 
-def convert_steps_to_degrees(steps: int | np.ndarray, models: str | list[str]) -> np.ndarray:
+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.
     The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
     """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
     steps = np.array(steps, dtype=float)
     degrees = steps * (360.0 / resolutions)
     return degrees
 
 
+def adjusted__to_homing_ticks(
+    raw_motor_ticks: int | np.ndarray, encoder_offset: int, models: list[str]
+) -> np.ndarray:
+    """
+    raw_motor_ticks : int in [0..4095]  (the homed, shifted value)
+    encoder_offset : int (the offset computed so that `home` becomes zero)
+    Returns the homed servo ticks in [-2048 .. +2047].
+    """
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
+    raw_motor_ticks = np.asarray(raw_motor_ticks)
+    shifted = (raw_motor_ticks + encoder_offset) % resolutions
+    shifted = np.where(shifted > 2047, shifted - 4096, shifted)
+    return shifted
+
+
+def adjusted_to_motor_ticks(
+    adjusted_pos: int | np.ndarray, encoder_offset: int, models: list[str]
+) -> np.ndarray:
+    """
+    Inverse of read_adjusted_position().
+    adjusted_pos : int in [-2048 .. +2047]  (the homed, shifted value)
+    encoder_offset : int (the offset computed so that `home` becomes zero)
+    Returns the raw servo ticks in [0..4095].
+    """
+    resolutions = np.array([MODEL_RESOLUTION[m] for m in models], dtype=float)
+    adjusted_pos = np.asarray(adjusted_pos)
+    adjusted_pos = np.where(adjusted_pos < 0, adjusted_pos + 4096, adjusted_pos)
+    raw_ticks = (adjusted_pos - encoder_offset) % resolutions
+    return raw_ticks
+
+
 def convert_to_bytes(value, bytes, mock=False):
     if mock:
         return value
@@ -415,7 +446,10 @@ class FeetechMotorsBus:
                 drive_mode = self.calibration["drive_mode"][calib_idx]
                 homing_offset = self.calibration["homing_offset"][calib_idx]
                 _, model = self.motors[name]
-                resolution = self.model_resolution[model]
+
+                # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
+                values[i] = adjusted__to_homing_ticks(values[i], homing_offset, [model])
+                values[i] = convert_steps_to_degrees(values[i], [model])
 
                 # Update direction of rotation of the motor to match between leader and follower.
                 # In fact, the motor of the leader for a given joint can be assembled in an
@@ -423,14 +457,6 @@ class FeetechMotorsBus:
                 if drive_mode:
                     values[i] *= -1
 
-                # Convert from range [-2**31, 2**31[ to
-                # nominal range ]-resolution, resolution[ (e.g. ]-2048, 2048[)
-                values[i] += homing_offset
-
-                # Convert from range ]-resolution, resolution[ to
-                # universal float32 centered degree range ]-180, 180[
-                values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
-
                 if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
                     raise JointOutOfRangeError(
                         f"Wrong motor position range detected for {name}. "
@@ -474,15 +500,10 @@ class FeetechMotorsBus:
                 drive_mode = self.calibration["drive_mode"][calib_idx]
                 homing_offset = self.calibration["homing_offset"][calib_idx]
                 _, model = self.motors[name]
-                resolution = self.model_resolution[model]
 
-                # Convert from nominal 0-centered degree range [-180, 180] to
+                # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
+                values[i] = convert_degrees_to_steps(values[i], [model])
-                values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)
+                values[i] = adjusted_to_motor_ticks(values[i], homing_offset, [model])
-
-                # Substract the homing offsets to come back to actual motor range of values
-                # which can be arbitrary.
-                values[i] -= homing_offset
 
                 # Remove drive mode, which is the rotation direction of the motor, to come back to
                 # actual motor rotation direction which can be arbitrary.
@@ -598,7 +619,8 @@ class FeetechMotorsBus:
 
         values = np.array(values)
 
-        # TODO: Apply calibration and homign offset, output is homeing = 0 and direction.
+        if self.calibration is not None:
+            values = self.apply_calibration(values, motor_names)
 
         # log the number of seconds it took to read the data from the motors
         delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
@@ -670,7 +692,8 @@ class FeetechMotorsBus:
             motor_ids.append(motor_idx)
             models.append(model)
 
-        # TODO: add invesre of apply homing to go back to motor ticks
+        if self.calibration is not None:
+            values = self.revert_calibration(values, motor_names)
 
         values = values.tolist()
 

lerobot/common/robot_devices/robots/feetech_calibration.py

@@ -1,14 +1,11 @@
 """Logic to calibrate a robot arm built with feetech motors"""
 # TODO(rcadene, aliberts): move this logic into the robot code when refactoring
 
-import time
-
 import numpy as np
 
 from lerobot.common.robot_devices.motors.feetech import (
     CalibrationMode,
     TorqueMode,
-    convert_degrees_to_steps,
 )
 from lerobot.common.robot_devices.motors.utils import MotorsBus
 
@@ -16,469 +13,149 @@ URL_TEMPLATE = (
     "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
 )
 
-# The following positions are provided in nominal degree range ]-180, +180[
-# For more info on these constants, see comments in the code where they get used.
-ZERO_POSITION_DEGREE = 0
-ROTATED_POSITION_DEGREE = 90
 
-
+def disable_torque(arm: MotorsBus):
-def assert_drive_mode(drive_mode):
-    # `drive_mode` is in [0,1] with 0 means original rotation direction for the motor, and 1 means inverted.
-    if not np.all(np.isin(drive_mode, [0, 1])):
-        raise ValueError(f"`drive_mode` contains values other than 0 or 1: ({drive_mode})")
-
-
-def apply_drive_mode(position, drive_mode):
-    assert_drive_mode(drive_mode)
-    # Convert `drive_mode` from [0, 1] with 0 indicates original rotation direction and 1 inverted,
-    # to [-1, 1] with 1 indicates original rotation direction and -1 inverted.
-    signed_drive_mode = -(drive_mode * 2 - 1)
-    position *= signed_drive_mode
-    return position
-
-
-def move_until_block(arm, motor_name, positive_direction=True, while_move_hook=None):
-    count = 0
-    while True:
-        present_pos = arm.read("Present_Position", motor_name)
-        if positive_direction:
-            # Move +100 steps every time. Lower the steps to lower the speed at which the arm moves.
-            arm.write("Goal_Position", present_pos + 100, motor_name)
-        else:
-            arm.write("Goal_Position", present_pos - 100, motor_name)
-
-        if while_move_hook is not None:
-            while_move_hook()
-
-        present_pos = arm.read("Present_Position", motor_name).item()
-        present_speed = arm.read("Present_Speed", motor_name).item()
-        present_current = arm.read("Present_Current", motor_name).item()
-        # present_load = arm.read("Present_Load", motor_name).item()
-        # present_voltage = arm.read("Present_Voltage", motor_name).item()
-        # present_temperature = arm.read("Present_Temperature", motor_name).item()
-
-        # print(f"{present_pos=}")
-        # print(f"{present_speed=}")
-        # print(f"{present_current=}")
-        # print(f"{present_load=}")
-        # print(f"{present_voltage=}")
-        # print(f"{present_temperature=}")
-
-        if present_speed == 0 and present_current > 40:
-            count += 1
-            if count > 100 or present_current > 300:
-                return present_pos
-        else:
-            count = 0
-
-
-def move_to_calibrate(
-    arm,
-    motor_name,
-    invert_drive_mode=False,
-    positive_first=True,
-    in_between_move_hook=None,
-    while_move_hook=None,
-):
-    initial_pos = arm.read("Present_Position", motor_name)
-
-    if positive_first:
-        p_present_pos = move_until_block(
-            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
-        )
-    else:
-        n_present_pos = move_until_block(
-            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
-        )
-
-    if in_between_move_hook is not None:
-        in_between_move_hook()
-
-    if positive_first:
-        n_present_pos = move_until_block(
-            arm, motor_name, positive_direction=False, while_move_hook=while_move_hook
-        )
-    else:
-        p_present_pos = move_until_block(
-            arm, motor_name, positive_direction=True, while_move_hook=while_move_hook
-        )
-
-    zero_pos = (n_present_pos + p_present_pos) / 2
-
-    calib_data = {
-        "initial_pos": initial_pos,
-        "homing_offset": zero_pos if invert_drive_mode else -zero_pos,
-        "invert_drive_mode": invert_drive_mode,
-        "drive_mode": -1 if invert_drive_mode else 0,
-        "zero_pos": zero_pos,
-        "start_pos": n_present_pos if invert_drive_mode else p_present_pos,
-        "end_pos": p_present_pos if invert_drive_mode else n_present_pos,
-    }
-    return calib_data
-
-
-def apply_offset(calib, offset):
-    calib["zero_pos"] += offset
-    if calib["drive_mode"]:
-        calib["homing_offset"] += offset
-    else:
-        calib["homing_offset"] -= offset
-    return calib
-
-
-def run_arm_auto_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    if robot_type == "so100":
-        return run_arm_auto_calibration_so100(arm, robot_type, arm_name, arm_type)
-    elif robot_type == "moss":
-        return run_arm_auto_calibration_moss(arm, robot_type, arm_name, arm_type)
-    else:
-        raise ValueError(robot_type)
-
-
-def run_arm_auto_calibration_so100(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
     if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
         raise ValueError("To run calibration, the torque must be disabled on all motors.")
 
-    if not (robot_type == "so100" and arm_type == "follower"):
-        raise NotImplementedError("Auto calibration only supports the follower of so100 arms for now.")
 
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
+def get_calibration_modes(arm: MotorsBus):
+    """Returns calibration modes for each motor (DEGREE for rotational, LINEAR for gripper)."""
+    return [
+        CalibrationMode.LINEAR.name if name == "gripper" else CalibrationMode.DEGREE.name
+        for name in arm.motor_names
+    ]
 
-    print("\nMove arm to initial position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
 
-    # Lower the acceleration of the motors (in [0,254])
+def calibrate_homing_motor(motor_id, motor_bus):
-    initial_acceleration = arm.read("Acceleration")
+    """
-    arm.write("Lock", 0)
+    1) Reads servo ticks.
-    arm.write("Acceleration", 10)
+    2) Calculates the offset so that 'home_ticks' becomes 0.
-    time.sleep(1)
+    3) Returns the offset
+    """
 
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
+    home_ticks = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts at 0
 
-    print(f'{arm.read("Present_Position", "elbow_flex")=}')
+    # 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]
 
-    calib = {}
+    # Convert to a signed range [-2048..2047]
+    if encoder_offset > 2047:
+        encoder_offset -= 4096
 
-    init_wf_pos = arm.read("Present_Position", "wrist_flex")
+    print(f"Encoder offset: {encoder_offset}")
-    init_sl_pos = arm.read("Present_Position", "shoulder_lift")
-    init_ef_pos = arm.read("Present_Position", "elbow_flex")
-    arm.write("Goal_Position", init_wf_pos - 800, "wrist_flex")
-    arm.write("Goal_Position", init_sl_pos + 150 + 1024, "shoulder_lift")
-    arm.write("Goal_Position", init_ef_pos - 2048, "elbow_flex")
-    time.sleep(2)
 
-    print("Calibrate shoulder_pan")
+    return encoder_offset
-    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
 
-    print("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    time.sleep(1)
 
-    print("Calibrate wrist_flex")
+def calibrate_linear_motor(motor_id, motor_bus):
-    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex")
+    motor_names = motor_bus.motor_names
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80)
+    motor_name = motor_names[motor_id - 1]
 
-    def in_between_move_hook():
+    input(f"Close the {motor_name}, then press Enter...")
-        nonlocal arm, calib
+    start_pos = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts ar 0
-        time.sleep(2)
+    print(f"  [Motor {motor_id}] start position recorded: {start_pos}")
-        ef_pos = arm.read("Present_Position", "elbow_flex")
-        sl_pos = arm.read("Present_Position", "shoulder_lift")
-        arm.write("Goal_Position", ef_pos + 1024, "elbow_flex")
-        arm.write("Goal_Position", sl_pos - 1024, "shoulder_lift")
-        time.sleep(2)
 
-    print("Calibrate elbow_flex")
+    input("Open the {motor_name} fully, then press Enter...")
-    calib["elbow_flex"] = move_to_calibrate(
+    end_pos = motor_bus.read("Present_Position")[motor_id - 1]  # Read index starts ar 0
-        arm, "elbow_flex", positive_first=False, in_between_move_hook=in_between_move_hook
+    print(f"  [Motor {motor_id}] end position recorded: {end_pos}")
-    )
-    calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=80 - 1024)
 
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex")
+    return start_pos, end_pos
-    time.sleep(1)
 
-    def in_between_move_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex")
 
-    print("Calibrate shoulder_lift")
+def single_motor_calibration(arm: MotorsBus, motor_id: int):
-    calib["shoulder_lift"] = move_to_calibrate(
+    """Calibrates a single motor and returns its calibration data for updating the calibration file."""
-        arm,
-        "shoulder_lift",
-        invert_drive_mode=True,
-        positive_first=False,
-        in_between_move_hook=in_between_move_hook,
-    )
-    # add an 30 steps as offset to align with body
-    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=1024 - 50)
 
-    def while_move_hook():
+    disable_torque(arm)
-        nonlocal arm, calib
+    print(f"\n--- Calibrating Motor {motor_id} ---")
-        positions = {
-            "shoulder_lift": round(calib["shoulder_lift"]["zero_pos"] - 1600),
-            "elbow_flex": round(calib["elbow_flex"]["zero_pos"] + 1700),
-            "wrist_flex": round(calib["wrist_flex"]["zero_pos"] + 800),
-            "gripper": round(calib["gripper"]["end_pos"]),
-        }
-        arm.write("Goal_Position", list(positions.values()), list(positions.keys()))
 
-    arm.write("Goal_Position", round(calib["shoulder_lift"]["zero_pos"] - 1600), "shoulder_lift")
+    start_pos = 0
-    time.sleep(2)
+    end_pos = 0
-    arm.write("Goal_Position", round(calib["elbow_flex"]["zero_pos"] + 1700), "elbow_flex")
+    encoder_offset = 0
-    time.sleep(2)
-    arm.write("Goal_Position", round(calib["wrist_flex"]["zero_pos"] + 800), "wrist_flex")
-    time.sleep(2)
-    arm.write("Goal_Position", round(calib["gripper"]["end_pos"]), "gripper")
-    time.sleep(2)
 
-    print("Calibrate wrist_roll")
+    if motor_id == 6:
-    calib["wrist_roll"] = move_to_calibrate(
+        start_pos, end_pos = calibrate_linear_motor(motor_id, arm)
-        arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook
+    else:
-    )
+        input("Move the motor to middle (home) position, then press Enter...")
+        encoder_offset = calibrate_homing_motor(motor_id, arm)
 
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
+    print(f"Calibration for motor ID:{motor_id} done.")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 2048, "elbow_flex")
-    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] - 2048, "shoulder_lift")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
-
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
 
+    # Create a calibration dictionary for the single motor
     calib_dict = {
-        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
+        "homing_offset": int(encoder_offset),
-        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
+        "drive_mode": 0,
-        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
+        "start_pos": int(start_pos),
-        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
+        "end_pos": int(end_pos),
-        "calib_mode": calib_modes,
+        "calib_mode": get_calibration_modes(arm)[motor_id - 1],
-        "motor_names": arm.motor_names,
+        "motor_name": arm.motor_names[motor_id - 1],
     }
 
-    # Re-enable original accerlation
-    arm.write("Lock", 0)
-    arm.write("Acceleration", initial_acceleration)
-    time.sleep(1)
-
     return calib_dict
 
 
-def run_arm_auto_calibration_moss(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """All the offsets and magic numbers are hand tuned, and are unique to SO-100 follower arms"""
+    """
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
+    Runs a full calibration process for all motors in a robotic arm.
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
 
-    if not (robot_type == "moss" and arm_type == "follower"):
+    This function calibrates each motor in the arm, determining encoder offsets and
-        raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.")
+    start/end positions for linear and rotational motors. The calibration data is then
+    stored in a dictionary for later use.
 
-    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
+    **Calibration Process:**
+    - The user is prompted to move the arm to its homing position before starting.
+    - Motors with rotational motion are calibrated using a homing method.
+    - Linear actuators (e.g., grippers) are calibrated separately.
+    - Encoder offsets, start positions, and end positions are recorded.
 
-    print("\nMove arm to initial position")
+    **Example Usage:**
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="initial"))
-    input("Press Enter to continue...")
-
-    # Lower the acceleration of the motors (in [0,254])
-    initial_acceleration = arm.read("Acceleration")
-    arm.write("Lock", 0)
-    arm.write("Acceleration", 10)
-    time.sleep(1)
-
-    arm.write("Torque_Enable", TorqueMode.ENABLED.value)
-
-    sl_pos = arm.read("Present_Position", "shoulder_lift")
-    arm.write("Goal_Position", sl_pos - 1024 - 450, "shoulder_lift")
-    ef_pos = arm.read("Present_Position", "elbow_flex")
-    arm.write("Goal_Position", ef_pos + 1024 + 450, "elbow_flex")
-    time.sleep(2)
-
-    calib = {}
-
-    print("Calibrate shoulder_pan")
-    calib["shoulder_pan"] = move_to_calibrate(arm, "shoulder_pan")
-    arm.write("Goal_Position", calib["shoulder_pan"]["zero_pos"], "shoulder_pan")
-    time.sleep(1)
-
-    print("Calibrate gripper")
-    calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True)
-    time.sleep(1)
-
-    print("Calibrate wrist_flex")
-    calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex", invert_drive_mode=True)
-    calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=-210 + 1024)
-
-    wr_pos = arm.read("Present_Position", "wrist_roll")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", wr_pos - 1024, "wrist_roll")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["gripper"]["end_pos"], "gripper")
-    time.sleep(1)
-
-    print("Calibrate wrist_roll")
-    calib["wrist_roll"] = move_to_calibrate(arm, "wrist_roll", invert_drive_mode=True)
-    calib["wrist_roll"] = apply_offset(calib["wrist_roll"], offset=790)
-
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"] - 1024, "wrist_roll")
-    arm.write("Goal_Position", calib["gripper"]["start_pos"], "gripper")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll")
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 2048, "wrist_flex")
-
-    def in_between_move_elbow_flex_hook():
-        nonlocal arm, calib
-        arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex")
-
-    print("Calibrate elbow_flex")
-    calib["elbow_flex"] = move_to_calibrate(
-        arm,
-        "elbow_flex",
-        invert_drive_mode=True,
-        in_between_move_hook=in_between_move_elbow_flex_hook,
-    )
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-
-    def in_between_move_shoulder_lift_hook():
-        nonlocal arm, calib
-        sl = arm.read("Present_Position", "shoulder_lift")
-        arm.write("Goal_Position", sl - 1500, "shoulder_lift")
-        time.sleep(1)
-        arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1536, "elbow_flex")
-        time.sleep(1)
-        arm.write("Goal_Position", calib["wrist_flex"]["start_pos"], "wrist_flex")
-        time.sleep(1)
-
-    print("Calibrate shoulder_lift")
-    calib["shoulder_lift"] = move_to_calibrate(
-        arm, "shoulder_lift", in_between_move_hook=in_between_move_shoulder_lift_hook
-    )
-    calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=-1024)
-
-    arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1024, "wrist_flex")
-    time.sleep(1)
-    arm.write("Goal_Position", calib["shoulder_lift"]["zero_pos"] + 2048, "shoulder_lift")
-    arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] - 1024 - 400, "elbow_flex")
-    time.sleep(2)
-
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
-        else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
-
-    calib_dict = {
-        "homing_offset": [calib[name]["homing_offset"] for name in arm.motor_names],
-        "drive_mode": [calib[name]["drive_mode"] for name in arm.motor_names],
-        "start_pos": [calib[name]["start_pos"] for name in arm.motor_names],
-        "end_pos": [calib[name]["end_pos"] for name in arm.motor_names],
-        "calib_mode": calib_modes,
-        "motor_names": arm.motor_names,
-    }
-
-    # Re-enable original accerlation
-    arm.write("Lock", 0)
-    arm.write("Acceleration", initial_acceleration)
-    time.sleep(1)
-
-    return calib_dict
-
-
-def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
-    """This function ensures that a neural network trained on data collected on a given robot
-    can work on another robot. For instance before calibration, setting a same goal position
-    for each motor of two different robots will get two very different positions. But after calibration,
-    the two robots will move to the same position.To this end, this function computes the homing offset
-    and the drive mode for each motor of a given robot.
-
-    Homing offset is used to shift the motor position to a ]-2048, +2048[ nominal range (when the motor uses 2048 steps
-    to complete a half a turn). This range is set around an arbitrary "zero position" corresponding to all motor positions
-    being 0. During the calibration process, you will need to manually move the robot to this "zero position".
-
-    Drive mode is used to invert the rotation direction of the motor. This is useful when some motors have been assembled
-    in the opposite orientation for some robots. During the calibration process, you will need to manually move the robot
-    to the "rotated position".
-
-    After calibration, the homing offsets and drive modes are stored in a cache.
-
-    Example of usage:
     ```python
-    run_arm_calibration(arm, "so100", "left", "follower")
+    run_full_arm_calibration(arm, "so100", "left", "follower")
     ```
     """
-    if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
+    disable_torque(arm)
-        raise ValueError("To run calibration, the torque must be disabled on all motors.")
 
     print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")
 
-    print("\nMove arm to zero position")
+    print("\nMove arm to homing position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero"))
+    print(
+        "See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")
+    )  # TODO: replace with new instruction homing pos (all motors in center)
     input("Press Enter to continue...")
 
-    # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
+    start_positions = np.zeros(len(arm.motor_indices))
-    # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
+    end_positions = np.zeros(len(arm.motor_indices))
-    # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
+    encoder_offsets = np.zeros(len(arm.motor_indices))
-    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
 
-    # Compute homing offset so that `present_position + homing_offset ~= target_position`.
+    # Call single motor calibration for each motor id
-    zero_pos = arm.read("Present_Position")
+    for i, id in enumerate(arm.motor_indices):
-    homing_offset = zero_target_pos - zero_pos
+        if id == 6:
-
+            start_positions[i], end_positions[i] = calibrate_linear_motor(id, arm)
-    # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
+            encoder_offsets[i] = 0
-    # This allows to identify the rotation direction of each motor.
-    # For instance, if the motor rotates 90 degree, and its value is -90 after applying the homing offset, then we know its rotation direction
-    # is inverted. However, for the calibration being successful, we need everyone to follow the same target position.
-    # Sometimes, there is only one possible rotation direction. For instance, if the gripper is closed, there is only one direction which
-    # corresponds to opening the gripper. When the rotation direction is ambiguous, we arbitrarely rotate clockwise from the point of view
-    # of the previous motor in the kinetic chain.
-    print("\nMove arm to rotated target position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
-    input("Press Enter to continue...")
-
-    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)
-
-    # Find drive mode by rotating each motor by a quarter of a turn.
-    # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
-    rotated_pos = arm.read("Present_Position")
-    drive_mode = (rotated_pos < zero_pos).astype(np.int32)
-
-    # Re-compute homing offset to take into account drive mode
-    rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
-    homing_offset = rotated_target_pos - rotated_drived_pos
-
-    print("\nMove arm to rest position")
-    print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rest"))
-    input("Press Enter to continue...")
-    print()
-
-    # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    calib_modes = []
-    for name in arm.motor_names:
-        if name == "gripper":
-            calib_modes.append(CalibrationMode.LINEAR.name)
         else:
-            calib_modes.append(CalibrationMode.DEGREE.name)
+            encoder_offsets[i] = calibrate_homing_motor(id, arm)
+            start_positions[i] = 0
+            end_positions[i] = 0
+
+    print(f"\n calibration of {robot_type} {arm_name} {arm_type} done!")
 
     calib_dict = {
-        "homing_offset": homing_offset.tolist(),
+        "homing_offset": encoder_offsets.astype(int).tolist(),
-        "drive_mode": drive_mode.tolist(),
+        "drive_mode": np.zeros(len(arm.motor_indices), dtype=int).tolist(),
-        "start_pos": zero_pos.tolist(),
+        "start_pos": start_positions.astype(int).tolist(),
-        "end_pos": rotated_pos.tolist(),
+        "end_pos": end_positions.astype(int).tolist(),
-        "calib_mode": calib_modes,
+        "calib_mode": get_calibration_modes(arm),
         "motor_names": arm.motor_names,
     }
     return calib_dict
+
+
+def run_full_auto_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str):
+    """TODO: Add this method later as extra
+    Example of usage:
+    ```python
+    run_full_auto_arm_calibration(arm, "so100", "left", "follower")
+    ```
+    """
+    print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...")

lerobot/common/robot_devices/robots/manipulator.py

@@ -374,10 +374,10 @@ class ManipulatorRobot:
 
                 elif self.robot_type in ["so100", "moss"]:
                     from lerobot.common.robot_devices.robots.feetech_calibration import (
-                        run_arm_manual_calibration,
+                        run_full_arm_calibration,
                     )
 
-                    calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type)
+                    calibration = run_full_arm_calibration(arm, self.robot_type, name, arm_type)
 
                 print(f"Calibration is done! Saving calibration file '{arm_calib_path}'")
                 arm_calib_path.parent.mkdir(parents=True, exist_ok=True)

lerobot/scripts/configure_motor.py

@@ -114,15 +114,20 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des):
             # 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("Goal_Position", 2048)
+            motor_bus.write("Min_Angle_Limit", 0)  # default 0
-            time.sleep(4)
-            print("Present Position", motor_bus.read("Present_Position"))
-
             motor_bus.write("Offset", 0)
-            time.sleep(4)
+            motor_bus.write("Mode", 0)
+            motor_bus.write("Goal_Position", 0)
+            motor_bus.write("Torque_Enable", 0)
+            motor_bus.write("Lock", 1)
             print("Offset", motor_bus.read("Offset"))
 
+            # TODO(pepijn):
+            # single_calibration = single_motor_calibration(motor_bus, motor_idx_des)
+
+            # TODO(pepijn): store single_calibration
+
     except Exception as e:
         print(f"Error occurred during motor configuration: {e}")