From 287dc13d96766d7fac9bcc4e254911a6d2886ff7 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Thu, 20 Mar 2025 14:44:23 +0100 Subject: [PATCH] Remove CLI for calibration visualization + move to debugging --- .../debugging/motors_bus.py} | 63 ++++++------------- .../motors/feetech/feetech_calibration.py | 12 ++-- 2 files changed, 26 insertions(+), 49 deletions(-) rename lerobot/{scripts/calibration_visualization_so100.py => common/debugging/motors_bus.py} (57%) diff --git a/lerobot/scripts/calibration_visualization_so100.py b/lerobot/common/debugging/motors_bus.py similarity index 57% rename from lerobot/scripts/calibration_visualization_so100.py rename to lerobot/common/debugging/motors_bus.py index 10ac7c4d..f41a454b 100644 --- a/lerobot/scripts/calibration_visualization_so100.py +++ b/lerobot/common/debugging/motors_bus.py @@ -1,21 +1,18 @@ """ -usage: +Usage example ```python -python lerobot/scripts/calibration_visualization_so100.py \ - --teleop.type=so100 \ - --teleop.port=/dev/tty.usbmodem58760430541 +from lerobot.common.debugging.motors_bus import visualize_motors_bus +from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig -python lerobot/scripts/calibration_visualization_so100.py \ - --robot.type=so100 \ - --robot.port=/dev/tty.usbmodem585A0084711 +cfg = SO100RobotConfig(port="/dev/tty.usbmodem58760430541") +so100 = SO100Robot(cfg) + +visualize_motors_bus(so100.arm) ``` """ import time -from dataclasses import dataclass - -import draccus from lerobot.common.motors import MotorsBus from lerobot.common.motors.feetech.feetech_calibration import ( @@ -24,64 +21,44 @@ from lerobot.common.motors.feetech.feetech_calibration import ( convert_degrees_to_ticks, convert_ticks_to_degrees, ) -from lerobot.common.robots import RobotConfig -from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig # noqa: F401 -from lerobot.common.teleoperators import TeleoperatorConfig -from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig # noqa: F401 -@dataclass -class DebugConfig: - teleop: TeleoperatorConfig | None = None - robot: RobotConfig | None = None - - def __post_init__(self): - if bool(self.teleop) == bool(self.robot): - raise ValueError("Select a single device.") - - -@draccus.wrap() -def debug_device_calibration(cfg: DebugConfig): - # TODO(aliberts): make device and automatically get its motors_bus - device = SO100Teleop(cfg.teleop) if cfg.teleop else SO100Robot(cfg.robot) - visualize_motors_bus(device.arm) - - -def visualize_motors_bus(motor_bus: MotorsBus): +def visualize_motors_bus(motors_bus: MotorsBus): """ Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks. """ - motor_bus.connect() + if not motors_bus.is_connected: + motors_bus.connect() # Disable torque on all motors so you can move them freely by hand - # values_dict = {idx: 0 for idx in motors_bus.motor_ids} - # motor_bus.write("Torque_Enable", values_dict) - # print("Torque disabled on all joints.") + values_dict = {idx: 0 for idx in motors_bus.motor_ids} + motors_bus.write("Torque_Enable", values_dict) + print("Torque disabled on all joints.") try: print("\nPress Ctrl+C to quit.\n") while True: # Read *raw* positions (no calibration). - raw_positions = motor_bus.read("Present_Position") + raw_positions = motors_bus.read("Present_Position") # # Read *already-homed* positions # homed_positions = motor_bus.read("Present_Position") for name, raw_ticks in raw_positions.items(): - idx = motor_bus.motors[name][0] - model = motor_bus.motors[name][1] + idx = motors_bus.motors[name][0] + model = motors_bus.motors[name][1] # homed_val = homed_positions[i] # degrees or % if linear # Manually compute "adjusted ticks" from raw ticks - manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, motor_bus, idx) + manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, motors_bus, idx) # Convert to degrees manual_degs = convert_ticks_to_degrees(manual_adjusted, model) # Convert that deg back to ticks manual_ticks = convert_degrees_to_ticks(manual_degs, model) # Then invert them using offset & bus drive mode - inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, motor_bus, idx) + inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, motors_bus, idx) print( f"{name:15s} | " @@ -98,8 +75,8 @@ def visualize_motors_bus(motor_bus: MotorsBus): pass finally: print("\nExiting. Disconnecting bus...") - motor_bus.disconnect() + motors_bus.disconnect() if __name__ == "__main__": - debug_device_calibration() + visualize_motors_bus() diff --git a/lerobot/common/motors/feetech/feetech_calibration.py b/lerobot/common/motors/feetech/feetech_calibration.py index f967f82d..ad51f222 100644 --- a/lerobot/common/motors/feetech/feetech_calibration.py +++ b/lerobot/common/motors/feetech/feetech_calibration.py @@ -266,7 +266,7 @@ def convert_degrees_to_ticks(degrees, model): return int(degrees * (resolutions / 360.0)) -def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int: +def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motor_bus: MotorsBus, motor_id: int) -> int: """ Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048' becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution). @@ -282,8 +282,8 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i # Flip sign if drive_mode is set. drive_mode = 0 - if motorbus.calibration is not None: - drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] + if motor_bus.calibration is not None: + drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1] if drive_mode: ticks *= -1 @@ -291,15 +291,15 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i return ticks -def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int: +def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motor_bus: MotorsBus, motor_id: int) -> int: """ Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047] and recovers the raw [0..(res-1)] ticks with 2048 as midpoint. """ # Flip sign if drive_mode was set. drive_mode = 0 - if motorbus.calibration is not None: - drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] + if motor_bus.calibration is not None: + drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1] if drive_mode: adjusted_pos *= -1