From 56c04ffc5313226f66b79bbe02e9074d68bc7724 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 21 Mar 2025 11:28:11 +0100 Subject: [PATCH] Move dxl table & cleanup --- lerobot/common/motors/dynamixel/__init__.py | 3 +- lerobot/common/motors/dynamixel/dynamixel.py | 138 +------------------ lerobot/common/motors/dynamixel/tables.py | 110 +++++++++++++++ tests/mocks/mock_dynamixel.py | 2 +- 4 files changed, 113 insertions(+), 140 deletions(-) create mode 100644 lerobot/common/motors/dynamixel/tables.py diff --git a/lerobot/common/motors/dynamixel/__init__.py b/lerobot/common/motors/dynamixel/__init__.py index cd86bca1..1bd35361 100644 --- a/lerobot/common/motors/dynamixel/__init__.py +++ b/lerobot/common/motors/dynamixel/__init__.py @@ -1,4 +1,3 @@ from .dynamixel import DynamixelMotorsBus from .dynamixel_calibration import run_arm_calibration - -__all__ = ["DynamixelMotorsBus", "run_arm_calibration"] +from .tables import * diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 413ed173..072e995c 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -20,153 +20,17 @@ from copy import deepcopy -import numpy as np - from ..motors_bus import MotorsBus +from .tables import MODEL_BAUDRATE_TABLE, MODEL_CONTROL_TABLE, MODEL_RESOLUTION PROTOCOL_VERSION = 2.0 BAUDRATE = 1_000_000 DEFAULT_TIMEOUT_MS = 1000 - MAX_ID_RANGE = 252 -# The following bounds define the lower and upper joints range (after calibration). -# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees -# which corresponds to a half rotation on the left and half rotation on the right. -# Some joints might require higher range, so we allow up to [-270, 270] degrees until -# an error is raised. -LOWER_BOUND_DEGREE = -270 -UPPER_BOUND_DEGREE = 270 - -# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper), -# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully -# closed, and 100% is fully open. To account for slight calibration issue, we allow up to -# [-10, 110] until an error is raised. -LOWER_BOUND_LINEAR = -10 -UPPER_BOUND_LINEAR = 110 - -HALF_TURN_DEGREE = 180 - -# https://emanual.robotis.com/docs/en/dxl/x/xl330-m077 -# https://emanual.robotis.com/docs/en/dxl/x/xl330-m288 -# https://emanual.robotis.com/docs/en/dxl/x/xl430-w250 -# https://emanual.robotis.com/docs/en/dxl/x/xm430-w350 -# https://emanual.robotis.com/docs/en/dxl/x/xm540-w270 -# https://emanual.robotis.com/docs/en/dxl/x/xc430-w150 - -# data_name: (address, size_byte) -X_SERIES_CONTROL_TABLE = { - "Model_Number": (0, 2), - "Model_Information": (2, 4), - "Firmware_Version": (6, 1), - "ID": (7, 1), - "Baud_Rate": (8, 1), - "Return_Delay_Time": (9, 1), - "Drive_Mode": (10, 1), - "Operating_Mode": (11, 1), - "Secondary_ID": (12, 1), - "Protocol_Type": (13, 1), - "Homing_Offset": (20, 4), - "Moving_Threshold": (24, 4), - "Temperature_Limit": (31, 1), - "Max_Voltage_Limit": (32, 2), - "Min_Voltage_Limit": (34, 2), - "PWM_Limit": (36, 2), - "Current_Limit": (38, 2), - "Acceleration_Limit": (40, 4), - "Velocity_Limit": (44, 4), - "Max_Position_Limit": (48, 4), - "Min_Position_Limit": (52, 4), - "Shutdown": (63, 1), - "Torque_Enable": (64, 1), - "LED": (65, 1), - "Status_Return_Level": (68, 1), - "Registered_Instruction": (69, 1), - "Hardware_Error_Status": (70, 1), - "Velocity_I_Gain": (76, 2), - "Velocity_P_Gain": (78, 2), - "Position_D_Gain": (80, 2), - "Position_I_Gain": (82, 2), - "Position_P_Gain": (84, 2), - "Feedforward_2nd_Gain": (88, 2), - "Feedforward_1st_Gain": (90, 2), - "Bus_Watchdog": (98, 1), - "Goal_PWM": (100, 2), - "Goal_Current": (102, 2), - "Goal_Velocity": (104, 4), - "Profile_Acceleration": (108, 4), - "Profile_Velocity": (112, 4), - "Goal_Position": (116, 4), - "Realtime_Tick": (120, 2), - "Moving": (122, 1), - "Moving_Status": (123, 1), - "Present_PWM": (124, 2), - "Present_Current": (126, 2), - "Present_Velocity": (128, 4), - "Present_Position": (132, 4), - "Velocity_Trajectory": (136, 4), - "Position_Trajectory": (140, 4), - "Present_Input_Voltage": (144, 2), - "Present_Temperature": (146, 1), -} - -X_SERIES_BAUDRATE_TABLE = { - 0: 9_600, - 1: 57_600, - 2: 115_200, - 3: 1_000_000, - 4: 2_000_000, - 5: 3_000_000, - 6: 4_000_000, -} - CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"] -MODEL_CONTROL_TABLE = { - "x_series": X_SERIES_CONTROL_TABLE, - "xl330-m077": X_SERIES_CONTROL_TABLE, - "xl330-m288": X_SERIES_CONTROL_TABLE, - "xl430-w250": X_SERIES_CONTROL_TABLE, - "xm430-w350": X_SERIES_CONTROL_TABLE, - "xm540-w270": X_SERIES_CONTROL_TABLE, - "xc430-w150": X_SERIES_CONTROL_TABLE, -} - -MODEL_RESOLUTION = { - "x_series": 4096, - "xl330-m077": 4096, - "xl330-m288": 4096, - "xl430-w250": 4096, - "xm430-w350": 4096, - "xm540-w270": 4096, - "xc430-w150": 4096, -} - -MODEL_BAUDRATE_TABLE = { - "x_series": X_SERIES_BAUDRATE_TABLE, - "xl330-m077": X_SERIES_BAUDRATE_TABLE, - "xl330-m288": X_SERIES_BAUDRATE_TABLE, - "xl430-w250": X_SERIES_BAUDRATE_TABLE, - "xm430-w350": X_SERIES_BAUDRATE_TABLE, - "xm540-w270": X_SERIES_BAUDRATE_TABLE, - "xc430-w150": X_SERIES_BAUDRATE_TABLE, -} - -NUM_READ_RETRY = 10 -NUM_WRITE_RETRY = 10 - - -def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray: - """This function converts the degree range to the step range for indicating motors rotation. - It assumes a motor achieves a full rotation by going from -180 degree position to +180. - The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation. - """ - resolutions = [MODEL_RESOLUTION[model] for model in models] - steps = degrees / 180 * np.array(resolutions) / 2 - steps = steps.astype(int) - return steps - class DynamixelMotorsBus(MotorsBus): """ diff --git a/lerobot/common/motors/dynamixel/tables.py b/lerobot/common/motors/dynamixel/tables.py new file mode 100644 index 00000000..06c5a0ad --- /dev/null +++ b/lerobot/common/motors/dynamixel/tables.py @@ -0,0 +1,110 @@ +# data_name: (address, size_byte) +# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table +X_SERIES_CONTROL_TABLE = { + "Model_Number": (0, 2), + "Model_Information": (2, 4), + "Firmware_Version": (6, 1), + "ID": (7, 1), + "Baud_Rate": (8, 1), + "Return_Delay_Time": (9, 1), + "Drive_Mode": (10, 1), + "Operating_Mode": (11, 1), + "Secondary_ID": (12, 1), + "Protocol_Type": (13, 1), + "Homing_Offset": (20, 4), + "Moving_Threshold": (24, 4), + "Temperature_Limit": (31, 1), + "Max_Voltage_Limit": (32, 2), + "Min_Voltage_Limit": (34, 2), + "PWM_Limit": (36, 2), + "Current_Limit": (38, 2), + "Acceleration_Limit": (40, 4), + "Velocity_Limit": (44, 4), + "Max_Position_Limit": (48, 4), + "Min_Position_Limit": (52, 4), + "Shutdown": (63, 1), + "Torque_Enable": (64, 1), + "LED": (65, 1), + "Status_Return_Level": (68, 1), + "Registered_Instruction": (69, 1), + "Hardware_Error_Status": (70, 1), + "Velocity_I_Gain": (76, 2), + "Velocity_P_Gain": (78, 2), + "Position_D_Gain": (80, 2), + "Position_I_Gain": (82, 2), + "Position_P_Gain": (84, 2), + "Feedforward_2nd_Gain": (88, 2), + "Feedforward_1st_Gain": (90, 2), + "Bus_Watchdog": (98, 1), + "Goal_PWM": (100, 2), + "Goal_Current": (102, 2), + "Goal_Velocity": (104, 4), + "Profile_Acceleration": (108, 4), + "Profile_Velocity": (112, 4), + "Goal_Position": (116, 4), + "Realtime_Tick": (120, 2), + "Moving": (122, 1), + "Moving_Status": (123, 1), + "Present_PWM": (124, 2), + "Present_Current": (126, 2), + "Present_Velocity": (128, 4), + "Present_Position": (132, 4), + "Velocity_Trajectory": (136, 4), + "Position_Trajectory": (140, 4), + "Present_Input_Voltage": (144, 2), + "Present_Temperature": (146, 1), +} + +# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#baud-rate8 +X_SERIES_BAUDRATE_TABLE = { + 0: 9_600, + 1: 57_600, + 2: 115_200, + 3: 1_000_000, + 4: 2_000_000, + 5: 3_000_000, + 6: 4_000_000, +} + +# {model: model_resolution} +# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications +MODEL_RESOLUTION = { + "x_series": 4096, + "xl330-m077": 4096, + "xl330-m288": 4096, + "xl430-w250": 4096, + "xm430-w350": 4096, + "xm540-w270": 4096, + "xc430-w150": 4096, +} + +# {model: model_number} +# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area +MODELS_TABLE = { + "xl330-m077": 1190, + "xl330-m288": 1200, + "xl430-w250": 1060, + "xm430-w350": 1020, + "xm540-w270": 1120, + "xc430-w150": 1070, +} + +MODEL_CONTROL_TABLE = { + "x_series": X_SERIES_CONTROL_TABLE, + "xl330-m077": X_SERIES_CONTROL_TABLE, + "xl330-m288": X_SERIES_CONTROL_TABLE, + "xl430-w250": X_SERIES_CONTROL_TABLE, + "xm430-w350": X_SERIES_CONTROL_TABLE, + "xm540-w270": X_SERIES_CONTROL_TABLE, + "xc430-w150": X_SERIES_CONTROL_TABLE, +} + +MODEL_BAUDRATE_TABLE = { + "x_series": X_SERIES_BAUDRATE_TABLE, + "xl330-m077": X_SERIES_BAUDRATE_TABLE, + "xl330-m288": X_SERIES_BAUDRATE_TABLE, + "xl430-w250": X_SERIES_BAUDRATE_TABLE, + "xm430-w350": X_SERIES_BAUDRATE_TABLE, + "xm540-w270": X_SERIES_BAUDRATE_TABLE, + "xc430-w150": X_SERIES_BAUDRATE_TABLE, +} diff --git a/tests/mocks/mock_dynamixel.py b/tests/mocks/mock_dynamixel.py index e45ff1b4..3f55f058 100644 --- a/tests/mocks/mock_dynamixel.py +++ b/tests/mocks/mock_dynamixel.py @@ -6,7 +6,7 @@ import dynamixel_sdk as dxl import serial from mock_serial import MockSerial -from lerobot.common.motors.dynamixel.dynamixel import X_SERIES_CONTROL_TABLE +from lerobot.common.motors.dynamixel import X_SERIES_CONTROL_TABLE # https://emanual.robotis.com/docs/en/dxl/crc/ DXL_CRC_TABLE = [