diff --git a/.cache/calibration/so100/main_follower.json b/.cache/calibration/so100/main_follower.json new file mode 100644 index 000000000..7279b79bc --- /dev/null +++ b/.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"]} diff --git a/.cache/calibration/so100/main_leader.json b/.cache/calibration/so100/main_leader.json new file mode 100644 index 000000000..70516c1c8 --- /dev/null +++ b/.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"]} diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 06589bd9e..219e5ab83 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/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 -UPPER_BOUND_DEGREE = 270 +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 @@ -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 - # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096) - values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2) - - # Substract the homing offsets to come back to actual motor range of values - # which can be arbitrary. - values[i] -= homing_offset + # Convert degrees to homed ticks, then convert the homed ticks to raw ticks + values[i] = convert_degrees_to_steps(values[i], [model]) + values[i] = adjusted_to_motor_ticks(values[i], homing_offset, [model]) # 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() diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index b015951a0..5816126e9 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/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 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""" +def disable_torque(arm: MotorsBus): 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]) - initial_acceleration = arm.read("Acceleration") - arm.write("Lock", 0) - arm.write("Acceleration", 10) - time.sleep(1) +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 + """ - 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") - 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(f"Encoder offset: {encoder_offset}") - 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) + return encoder_offset - 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") - calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80) +def calibrate_linear_motor(motor_id, motor_bus): + motor_names = motor_bus.motor_names + motor_name = motor_names[motor_id - 1] - def in_between_move_hook(): - nonlocal arm, calib - time.sleep(2) - 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) + input(f"Close the {motor_name}, then press Enter...") + start_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0 + print(f" [Motor {motor_id}] start position recorded: {start_pos}") - print("Calibrate elbow_flex") - calib["elbow_flex"] = move_to_calibrate( - arm, "elbow_flex", positive_first=False, in_between_move_hook=in_between_move_hook - ) - calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=80 - 1024) + input("Open the {motor_name} fully, then press Enter...") + end_pos = motor_bus.read("Present_Position")[motor_id - 1] # Read index starts ar 0 + print(f" [Motor {motor_id}] end position recorded: {end_pos}") - arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex") - time.sleep(1) + return start_pos, end_pos - def in_between_move_hook(): - nonlocal arm, calib - arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex") - print("Calibrate shoulder_lift") - calib["shoulder_lift"] = move_to_calibrate( - 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 single_motor_calibration(arm: MotorsBus, motor_id: int): + """Calibrates a single motor and returns its calibration data for updating the calibration file.""" - def while_move_hook(): - nonlocal arm, calib - 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())) + disable_torque(arm) + print(f"\n--- Calibrating Motor {motor_id} ---") - arm.write("Goal_Position", round(calib["shoulder_lift"]["zero_pos"] - 1600), "shoulder_lift") - time.sleep(2) - arm.write("Goal_Position", round(calib["elbow_flex"]["zero_pos"] + 1700), "elbow_flex") - 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) + start_pos = 0 + end_pos = 0 + encoder_offset = 0 - print("Calibrate wrist_roll") - calib["wrist_roll"] = move_to_calibrate( - arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook - ) + if motor_id == 6: + start_pos, end_pos = calibrate_linear_motor(motor_id, arm) + 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") - 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) + print(f"Calibration for motor ID:{motor_id} done.") + # Create a calibration dictionary for the single motor 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, + "homing_offset": int(encoder_offset), + "drive_mode": 0, + "start_pos": int(start_pos), + "end_pos": int(end_pos), + "calib_mode": get_calibration_modes(arm)[motor_id - 1], + "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): - """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.") +def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str): + """ + Runs a full calibration process for all motors in a robotic arm. - if not (robot_type == "moss" and arm_type == "follower"): - raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.") + This function calibrates each motor in the arm, determining encoder offsets and + 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") - 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: + **Example 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(): - raise ValueError("To run calibration, the torque must be disabled on all motors.") + disable_torque(arm) print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") - print("\nMove arm to zero position") - print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="zero")) + print("\nMove arm to homing position") + 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. - # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will - # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position. - zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models) + start_positions = np.zeros(len(arm.motor_indices)) + end_positions = np.zeros(len(arm.motor_indices)) + encoder_offsets = np.zeros(len(arm.motor_indices)) - # Compute homing offset so that `present_position + homing_offset ~= target_position`. - zero_pos = arm.read("Present_Position") - homing_offset = zero_target_pos - zero_pos - - # The rotated target position corresponds to a rotation of a quarter turn from the zero position. - # 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) + # Call single motor calibration for each motor id + for i, id in enumerate(arm.motor_indices): + if id == 6: + start_positions[i], end_positions[i] = calibrate_linear_motor(id, arm) + encoder_offsets[i] = 0 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(), - "drive_mode": drive_mode.tolist(), - "start_pos": zero_pos.tolist(), - "end_pos": rotated_pos.tolist(), - "calib_mode": calib_modes, + "homing_offset": encoder_offsets.astype(int).tolist(), + "drive_mode": np.zeros(len(arm.motor_indices), dtype=int).tolist(), + "start_pos": start_positions.astype(int).tolist(), + "end_pos": end_positions.astype(int).tolist(), + "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}...") diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 618105064..b4b9235f9 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/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) diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index 18707397f..1b51a50bf 100644 --- a/lerobot/scripts/configure_motor.py +++ b/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("Goal_Position", 2048) - time.sleep(4) - print("Present Position", motor_bus.read("Present_Position")) - + motor_bus.write("Max_Angle_Limit", 4095) # default 4095 + motor_bus.write("Min_Angle_Limit", 0) # default 0 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}")