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

Remove CLI for calibration visualization + move to debugging

This commit is contained in:
Simon Alibert 2025-03-20 14:44:23 +01:00
parent 02a1cf6a4e
commit 287dc13d96
2 changed files with 26 additions and 49 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
lerobot/scripts/calibration_visualization_so100.py → lerobot/common/debugging/motors_bus.py
View File

@ -1,21 +1,18 @@
""" """
usage: Usage example
```python ```python
python lerobot/scripts/calibration_visualization_so100.py \ from lerobot.common.debugging.motors_bus import visualize_motors_bus
--teleop.type=so100 \ from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig
--teleop.port=/dev/tty.usbmodem58760430541
python lerobot/scripts/calibration_visualization_so100.py \ cfg = SO100RobotConfig(port="/dev/tty.usbmodem58760430541")
--robot.type=so100 \ so100 = SO100Robot(cfg)
--robot.port=/dev/tty.usbmodem585A0084711
visualize_motors_bus(so100.arm)
``` ```
""" """
import time import time
from dataclasses import dataclass
import draccus
from lerobot.common.motors import MotorsBus from lerobot.common.motors import MotorsBus
from lerobot.common.motors.feetech.feetech_calibration import ( 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_degrees_to_ticks,
convert_ticks_to_degrees, 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 def visualize_motors_bus(motors_bus: MotorsBus):
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):
""" """
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.
""" """
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 # Disable torque on all motors so you can move them freely by hand
# values_dict = {idx: 0 for idx in motors_bus.motor_ids} values_dict = {idx: 0 for idx in motors_bus.motor_ids}
# motor_bus.write("Torque_Enable", values_dict) motors_bus.write("Torque_Enable", values_dict)
# print("Torque disabled on all joints.") 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 = motor_bus.read("Present_Position") raw_positions = motors_bus.read("Present_Position")
# # Read *already-homed* positions # # Read *already-homed* positions
# homed_positions = motor_bus.read("Present_Position") # homed_positions = motor_bus.read("Present_Position")
for name, raw_ticks in raw_positions.items(): for name, raw_ticks in raw_positions.items():
idx = motor_bus.motors[name][0] idx = motors_bus.motors[name][0]
model = motor_bus.motors[name][1] model = motors_bus.motors[name][1]
# 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, motor_bus, idx) manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, motors_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, motor_bus, idx) inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, motors_bus, idx)
print( print(
f"{name:15s} | " f"{name:15s} | "
@ -98,8 +75,8 @@ def visualize_motors_bus(motor_bus: MotorsBus):
pass pass
finally: finally:
print("\nExiting. Disconnecting bus...") print("\nExiting. Disconnecting bus...")
motor_bus.disconnect() motors_bus.disconnect()
if __name__ == "__main__": if __name__ == "__main__":
debug_device_calibration() visualize_motors_bus()

12
lerobot/common/motors/feetech/feetech_calibration.py
View File

@ -266,7 +266,7 @@ def convert_degrees_to_ticks(degrees, model):
return int(degrees * (resolutions / 360.0)) 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' 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). 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. # Flip sign if drive_mode is set.
drive_mode = 0 drive_mode = 0
if motorbus.calibration is not None: if motor_bus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1]
if drive_mode: if drive_mode:
ticks *= -1 ticks *= -1
@ -291,15 +291,15 @@ def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_i
return ticks 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] 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. and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
""" """
# Flip sign if drive_mode was set. # Flip sign if drive_mode was set.
drive_mode = 0 drive_mode = 0
if motorbus.calibration is not None: if motor_bus.calibration is not None:
drive_mode = motorbus.calibration["drive_mode"][motor_id - 1] drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1]
if drive_mode: if drive_mode:
adjusted_pos *= -1 adjusted_pos *= -1