diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index cb67a5495..4c7fa5e06 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/lerobot/common/robot_devices/robots/feetech_calibration.py @@ -37,18 +37,12 @@ def apply_drive_mode(position, drive_mode): return position -def compute_nearest_rounded_position(position, models): - # delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models) - # nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn - # return nearest_pos.astype(position.dtype) - 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) @@ -58,74 +52,67 @@ def move_until_block(arm, motor_name, positive_direction=True, while_move_hook=N present_pos = arm.read("Present_Position", motor_name).item() present_speed = arm.read("Present_Speed", motor_name).item() - present_load = arm.read("Present_Load", 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_load > 100 and present_speed == 0: + if present_speed == 0 and present_current > 50: count += 1 - if count > 100: + if count > 100 or present_current > 300: return present_pos else: count = 0 -def move_to_calibrate(arm, motor_name, positive_first=True, in_between_move_hook=None, while_move_hook=None): +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 ) - p_delta_pos = abs(initial_pos - p_present_pos) - - if in_between_move_hook is not None: - in_between_move_hook() - - n_present_pos = move_until_block( - arm, motor_name, positive_direction=False, while_move_hook=while_move_hook - ) - n_delta_pos = abs(initial_pos - n_present_pos) else: n_present_pos = move_until_block( arm, motor_name, positive_direction=False, while_move_hook=while_move_hook ) - n_delta_pos = abs(initial_pos - n_present_pos) - if in_between_move_hook is not None: - in_between_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 ) - p_delta_pos = abs(initial_pos - p_present_pos) - - if p_delta_pos < n_delta_pos: - invert_drive_mode = False - min_pos = n_present_pos - max_pos = p_present_pos - zero_pos = (min_pos + max_pos) / 2 - homing_offset = -zero_pos - else: - invert_drive_mode = True - min_pos = p_present_pos - max_pos = n_present_pos - zero_pos = (min_pos + max_pos) / 2 - homing_offset = zero_pos + + zero_pos = (n_present_pos + p_present_pos) / 2 calib_data = { - "homing_offset": homing_offset, + "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, - "min_pos": min_pos, - "max_pos": max_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 @@ -139,76 +126,85 @@ def apply_offset(calib, offset): return calib -def run_arm_auto_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type: str): +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}...") 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) + print(f'{arm.read("Present_Position", "elbow_flex")=}') + calib = {} - # Calibrate shoulder_pan + 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("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) - # Calibrate elbow_flex - - calib["elbow_flex"] = move_to_calibrate(arm, "elbow_flex") - calib["elbow_flex"] = apply_offset(calib["elbow_flex"], offset=130 - 1024) - - arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024, "elbow_flex") + print("Calibrate gripper") + calib["gripper"] = move_to_calibrate(arm, "gripper", invert_drive_mode=True) time.sleep(1) - # Calibrate wrist_flex - + print("Calibrate wrist_flex") calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex") - calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=100) + calib["wrist_flex"] = apply_offset(calib["wrist_flex"], offset=80) - arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"], "wrist_flex") - time.sleep(1) - - # Calibrate gripper - - calib["gripper"] = move_to_calibrate(arm, "gripper") - - tmp_max_pos = calib["gripper"]["max_pos"] - calib["gripper"]["max_pos"] = calib["gripper"]["min_pos"] - calib["gripper"]["min_pos"] = tmp_max_pos + 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) + + 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) - arm.write("Goal_Position", calib["gripper"]["min_pos"], "gripper") - time.sleep(1) - arm.write("Goal_Position", calib["wrist_flex"]["zero_pos"] - 1350, "wrist_flex") + arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] + 1024 + 512, "elbow_flex") time.sleep(1) - # Calibrate shoulder_lift - def in_between_move_hook(): nonlocal arm, calib - initial_pos = arm.read("Present_Position", "shoulder_lift") - arm.write("Goal_Position", initial_pos + 900, "shoulder_lift") - time.sleep(1) - arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"] - 150, "elbow_flex") - time.sleep(1) + arm.write("Goal_Position", calib["elbow_flex"]["zero_pos"], "elbow_flex") + print("Calibrate shoulder_lift") calib["shoulder_lift"] = move_to_calibrate( - arm, "shoulder_lift", positive_first=False, in_between_move_hook=in_between_move_hook + 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=30 + 1024 - 120) - - # Calibrate wrist_roll + calib["shoulder_lift"] = apply_offset(calib["shoulder_lift"], offset=1024 - 50) def while_move_hook(): nonlocal arm, calib @@ -216,7 +212,7 @@ def while_move_hook(): "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"]["max_pos"] - 400), + "gripper": round(calib["gripper"]["end_pos"]), } arm.write("Goal_Position", list(positions.values()), list(positions.keys())) @@ -226,16 +222,17 @@ def while_move_hook(): 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"]["max_pos"] - 400), "gripper") + arm.write("Goal_Position", round(calib["gripper"]["end_pos"]), "gripper") time.sleep(2) + print("Calibrate wrist_roll") calib["wrist_roll"] = move_to_calibrate( - arm, "wrist_roll", positive_first=False, while_move_hook=while_move_hook + arm, "wrist_roll", invert_drive_mode=True, positive_first=False, while_move_hook=while_move_hook ) arm.write("Goal_Position", calib["wrist_roll"]["zero_pos"], "wrist_roll") time.sleep(1) - arm.write("Goal_Position", calib["gripper"]["min_pos"], "gripper") + 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) @@ -255,12 +252,141 @@ def while_move_hook(): 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]["min_pos"] for name in arm.motor_names], - "end_pos": [calib[name]["max_pos"] 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_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.") + + if not (robot_type == "moss" and arm_type == "follower"): + raise NotImplementedError("Auto calibration only supports the follower of moss arms for now.") + + print(f"\nRunning calibration of {robot_type} {arm_name} {arm_type}...") + + 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", load_threshold=350, count_threshold=200) + 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, count_threshold=200) + time.sleep(1) + + print("Calibrate wrist_flex") + calib["wrist_flex"] = move_to_calibrate(arm, "wrist_flex", invert_drive_mode=True, count_threshold=200) + 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, count_threshold=200) + 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, + count_threshold=200, + 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 @@ -283,7 +409,7 @@ def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, a Example of usage: ```python - run_arm_calibration(arm, "koch", "left", "follower") + run_arm_calibration(arm, "so100", "left", "follower") ``` """ if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): @@ -339,7 +465,7 @@ def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, a else: calib_modes.append(CalibrationMode.DEGREE.name) - calib_data = { + calib_dict = { "homing_offset": homing_offset.tolist(), "drive_mode": drive_mode.tolist(), "start_pos": zero_pos.tolist(), @@ -347,4 +473,4 @@ def run_arm_manual_calibration(arm: MotorsBus, robot_type: str, arm_name: str, a "calib_mode": calib_modes, "motor_names": arm.motor_names, } - return calib_data + return calib_dict diff --git a/lerobot/common/robot_devices/robots/manipulator.py b/lerobot/common/robot_devices/robots/manipulator.py index 46eba1f62..0ad219f9e 100644 --- a/lerobot/common/robot_devices/robots/manipulator.py +++ b/lerobot/common/robot_devices/robots/manipulator.py @@ -337,24 +337,27 @@ def load_or_run_calibration_(name, arm, arm_type): elif self.robot_type in ["so100"]: from lerobot.common.robot_devices.robots.feetech_calibration import ( - run_arm_auto_calibration, + run_arm_auto_calibration_so100, run_arm_manual_calibration, ) if arm_type == "leader": calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type) elif arm_type == "follower": - calibration = run_arm_auto_calibration(arm, self.robot_type, name, arm_type) + calibration = run_arm_auto_calibration_so100(arm, self.robot_type, name, arm_type) else: raise ValueError(arm_type) elif self.robot_type in ["moss"]: from lerobot.common.robot_devices.robots.feetech_calibration import ( + run_arm_auto_calibration_moss, run_arm_manual_calibration, ) - if arm_type == "leader" or arm_type == "follower": + if arm_type == "leader": calibration = run_arm_manual_calibration(arm, self.robot_type, name, arm_type) + elif arm_type == "follower": + calibration = run_arm_auto_calibration_moss(arm, self.robot_type, name, arm_type) else: raise ValueError(arm_type) @@ -468,10 +471,10 @@ def set_so100_robot_preset(self): # Mode=0 for Position Control self.follower_arms[name].write("Mode", 0) # Set P_Coefficient to lower value to avoid shakiness (Default is 32) - self.follower_arms[name].write("P_Coefficient", 16, "shoulder_pan") + self.follower_arms[name].write("P_Coefficient", 16) # Set I_Coefficient and D_Coefficient to default value 0 and 32 - self.follower_arms[name].write("I_Coefficient", 0, "shoulder_pan") - self.follower_arms[name].write("D_Coefficient", 32, "shoulder_pan") + self.follower_arms[name].write("I_Coefficient", 0) + self.follower_arms[name].write("D_Coefficient", 32) # Close the write lock so that Maximum_Acceleration gets written to EPROM address, # which is mandatory for Maximum_Acceleration to take effect after rebooting. self.follower_arms[name].write("Lock", 0) diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index 4d833c088..18707397f 100644 --- a/lerobot/scripts/configure_motor.py +++ b/lerobot/scripts/configure_motor.py @@ -112,21 +112,17 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): if brand == "feetech": # 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("Goal_Position", 2047) + motor_bus.write("Goal_Position", 2048) time.sleep(4) print("Present Position", motor_bus.read("Present_Position")) - motor_bus.write("Offset", 2027) + motor_bus.write("Offset", 0) time.sleep(4) print("Offset", motor_bus.read("Offset")) - # Read present position for 15 seconds - for _ in range(30): - print("Present Position", motor_bus.read("Present_Position")) - time.sleep(0.5) - except Exception as e: print(f"Error occurred during motor configuration: {e}") diff --git a/media/moss/follower_initial.webp b/media/moss/follower_initial.webp new file mode 100644 index 000000000..e7ded16bd Binary files /dev/null and b/media/moss/follower_initial.webp differ diff --git a/media/moss/leader_rest.webp b/media/moss/leader_rest.webp new file mode 100644 index 000000000..cd77d294d Binary files /dev/null and b/media/moss/leader_rest.webp differ diff --git a/media/moss/leader_rotated.webp b/media/moss/leader_rotated.webp new file mode 100644 index 000000000..c3426650a Binary files /dev/null and b/media/moss/leader_rotated.webp differ diff --git a/media/moss/leader_zero.webp b/media/moss/leader_zero.webp new file mode 100644 index 000000000..d79ed3736 Binary files /dev/null and b/media/moss/leader_zero.webp differ diff --git a/media/so100/follower_initial.webp b/media/so100/follower_initial.webp new file mode 100644 index 000000000..7f93a773a Binary files /dev/null and b/media/so100/follower_initial.webp differ diff --git a/media/so100/leader_rest.webp b/media/so100/leader_rest.webp new file mode 100644 index 000000000..351667778 Binary files /dev/null and b/media/so100/leader_rest.webp differ diff --git a/media/so100/leader_rotated.webp b/media/so100/leader_rotated.webp new file mode 100644 index 000000000..1f770f6ce Binary files /dev/null and b/media/so100/leader_rotated.webp differ diff --git a/media/so100/leader_zero.webp b/media/so100/leader_zero.webp new file mode 100644 index 000000000..5f8c235f9 Binary files /dev/null and b/media/so100/leader_zero.webp differ