diff --git a/lerobot/scripts/calibration_visualization_so100.py b/lerobot/scripts/calibration_visualization_so100.py index 33573d8b..10ac7c4d 100644 --- a/lerobot/scripts/calibration_visualization_so100.py +++ b/lerobot/scripts/calibration_visualization_so100.py @@ -17,7 +17,8 @@ from dataclasses import dataclass import draccus -from lerobot.common.motors.feetech.feetech import ( +from lerobot.common.motors import MotorsBus +from lerobot.common.motors.feetech.feetech_calibration import ( adjusted_to_homing_ticks, adjusted_to_motor_ticks, convert_degrees_to_ticks, @@ -30,7 +31,7 @@ from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig # @dataclass -class DebugFeetechConfig: +class DebugConfig: teleop: TeleoperatorConfig | None = None robot: RobotConfig | None = None @@ -40,48 +41,52 @@ class DebugFeetechConfig: @draccus.wrap() -def debug_feetech_positions(cfg: DebugFeetechConfig): +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): """ Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks. """ - device = SO100Teleop(cfg.teleop) if cfg.teleop else SO100Robot(cfg.robot) - device.connect() + motor_bus.connect() # Disable torque on all motors so you can move them freely by hand - device.arm.write("Torque_Enable", 0, motor_names=device.arm.motor_names) - print("Torque disabled on all joints.") + # values_dict = {idx: 0 for idx in motors_bus.motor_ids} + # motor_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 = device.arm.read_with_motor_ids( - device.arm.motor_models, device.arm.motor_indices, data_name="Present_Position" - ) + raw_positions = motor_bus.read("Present_Position") - # Read *already-homed* positions - homed_positions = device.arm.read("Present_Position") + # # Read *already-homed* positions + # homed_positions = motor_bus.read("Present_Position") - for i, name in enumerate(device.arm.motor_names): - motor_idx, model = device.arm.motors[name] + for name, raw_ticks in raw_positions.items(): + idx = motor_bus.motors[name][0] + model = motor_bus.motors[name][1] - raw_ticks = raw_positions[i] # 0..4095 - homed_val = homed_positions[i] # degrees or % if linear + # 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, device.arm, motor_idx) + manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, motor_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, device.arm, motor_idx) + inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, motor_bus, idx) print( f"{name:15s} | " f"RAW={raw_ticks:4d} | " - f"HOMED_FROM_READ={homed_val:7.2f} | " + # f"HOMED_FROM_READ={homed_val:7.2f} | " f"HOMED_TICKS={manual_adjusted:6d} | " f"MANUAL_ADJ_DEG={manual_degs:7.2f} | " f"MANUAL_ADJ_TICKS={manual_ticks:6d} | " @@ -93,8 +98,8 @@ def debug_feetech_positions(cfg: DebugFeetechConfig): pass finally: print("\nExiting. Disconnecting bus...") - device.disconnect() + motor_bus.disconnect() if __name__ == "__main__": - debug_feetech_positions() + debug_device_calibration()