wangzixiang
/
lerobot
mirror of https://github.com/huggingface/lerobot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Fix calibration visualization

This commit is contained in:
Simon Alibert 2025-03-20 14:33:36 +01:00
parent 34cd1e47bf
commit 02a1cf6a4e
1 changed files with 26 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
lerobot/scripts/calibration_visualization_so100.py
View File

@ -17,7 +17,8 @@ from dataclasses import dataclass
import draccus 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_homing_ticks,
adjusted_to_motor_ticks, adjusted_to_motor_ticks,
convert_degrees_to_ticks, convert_degrees_to_ticks,
@ -30,7 +31,7 @@ from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig #
@dataclass @dataclass
class DebugFeetechConfig: class DebugConfig:
teleop: TeleoperatorConfig | None = None teleop: TeleoperatorConfig | None = None
robot: RobotConfig | None = None robot: RobotConfig | None = None
@ -40,48 +41,52 @@ class DebugFeetechConfig:
@draccus.wrap() @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. 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) motor_bus.connect()
device.connect()
# Disable torque on all motors so you can move them freely by hand # 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) # values_dict = {idx: 0 for idx in motors_bus.motor_ids}
print("Torque disabled on all joints.") # motor_bus.write("Torque_Enable", values_dict)
# print("Torque disabled on all joints.")
try: try:
print("\nPress Ctrl+C to quit.\n") print("\nPress Ctrl+C to quit.\n")
while True: while True:
# Read *raw* positions (no calibration). # Read *raw* positions (no calibration).
raw_positions = device.arm.read_with_motor_ids( raw_positions = motor_bus.read("Present_Position")
device.arm.motor_models, device.arm.motor_indices, data_name="Present_Position"
)
# Read *already-homed* positions # # Read *already-homed* positions
homed_positions = device.arm.read("Present_Position") # homed_positions = motor_bus.read("Present_Position")
for i, name in enumerate(device.arm.motor_names): for name, raw_ticks in raw_positions.items():
motor_idx, model = device.arm.motors[name] 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 # 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 # Convert to degrees
manual_degs = convert_ticks_to_degrees(manual_adjusted, model) manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
# Convert that deg back to ticks # Convert that deg back to ticks
manual_ticks = convert_degrees_to_ticks(manual_degs, model) manual_ticks = convert_degrees_to_ticks(manual_degs, model)
# Then invert them using offset & bus drive mode # 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( print(
f"{name:15s} | " f"{name:15s} | "
f"RAW={raw_ticks:4d} | " 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"HOMED_TICKS={manual_adjusted:6d} | "
f"MANUAL_ADJ_DEG={manual_degs:7.2f} | " f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
f"MANUAL_ADJ_TICKS={manual_ticks:6d} | " f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
@ -93,8 +98,8 @@ def debug_feetech_positions(cfg: DebugFeetechConfig):
pass pass
finally: finally:
print("\nExiting. Disconnecting bus...") print("\nExiting. Disconnecting bus...")
device.disconnect() motor_bus.disconnect()
if __name__ == "__main__": if __name__ == "__main__":
debug_feetech_positions() debug_device_calibration()