From 63e6f7901e35108ea6b098347314cc6d216a1b8b Mon Sep 17 00:00:00 2001 From: Pepijn Date: Tue, 18 Feb 2025 15:04:25 +0100 Subject: [PATCH] Added calibration visualization --- .../common/robot_devices/motors/feetech.py | 84 ++++++------ .../robots/feetech_calibration.py | 4 +- lerobot/scripts/calibration_visualization.py | 121 ++++++++++++++++++ lerobot/scripts/configure_motor.py | 5 +- lerobot/scripts/test_homing.py | 112 ---------------- 5 files changed, 163 insertions(+), 163 deletions(-) create mode 100644 lerobot/scripts/calibration_visualization.py delete mode 100644 lerobot/scripts/test_homing.py diff --git a/lerobot/common/robot_devices/motors/feetech.py b/lerobot/common/robot_devices/motors/feetech.py index 7e23a3a1..d87c1653 100644 --- a/lerobot/common/robot_devices/motors/feetech.py +++ b/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: - """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. +def convert_degrees_to_steps( + degrees: float | np.ndarray, models: list[str], multi_turn_index: int +) -> np.ndarray: """ + 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) - return steps + + # Remove full rotations from degrees + 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. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. + """ + Converts motor steps to degrees while accounting for multi-turn motion. + - 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]. - Tracks multi-turn rotations by adjusting a multi-turn index. + Converts raw motor ticks [0..4095] to homed servo ticks with multi-turn indexing. + - 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: - shifted -= 4096 + if shifted > resolutions // 2: + 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 delta > 2048: + # If jump forward > 180° (2048 steps), assume full rotation + if delta > resolutions // 2: multi_turn_index -= 1 - - # If we jump backward by more than half a turn, we must have wrapped positively - elif delta < -2048: + elif delta < -resolutions // 2: 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 shifted + multi_turn_index * 4096 + # Return final adjusted ticks with multi-turn indexing + 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 diff --git a/lerobot/common/robot_devices/robots/feetech_calibration.py b/lerobot/common/robot_devices/robots/feetech_calibration.py index 56d1ed1c..45e0e3fd 100644 --- a/lerobot/common/robot_devices/robots/feetech_calibration.py +++ b/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") diff --git a/lerobot/scripts/calibration_visualization.py b/lerobot/scripts/calibration_visualization.py new file mode 100644 index 00000000..6f8a81db --- /dev/null +++ b/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) diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index dc0d6826..f2e4090b 100644 --- a/lerobot/scripts/configure_motor.py +++ b/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) - - # TODO(pepijn): store single_calibration + # Then store single_calibration in yaml via dict (not overwrite but add the single calibration each time for motor id) except Exception as e: print(f"Error occurred during motor configuration: {e}") diff --git a/lerobot/scripts/test_homing.py b/lerobot/scripts/test_homing.py deleted file mode 100644 index b9839b4a..00000000 --- a/lerobot/scripts/test_homing.py +++ /dev/null @@ -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()