hangX/lerobot
1
0
Fork 0
forked from tangger/lerobot
Code Pull Requests Activity

Update configure_motor script

Browse Source
This commit is contained in:
Simon Alibert
2025-03-15 13:13:26 +01:00
parent ad0bacbfe4
commit 5006da72ff
1 changed files with 30 additions and 65 deletions
Download Patch File Download Diff File Expand all files Collapse all files

95
lerobot/scripts/configure_motor.py
View File

@@ -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()