From 9e34c1d731edd689a3df4e405dc6b454ffafb9ca Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Sat, 22 Mar 2025 01:24:48 +0100 Subject: [PATCH] Move feetech table & cleanup --- lerobot/common/motors/feetech/__init__.py | 7 +- lerobot/common/motors/feetech/feetech.py | 100 ++-------------------- lerobot/common/motors/feetech/tables.py | 80 +++++++++++++++++ tests/mocks/mock_feetech.py | 2 +- 4 files changed, 88 insertions(+), 101 deletions(-) create mode 100644 lerobot/common/motors/feetech/tables.py diff --git a/lerobot/common/motors/feetech/__init__.py b/lerobot/common/motors/feetech/__init__.py index 0134b92d..ae61d23b 100644 --- a/lerobot/common/motors/feetech/__init__.py +++ b/lerobot/common/motors/feetech/__init__.py @@ -1,8 +1,3 @@ from .feetech import FeetechMotorsBus from .feetech_calibration import apply_feetech_offsets_from_calibration, run_full_arm_calibration - -__all__ = [ - "FeetechMotorsBus", - "apply_feetech_offsets_from_calibration", - "run_full_arm_calibration", -] +from .tables import * diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index 5a315bf9..78fae13a 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -15,6 +15,12 @@ from copy import deepcopy from ..motors_bus import MotorsBus +from .tables import ( + CALIBRATION_REQUIRED, + MODEL_BAUDRATE_TABLE, + MODEL_CONTROL_TABLE, + MODEL_RESOLUTION, +) PROTOCOL_VERSION = 0 BAUDRATE = 1_000_000 @@ -22,100 +28,6 @@ DEFAULT_TIMEOUT_MS = 1000 MAX_ID_RANGE = 252 -# 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 - -# See this link for STS3215 Memory Table: -# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true -# data_name: (address, size_byte) -SCS_SERIES_CONTROL_TABLE = { - "Model": (3, 2), - "ID": (5, 1), - "Baud_Rate": (6, 1), - "Return_Delay": (7, 1), - "Response_Status_Level": (8, 1), - "Min_Angle_Limit": (9, 2), - "Max_Angle_Limit": (11, 2), - "Max_Temperature_Limit": (13, 1), - "Max_Voltage_Limit": (14, 1), - "Min_Voltage_Limit": (15, 1), - "Max_Torque_Limit": (16, 2), - "Phase": (18, 1), - "Unloading_Condition": (19, 1), - "LED_Alarm_Condition": (20, 1), - "P_Coefficient": (21, 1), - "D_Coefficient": (22, 1), - "I_Coefficient": (23, 1), - "Minimum_Startup_Force": (24, 2), - "CW_Dead_Zone": (26, 1), - "CCW_Dead_Zone": (27, 1), - "Protection_Current": (28, 2), - "Angular_Resolution": (30, 1), - "Offset": (31, 2), - "Mode": (33, 1), - "Protective_Torque": (34, 1), - "Protection_Time": (35, 1), - "Overload_Torque": (36, 1), - "Speed_closed_loop_P_proportional_coefficient": (37, 1), - "Over_Current_Protection_Time": (38, 1), - "Velocity_closed_loop_I_integral_coefficient": (39, 1), - "Torque_Enable": (40, 1), - "Acceleration": (41, 1), - "Goal_Position": (42, 2), - "Goal_Time": (44, 2), - "Goal_Speed": (46, 2), - "Torque_Limit": (48, 2), - "Lock": (55, 1), - "Present_Position": (56, 2), - "Present_Speed": (58, 2), - "Present_Load": (60, 2), - "Present_Voltage": (62, 1), - "Present_Temperature": (63, 1), - "Status": (65, 1), - "Moving": (66, 1), - "Present_Current": (69, 2), - # Not in the Memory Table - "Maximum_Acceleration": (85, 2), -} - -SCS_SERIES_BAUDRATE_TABLE = { - 0: 1_000_000, - 1: 500_000, - 2: 250_000, - 3: 128_000, - 4: 115_200, - 5: 57_600, - 6: 38_400, - 7: 19_200, -} - -CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] - -MODEL_CONTROL_TABLE = { - "scs_series": SCS_SERIES_CONTROL_TABLE, - "sts3215": SCS_SERIES_CONTROL_TABLE, -} - -MODEL_RESOLUTION = { - "scs_series": 4096, - "sts3215": 4096, -} - -MODEL_BAUDRATE_TABLE = { - "scs_series": SCS_SERIES_BAUDRATE_TABLE, - "sts3215": SCS_SERIES_BAUDRATE_TABLE, -} - -# High number of retries is needed for feetech compared to dynamixel motors. -NUM_READ_RETRY = 20 -NUM_WRITE_RETRY = 20 - class FeetechMotorsBus(MotorsBus): """ diff --git a/lerobot/common/motors/feetech/tables.py b/lerobot/common/motors/feetech/tables.py new file mode 100644 index 00000000..5de95b61 --- /dev/null +++ b/lerobot/common/motors/feetech/tables.py @@ -0,0 +1,80 @@ +# See this link for STS3215 Memory Table: +# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true +# data_name: (address, size_byte) +SCS_SERIES_CONTROL_TABLE = { + "Model": (3, 2), + "ID": (5, 1), + "Baud_Rate": (6, 1), + "Return_Delay": (7, 1), + "Response_Status_Level": (8, 1), + "Min_Angle_Limit": (9, 2), + "Max_Angle_Limit": (11, 2), + "Max_Temperature_Limit": (13, 1), + "Max_Voltage_Limit": (14, 1), + "Min_Voltage_Limit": (15, 1), + "Max_Torque_Limit": (16, 2), + "Phase": (18, 1), + "Unloading_Condition": (19, 1), + "LED_Alarm_Condition": (20, 1), + "P_Coefficient": (21, 1), + "D_Coefficient": (22, 1), + "I_Coefficient": (23, 1), + "Minimum_Startup_Force": (24, 2), + "CW_Dead_Zone": (26, 1), + "CCW_Dead_Zone": (27, 1), + "Protection_Current": (28, 2), + "Angular_Resolution": (30, 1), + "Offset": (31, 2), + "Mode": (33, 1), + "Protective_Torque": (34, 1), + "Protection_Time": (35, 1), + "Overload_Torque": (36, 1), + "Speed_closed_loop_P_proportional_coefficient": (37, 1), + "Over_Current_Protection_Time": (38, 1), + "Velocity_closed_loop_I_integral_coefficient": (39, 1), + "Torque_Enable": (40, 1), + "Acceleration": (41, 1), + "Goal_Position": (42, 2), + "Goal_Time": (44, 2), + "Goal_Speed": (46, 2), + "Torque_Limit": (48, 2), + "Lock": (55, 1), + "Present_Position": (56, 2), + "Present_Speed": (58, 2), + "Present_Load": (60, 2), + "Present_Voltage": (62, 1), + "Present_Temperature": (63, 1), + "Status": (65, 1), + "Moving": (66, 1), + "Present_Current": (69, 2), + # Not in the Memory Table + "Maximum_Acceleration": (85, 2), +} + +SCS_SERIES_BAUDRATE_TABLE = { + 0: 1_000_000, + 1: 500_000, + 2: 250_000, + 3: 128_000, + 4: 115_200, + 5: 57_600, + 6: 38_400, + 7: 19_200, +} + +MODEL_CONTROL_TABLE = { + "scs_series": SCS_SERIES_CONTROL_TABLE, + "sts3215": SCS_SERIES_CONTROL_TABLE, +} + +MODEL_RESOLUTION = { + "scs_series": 4096, + "sts3215": 4096, +} + +MODEL_BAUDRATE_TABLE = { + "scs_series": SCS_SERIES_BAUDRATE_TABLE, + "sts3215": SCS_SERIES_BAUDRATE_TABLE, +} + +CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] diff --git a/tests/mocks/mock_feetech.py b/tests/mocks/mock_feetech.py index e0a38042..7ff994ee 100644 --- a/tests/mocks/mock_feetech.py +++ b/tests/mocks/mock_feetech.py @@ -6,7 +6,7 @@ import scservo_sdk as scs import serial from mock_serial import MockSerial -from lerobot.common.motors.feetech.feetech import SCS_SERIES_CONTROL_TABLE +from lerobot.common.motors.feetech.tables import SCS_SERIES_CONTROL_TABLE # https://files.waveshare.com/upload/2/27/Communication_Protocol_User_Manual-EN%28191218-0923%29.pdf INSTRUCTION_TYPES = {