From 5006da72ff852eb53524353490cb33a5a86c4820 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Sat, 15 Mar 2025 13:13:26 +0100 Subject: [PATCH] Update configure_motor script --- lerobot/scripts/configure_motor.py | 95 ++++++++++-------------------- 1 file changed, 30 insertions(+), 65 deletions(-) diff --git a/lerobot/scripts/configure_motor.py b/lerobot/scripts/configure_motor.py index 6569db195..617db2fc7 100644 --- a/lerobot/scripts/configure_motor.py +++ b/lerobot/scripts/configure_motor.py @@ -28,71 +28,31 @@ python lerobot/scripts/configure_motor.py \ import argparse import time +from lerobot.common.motors.dynamixel.dynamixel import MODEL_RESOLUTION as DXL_MODEL_RESOLUTION +from lerobot.common.motors.feetech.feetech import MODEL_RESOLUTION as FTCH_MODEL_RESOLUTION -def get_motor_bus_cls(brand: str) -> tuple: + +def configure_motor(port, brand, model, target_motor_idx, target_baudrate): if brand == "feetech": - from lerobot.common.motors.configs import FeetechMotorsBusConfig - from lerobot.common.motors.feetech.feetech import ( - MODEL_BAUDRATE_TABLE, - SCS_SERIES_BAUDRATE_TABLE, - FeetechMotorsBus, - ) + from lerobot.common.motors.feetech.feetech import FeetechMotorsBus - return FeetechMotorsBusConfig, FeetechMotorsBus, MODEL_BAUDRATE_TABLE, SCS_SERIES_BAUDRATE_TABLE + motor_bus = FeetechMotorsBus(port=port, motors={"motor": (target_motor_idx, model)}) elif brand == "dynamixel": - from lerobot.common.motors.configs import DynamixelMotorsBusConfig - from lerobot.common.motors.dynamixel.dynamixel import ( - MODEL_BAUDRATE_TABLE, - X_SERIES_BAUDRATE_TABLE, - DynamixelMotorsBus, - ) + from lerobot.common.motors.dynamixel.dynamixel import DynamixelMotorsBus - return DynamixelMotorsBusConfig, DynamixelMotorsBus, MODEL_BAUDRATE_TABLE, X_SERIES_BAUDRATE_TABLE + motor_bus = DynamixelMotorsBus(port=port, motors={"motor": (target_motor_idx, model)}) - else: - raise ValueError( - f"Currently we do not support this motor brand: {brand}. We currently support feetech and dynamixel motors." - ) - - -def configure_motor(port, brand, model, motor_idx_des, baudrate_des): - motor_bus_config_cls, motor_bus_cls, model_baudrate_table, series_baudrate_table = get_motor_bus_cls( - brand - ) - - # Check if the provided model exists in the model_baud_rate_table - if model not in model_baudrate_table: - raise ValueError( - f"Invalid model '{model}' for brand '{brand}'. Supported models: {list(model_baudrate_table.keys())}" - ) - - # Setup motor names, indices, and models - motor_name = "motor" - motor_index_arbitrary = motor_idx_des # Use the motor ID passed via argument - motor_model = model # Use the motor model passed via argument - - config = motor_bus_config_cls(port=port, motors={motor_name: (motor_index_arbitrary, motor_model)}) - - # Initialize the MotorBus with the correct port and motor configurations - motor_bus = motor_bus_cls(config=config) - - # 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 + motor_bus.connect() # Motor bus is connected, proceed with the rest of the operations try: print("Scanning all baudrates and motor indices") - all_baudrates = set(series_baudrate_table.values()) + model_baudrates = list(motor_bus.model_baudrate_table[model].values()) motor_index = -1 # Set the motor index to an out-of-range value. - for baudrate in all_baudrates: - motor_bus.set_bus_baudrate(baudrate) + for baudrate in model_baudrates: + motor_bus.set_baudrate(baudrate) present_ids = motor_bus.find_motor_indices(list(range(1, 10))) if len(present_ids) > 1: raise ValueError( @@ -116,14 +76,14 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): # Allows ID and BAUDRATE to be written in memory motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) - if baudrate != baudrate_des: - print(f"Setting its baudrate to {baudrate_des}") - baudrate_idx = list(series_baudrate_table.values()).index(baudrate_des) + if baudrate != target_baudrate: + print(f"Setting its baudrate to {target_baudrate}") + baudrate_idx = model_baudrates.index(target_baudrate) # The write can fail, so we allow retries motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Baud_Rate", baudrate_idx) time.sleep(0.5) - motor_bus.set_bus_baudrate(baudrate_des) + motor_bus.set_bus_baudrate(target_baudrate) present_baudrate_idx = motor_bus.read_with_motor_ids( motor_bus.motor_models, motor_index, "Baud_Rate", num_retry=2 ) @@ -131,13 +91,15 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): if present_baudrate_idx != baudrate_idx: raise OSError("Failed to write baudrate.") - print(f"Setting its index to desired index {motor_idx_des}") + print(f"Setting its index to desired index {target_motor_idx}") if brand == "feetech": motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "Lock", 0) - motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", motor_idx_des) + motor_bus.write_with_motor_ids(motor_bus.motor_models, motor_index, "ID", target_motor_idx) - 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: + present_idx = motor_bus.read_with_motor_ids( + motor_bus.motor_models, target_motor_idx, "ID", num_retry=2 + ) + if present_idx != target_motor_idx: raise OSError("Failed to write index.") if brand == "feetech": @@ -157,18 +119,21 @@ def configure_motor(port, brand, model, motor_idx_des, baudrate_des): print(f"Error occurred during motor configuration: {e}") finally: - motor_bus.disconnect() + if motor_bus.is_connected: + motor_bus.disconnect() print("Disconnected from motor bus.") if __name__ == "__main__": + model_choices = [*FTCH_MODEL_RESOLUTION.keys(), *DXL_MODEL_RESOLUTION.keys()] + brand_choices = ["feetech", "dynamixel"] parser = argparse.ArgumentParser() - parser.add_argument("--port", type=str, required=True, help="Motors bus port (e.g. dynamixel,feetech)") - parser.add_argument("--brand", type=str, required=True, help="Motor brand (e.g. dynamixel,feetech)") - parser.add_argument("--model", type=str, required=True, help="Motor model (e.g. xl330-m077,sts3215)") + parser.add_argument("--port", type=str, required=True, help="Motors bus port") + parser.add_argument("--brand", type=str, required=True, choices=brand_choices, help="Motor brand") + parser.add_argument("--model", type=str, required=True, choices=model_choices, help="Motor model") parser.add_argument("--ID", type=int, required=True, help="Desired ID of the current motor (e.g. 1,2,3)") parser.add_argument( - "--baudrate", type=int, default=1000000, help="Desired baudrate for the motor (default: 1000000)" + "--baudrate", type=int, default=1_000_000, help="Desired baudrate for the motor (default: 1_000_000)" ) args = parser.parse_args()