From f0505e81ccbf9cb856be6616b395211a3421cff3 Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Fri, 14 Mar 2025 22:00:09 +0100 Subject: [PATCH] Move common Feetech/Dxl code into MotorsBus base class --- lerobot/common/motors/dynamixel/dynamixel.py | 299 +----- .../common/motors/dynamixel/dynamixel_old.py | 895 ++++++++++++++++++ lerobot/common/motors/feetech/feetech.py | 288 +----- lerobot/common/motors/motors_bus.py | 340 ++++++- 4 files changed, 1268 insertions(+), 554 deletions(-) create mode 100644 lerobot/common/motors/dynamixel/dynamixel_old.py diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index a09b1847..bced8775 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -12,18 +12,21 @@ # See the License for the specific language governing permissions and # limitations under the License. -import enum import logging import math -import time -import traceback from copy import deepcopy +import dynamixel_sdk as dxl import numpy as np -import tqdm -from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError -from lerobot.common.utils.utils import capture_timestamp_utc +from ..motors_bus import ( + CalibrationMode, + JointOutOfRangeError, + MotorsBus, + TorqueMode, + assert_same_address, + get_group_sync_key, +) PROTOCOL_VERSION = 2.0 BAUDRATE = 1_000_000 @@ -38,6 +41,7 @@ MAX_ID_RANGE = 252 # 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 @@ -200,72 +204,7 @@ def convert_to_bytes(value, bytes, mock=False): return data -def get_group_sync_key(data_name, motor_names): - group_key = f"{data_name}_" + "_".join(motor_names) - return group_key - - -def get_result_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - rslt_name = f"{fn_name}_{group_key}" - return rslt_name - - -def get_queue_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - queue_name = f"{fn_name}_{group_key}" - return queue_name - - -def get_log_name(var_name, fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - log_name = f"{var_name}_{fn_name}_{group_key}" - return log_name - - -def assert_same_address(model_ctrl_table, motor_models, data_name): - all_addr = [] - all_bytes = [] - for model in motor_models: - addr, bytes = model_ctrl_table[model][data_name] - all_addr.append(addr) - all_bytes.append(bytes) - - if len(set(all_addr)) != 1: - raise NotImplementedError( - f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer." - ) - - if len(set(all_bytes)) != 1: - raise NotImplementedError( - f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer." - ) - - -class TorqueMode(enum.Enum): - ENABLED = 1 - DISABLED = 0 - - -class DriveMode(enum.Enum): - NON_INVERTED = 0 - INVERTED = 1 - - -class CalibrationMode(enum.Enum): - # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] - DEGREE = 0 - # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100] - LINEAR = 1 - - -class JointOutOfRangeError(Exception): - def __init__(self, message="Joint is out of range"): - self.message = message - super().__init__(self.message) - - -class DynamixelMotorsBus: +class DynamixelMotorsBus(MotorsBus): """ The DynamixelMotorsBus class allows to efficiently read and write to the attached motors. It relies on the python dynamixel sdk to communicate with the motors. For more info, see the [Dynamixel SDK Documentation](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20). @@ -304,124 +243,24 @@ class DynamixelMotorsBus: ``` """ + model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) + model_resolution = deepcopy(MODEL_RESOLUTION) + def __init__( self, port: str, motors: dict[str, tuple[int, str]], - mock: bool = False, ): - self.port = port - self.motors = motors - self.mock = mock - - self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) - self.model_resolution = deepcopy(MODEL_RESOLUTION) - - self.port_handler = None - self.packet_handler = None - self.calibration = None - self.is_connected = False - self.group_readers = {} - self.group_writers = {} - self.logs = {} - - def connect(self): - if self.is_connected: - raise DeviceAlreadyConnectedError( - f"DynamixelMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." - ) - - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl + super().__init__(port, motors) + def _set_handlers(self): self.port_handler = dxl.PortHandler(self.port) self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION) - try: - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - except Exception: - traceback.print_exc() - print( - "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n" - ) - raise + def _set_timeout(self, timeout: int = TIMEOUT_MS): + self.port_handler.setPacketTimeoutMillis(timeout) - # Allow to read and write - self.is_connected = True - - self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS) - - def reconnect(self): - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl - - self.port_handler = dxl.PortHandler(self.port) - self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION) - - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - - self.is_connected = True - - def are_motors_configured(self): - # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, - # a ConnectionError will be raised anyway. - try: - return (self.motor_indices == self.read("ID")).all() - except ConnectionError as e: - print(e) - return False - - def find_motor_indices(self, possible_ids=None, num_retry=2): - if possible_ids is None: - possible_ids = range(MAX_ID_RANGE) - - indices = [] - for idx in tqdm.tqdm(possible_ids): - try: - present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0] - except ConnectionError: - continue - - if idx != present_idx: - # sanity check - raise OSError( - "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged." - ) - indices.append(idx) - - return indices - - def set_bus_baudrate(self, baudrate): - present_bus_baudrate = self.port_handler.getBaudRate() - if present_bus_baudrate != baudrate: - print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.") - self.port_handler.setBaudRate(baudrate) - - if self.port_handler.getBaudRate() != baudrate: - raise OSError("Failed to write bus baud rate.") - - @property - def motor_names(self) -> list[str]: - return list(self.motors.keys()) - - @property - def motor_models(self) -> list[str]: - return [model for _, model in self.motors.values()] - - @property - def motor_indices(self) -> list[int]: - return [idx for idx, _ in self.motors.values()] - - def set_calibration(self, calibration: dict[str, list]): - self.calibration = calibration - - def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): + def _apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): """This function applies the calibration, automatically detects out of range errors for motors values and attempts to correct. For more info, see docstring of `apply_calibration` and `autocorrect_calibration`. @@ -430,7 +269,7 @@ class DynamixelMotorsBus: values = self.apply_calibration(values, motor_names) except JointOutOfRangeError as e: print(e) - self.autocorrect_calibration(values, motor_names) + self._autocorrect_calibration(values, motor_names) values = self.apply_calibration(values, motor_names) return values @@ -509,7 +348,7 @@ class DynamixelMotorsBus: return values - def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): + def _autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): """This function automatically detects issues with values of motors after calibration, and correct for these issues. Some motors might have values outside of expected maximum bounds after calibration. @@ -606,6 +445,7 @@ class DynamixelMotorsBus: self.calibration["homing_offset"][calib_idx] += resolution * factor def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): + # TODO(aliberts): remove np """Inverse of `apply_calibration`.""" if motor_names is None: motor_names = self.motor_names @@ -645,11 +485,6 @@ class DynamixelMotorsBus: return values def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl - return_list = True if not isinstance(motor_ids, list): return_list = False @@ -682,25 +517,7 @@ class DynamixelMotorsBus: else: return values[0] - def read(self, data_name, motor_names: str | list[str] | None = None): - if not self.is_connected: - raise DeviceNotConnectedError( - f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - + def _read(self, data_name, motor_names: str | list[str] | None = None): motor_ids = [] models = [] for name in motor_names: @@ -743,24 +560,11 @@ class DynamixelMotorsBus: values = values.astype(np.int32) if data_name in CALIBRATION_REQUIRED and self.calibration is not None: - values = self.apply_calibration_autocorrect(values, motor_names) - - # log the number of seconds it took to read the data from the motors - delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # log the utc time at which the data was received - ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() + values = self._apply_calibration_autocorrect(values, motor_names) return values def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY): - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl - if not isinstance(motor_ids, list): motor_ids = [motor_ids] if not isinstance(values, list): @@ -784,30 +588,7 @@ class DynamixelMotorsBus: f"{self.packet_handler.getTxRxResult(comm)}" ) - def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None): - if not self.is_connected: - raise DeviceNotConnectedError( - f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if self.mock: - import tests.motors.mock_dynamixel_sdk as dxl - else: - import dynamixel_sdk as dxl - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - - if isinstance(values, (int, float, np.integer)): - values = [int(values)] * len(motor_names) - - values = np.array(values) - + def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None: motor_ids = [] models = [] for name in motor_names: @@ -818,8 +599,6 @@ class DynamixelMotorsBus: if data_name in CALIBRATION_REQUIRED and self.calibration is not None: values = self.revert_calibration(values, motor_names) - values = values.tolist() - assert_same_address(self.model_ctrl_table, models, data_name) addr, bytes = self.model_ctrl_table[model][data_name] group_key = get_group_sync_key(data_name, motor_names) @@ -844,34 +623,6 @@ class DynamixelMotorsBus: f"{self.packet_handler.getTxRxResult(comm)}" ) - # log the number of seconds it took to write the data to the motors - delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # TODO(rcadene): should we log the time before sending the write command? - # log the utc time when the write has been completed - ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() - - def disconnect(self): - if not self.is_connected: - raise DeviceNotConnectedError( - f"DynamixelMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." - ) - - if self.port_handler is not None: - self.port_handler.closePort() - self.port_handler = None - - self.packet_handler = None - self.group_readers = {} - self.group_writers = {} - self.is_connected = False - - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() - def set_operating_mode(arm: DynamixelMotorsBus): if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): diff --git a/lerobot/common/motors/dynamixel/dynamixel_old.py b/lerobot/common/motors/dynamixel/dynamixel_old.py new file mode 100644 index 00000000..55f6069b --- /dev/null +++ b/lerobot/common/motors/dynamixel/dynamixel_old.py @@ -0,0 +1,895 @@ +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import enum +import logging +import math +import time +import traceback +from copy import deepcopy + +import numpy as np +import tqdm + +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError +from lerobot.common.utils.utils import capture_timestamp_utc + +PROTOCOL_VERSION = 2.0 +BAUDRATE = 1_000_000 +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 + + +def convert_to_bytes(value, bytes, mock=False): + if mock: + return value + + import dynamixel_sdk as dxl + + # Note: No need to convert back into unsigned int, since this byte preprocessing + # already handles it for us. + if bytes == 1: + data = [ + dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)), + ] + elif bytes == 2: + data = [ + dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)), + dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)), + ] + elif bytes == 4: + data = [ + dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)), + dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)), + dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)), + dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)), + ] + else: + raise NotImplementedError( + f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but " + f"{bytes} is provided instead." + ) + return data + + +def get_group_sync_key(data_name, motor_names): + group_key = f"{data_name}_" + "_".join(motor_names) + return group_key + + +def get_result_name(fn_name, data_name, motor_names): + group_key = get_group_sync_key(data_name, motor_names) + rslt_name = f"{fn_name}_{group_key}" + return rslt_name + + +def get_queue_name(fn_name, data_name, motor_names): + group_key = get_group_sync_key(data_name, motor_names) + queue_name = f"{fn_name}_{group_key}" + return queue_name + + +def get_log_name(var_name, fn_name, data_name, motor_names): + group_key = get_group_sync_key(data_name, motor_names) + log_name = f"{var_name}_{fn_name}_{group_key}" + return log_name + + +def assert_same_address(model_ctrl_table, motor_models, data_name): + all_addr = [] + all_bytes = [] + for model in motor_models: + addr, bytes = model_ctrl_table[model][data_name] + all_addr.append(addr) + all_bytes.append(bytes) + + if len(set(all_addr)) != 1: + raise NotImplementedError( + f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer." + ) + + if len(set(all_bytes)) != 1: + raise NotImplementedError( + f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer." + ) + + +class TorqueMode(enum.Enum): + ENABLED = 1 + DISABLED = 0 + + +class DriveMode(enum.Enum): + NON_INVERTED = 0 + INVERTED = 1 + + +class CalibrationMode(enum.Enum): + # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] + DEGREE = 0 + # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100] + LINEAR = 1 + + +class JointOutOfRangeError(Exception): + def __init__(self, message="Joint is out of range"): + self.message = message + super().__init__(self.message) + + +class DynamixelMotorsBus: + """ + The DynamixelMotorsBus class allows to efficiently read and write to the attached motors. It relies on + the python dynamixel sdk to communicate with the motors. For more info, see the [Dynamixel SDK Documentation](https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20). + + A DynamixelMotorsBus instance requires a port (e.g. `DynamixelMotorsBus(port="/dev/tty.usbmodem575E0031751"`)). + To find the port, you can run our utility script: + ```bash + python lerobot/scripts/find_motors_bus_port.py + >>> Finding all available ports for the MotorBus. + >>> ['/dev/tty.usbmodem575E0032081', '/dev/tty.usbmodem575E0031751'] + >>> Remove the usb cable from your DynamixelMotorsBus and press Enter when done. + >>> The port of this DynamixelMotorsBus is /dev/tty.usbmodem575E0031751. + >>> Reconnect the usb cable. + ``` + + Example of usage for 1 motor connected to the bus: + ```python + motor_name = "gripper" + motor_index = 6 + motor_model = "xl330-m288" + + motors_bus = DynamixelMotorsBus( + port="/dev/tty.usbmodem575E0031751", + motors={motor_name: (motor_index, motor_model)}, + ) + motors_bus.connect() + + position = motors_bus.read("Present_Position") + + # move from a few motor steps as an example + few_steps = 30 + motors_bus.write("Goal_Position", position + few_steps) + + # when done, consider disconnecting + motors_bus.disconnect() + ``` + """ + + def __init__( + self, + port: str, + motors: dict[str, tuple[int, str]], + mock: bool = False, + ): + self.port = port + self.motors = motors + self.mock = mock + + self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) + self.model_resolution = deepcopy(MODEL_RESOLUTION) + + self.port_handler = None + self.packet_handler = None + self.calibration = None + self.is_connected = False + self.group_readers = {} + self.group_writers = {} + self.logs = {} + + def connect(self): + if self.is_connected: + raise DeviceAlreadyConnectedError( + f"DynamixelMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." + ) + + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + self.port_handler = dxl.PortHandler(self.port) + self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION) + + try: + if not self.port_handler.openPort(): + raise OSError(f"Failed to open port '{self.port}'.") + except Exception: + traceback.print_exc() + print( + "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n" + ) + raise + + # Allow to read and write + self.is_connected = True + + self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS) + + def reconnect(self): + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + self.port_handler = dxl.PortHandler(self.port) + self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION) + + if not self.port_handler.openPort(): + raise OSError(f"Failed to open port '{self.port}'.") + + self.is_connected = True + + def are_motors_configured(self): + # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, + # a ConnectionError will be raised anyway. + try: + return (self.motor_indices == self.read("ID")).all() + except ConnectionError as e: + print(e) + return False + + def find_motor_indices(self, possible_ids=None, num_retry=2): + if possible_ids is None: + possible_ids = range(MAX_ID_RANGE) + + indices = [] + for idx in tqdm.tqdm(possible_ids): + try: + present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0] + except ConnectionError: + continue + + if idx != present_idx: + # sanity check + raise OSError( + "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged." + ) + indices.append(idx) + + return indices + + def set_bus_baudrate(self, baudrate): + present_bus_baudrate = self.port_handler.getBaudRate() + if present_bus_baudrate != baudrate: + print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.") + self.port_handler.setBaudRate(baudrate) + + if self.port_handler.getBaudRate() != baudrate: + raise OSError("Failed to write bus baud rate.") + + @property + def motor_names(self) -> list[str]: + return list(self.motors.keys()) + + @property + def motor_models(self) -> list[str]: + return [model for _, model in self.motors.values()] + + @property + def motor_indices(self) -> list[int]: + return [idx for idx, _ in self.motors.values()] + + def set_calibration(self, calibration: dict[str, list]): + self.calibration = calibration + + def apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None): + """This function applies the calibration, automatically detects out of range errors for motors values and attempts to correct. + + For more info, see docstring of `apply_calibration` and `autocorrect_calibration`. + """ + try: + values = self.apply_calibration(values, motor_names) + except JointOutOfRangeError as e: + print(e) + self.autocorrect_calibration(values, motor_names) + values = self.apply_calibration(values, motor_names) + return values + + def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): + """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with + a "zero position" at 0 degree. + + Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor + rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range. + + Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation + when given a goal position that is + or - their resolution. For instance, dynamixel xl330-m077 have a resolution of 4096, and + at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830, + or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor. + To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work + in the centered nominal degree range ]-180, 180[. + """ + if motor_names is None: + motor_names = self.motor_names + + # Convert from unsigned int32 original range [0, 2**32] to signed float32 range + values = values.astype(np.float32) + + for i, name in enumerate(motor_names): + calib_idx = self.calibration["motor_names"].index(name) + calib_mode = self.calibration["calib_mode"][calib_idx] + + if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: + drive_mode = self.calibration["drive_mode"][calib_idx] + homing_offset = self.calibration["homing_offset"][calib_idx] + _, model = self.motors[name] + resolution = self.model_resolution[model] + + # Update direction of rotation of the motor to match between leader and follower. + # In fact, the motor of the leader for a given joint can be assembled in an + # opposite direction in term of rotation than the motor of the follower on the same joint. + if drive_mode: + values[i] *= -1 + + # Convert from range [-2**31, 2**31] to + # nominal range [-resolution//2, resolution//2] (e.g. [-2048, 2048]) + values[i] += homing_offset + + # Convert from range [-resolution//2, resolution//2] to + # universal float32 centered degree range [-180, 180] + # (e.g. 2048 / (4096 // 2) * 180 = 180) + values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE + + if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE): + raise JointOutOfRangeError( + f"Wrong motor position range detected for {name}. " + f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), " + f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, " + f"but present value is {values[i]} degree. " + "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. " + "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`" + ) + + elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: + start_pos = self.calibration["start_pos"][calib_idx] + end_pos = self.calibration["end_pos"][calib_idx] + + # Rescale the present position to a nominal range [0, 100] %, + # useful for joints with linear motions like Aloha gripper + values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100 + + if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR): + raise JointOutOfRangeError( + f"Wrong motor position range detected for {name}. " + f"Expected to be in nominal range of [0, 100] % (a full linear translation), " + f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, " + f"but present value is {values[i]} %. " + "This might be due to a cable connection issue creating an artificial jump in motor values. " + "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`" + ) + + return values + + def autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): + """This function automatically detects issues with values of motors after calibration, and correct for these issues. + + Some motors might have values outside of expected maximum bounds after calibration. + For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given + a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position. + + Known issues: + #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors. + #2: Motor internal homing offset is shifted by a full turn, caused by using default calibration (e.g Aloha). + #3: motor internal homing offset is shifted by less or more than a full turn, caused by using default calibration + or by human error during manual calibration. + + Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn. + Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`, + that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue. + + Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. + """ + if motor_names is None: + motor_names = self.motor_names + + # Convert from unsigned int32 original range [0, 2**32] to signed float32 range + values = values.astype(np.float32) + + for i, name in enumerate(motor_names): + calib_idx = self.calibration["motor_names"].index(name) + calib_mode = self.calibration["calib_mode"][calib_idx] + + if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: + drive_mode = self.calibration["drive_mode"][calib_idx] + homing_offset = self.calibration["homing_offset"][calib_idx] + _, model = self.motors[name] + resolution = self.model_resolution[model] + + # Update direction of rotation of the motor to match between leader and follower. + # In fact, the motor of the leader for a given joint can be assembled in an + # opposite direction in term of rotation than the motor of the follower on the same joint. + if drive_mode: + values[i] *= -1 + + # Convert from initial range to range [-180, 180] degrees + calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE + in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE) + + # Solve this inequality to find the factor to shift the range into [-180, 180] degrees + # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE + # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE + # (- (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= ((resolution // 2) - values[i] - homing_offset) / resolution + low_factor = (-(resolution // 2) - values[i] - homing_offset) / resolution + upp_factor = ((resolution // 2) - values[i] - homing_offset) / resolution + + elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: + start_pos = self.calibration["start_pos"][calib_idx] + end_pos = self.calibration["end_pos"][calib_idx] + + # Convert from initial range to range [0, 100] in % + calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100 + in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR) + + # Solve this inequality to find the factor to shift the range into [0, 100] % + # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100 + # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 + # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100 + # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution + low_factor = (start_pos - values[i]) / resolution + upp_factor = (end_pos - values[i]) / resolution + + if not in_range: + # Get first integer between the two bounds + if low_factor < upp_factor: + factor = math.ceil(low_factor) + + if factor > upp_factor: + raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]") + else: + factor = math.ceil(upp_factor) + + if factor > low_factor: + raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]") + + if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: + out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" + in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees" + elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: + out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" + in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %" + + logging.warning( + f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, " + f"from '{out_of_range_str}' to '{in_range_str}'." + ) + + # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096. + self.calibration["homing_offset"][calib_idx] += resolution * factor + + def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): + """Inverse of `apply_calibration`.""" + if motor_names is None: + motor_names = self.motor_names + + for i, name in enumerate(motor_names): + calib_idx = self.calibration["motor_names"].index(name) + calib_mode = self.calibration["calib_mode"][calib_idx] + + if CalibrationMode[calib_mode] == CalibrationMode.DEGREE: + drive_mode = self.calibration["drive_mode"][calib_idx] + homing_offset = self.calibration["homing_offset"][calib_idx] + _, model = self.motors[name] + resolution = self.model_resolution[model] + + # Convert from nominal 0-centered degree range [-180, 180] to + # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096) + values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2) + + # Subtract the homing offsets to come back to actual motor range of values + # which can be arbitrary. + values[i] -= homing_offset + + # Remove drive mode, which is the rotation direction of the motor, to come back to + # actual motor rotation direction which can be arbitrary. + if drive_mode: + values[i] *= -1 + + elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR: + start_pos = self.calibration["start_pos"][calib_idx] + end_pos = self.calibration["end_pos"][calib_idx] + + # Convert from nominal lnear range of [0, 100] % to + # actual motor range of values which can be arbitrary. + values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos + + values = np.round(values).astype(np.int32) + return values + + def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + return_list = True + if not isinstance(motor_ids, list): + return_list = False + motor_ids = [motor_ids] + + assert_same_address(self.model_ctrl_table, self.motor_models, data_name) + addr, bytes = self.model_ctrl_table[motor_models[0]][data_name] + group = dxl.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes) + for idx in motor_ids: + group.addParam(idx) + + for _ in range(num_retry): + comm = group.txRxPacket() + if comm == dxl.COMM_SUCCESS: + break + + if comm != dxl.COMM_SUCCESS: + raise ConnectionError( + f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " + f"{self.packet_handler.getTxRxResult(comm)}" + ) + + values = [] + for idx in motor_ids: + value = group.getData(idx, addr, bytes) + values.append(value) + + if return_list: + return values + else: + return values[0] + + def read(self, data_name, motor_names: str | list[str] | None = None): + if not self.is_connected: + raise DeviceNotConnectedError( + f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." + ) + + start_time = time.perf_counter() + + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + if motor_names is None: + motor_names = self.motor_names + + if isinstance(motor_names, str): + motor_names = [motor_names] + + motor_ids = [] + models = [] + for name in motor_names: + motor_idx, model = self.motors[name] + motor_ids.append(motor_idx) + models.append(model) + + assert_same_address(self.model_ctrl_table, models, data_name) + addr, bytes = self.model_ctrl_table[model][data_name] + group_key = get_group_sync_key(data_name, motor_names) + + if data_name not in self.group_readers: + # create new group reader + self.group_readers[group_key] = dxl.GroupSyncRead( + self.port_handler, self.packet_handler, addr, bytes + ) + for idx in motor_ids: + self.group_readers[group_key].addParam(idx) + + for _ in range(NUM_READ_RETRY): + comm = self.group_readers[group_key].txRxPacket() + if comm == dxl.COMM_SUCCESS: + break + + if comm != dxl.COMM_SUCCESS: + raise ConnectionError( + f"Read failed due to communication error on port {self.port} for group_key {group_key}: " + f"{self.packet_handler.getTxRxResult(comm)}" + ) + + values = [] + for idx in motor_ids: + value = self.group_readers[group_key].getData(idx, addr, bytes) + values.append(value) + + values = np.array(values) + + # Convert to signed int to use range [-2048, 2048] for our motor positions. + if data_name in CONVERT_UINT32_TO_INT32_REQUIRED: + values = values.astype(np.int32) + + if data_name in CALIBRATION_REQUIRED and self.calibration is not None: + values = self.apply_calibration_autocorrect(values, motor_names) + + # log the number of seconds it took to read the data from the motors + delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) + self.logs[delta_ts_name] = time.perf_counter() - start_time + + # log the utc time at which the data was received + ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names) + self.logs[ts_utc_name] = capture_timestamp_utc() + + return values + + def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY): + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + if not isinstance(motor_ids, list): + motor_ids = [motor_ids] + if not isinstance(values, list): + values = [values] + + assert_same_address(self.model_ctrl_table, motor_models, data_name) + addr, bytes = self.model_ctrl_table[motor_models[0]][data_name] + group = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes) + for idx, value in zip(motor_ids, values, strict=True): + data = convert_to_bytes(value, bytes, self.mock) + group.addParam(idx, data) + + for _ in range(num_retry): + comm = group.txPacket() + if comm == dxl.COMM_SUCCESS: + break + + if comm != dxl.COMM_SUCCESS: + raise ConnectionError( + f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: " + f"{self.packet_handler.getTxRxResult(comm)}" + ) + + def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None): + if not self.is_connected: + raise DeviceNotConnectedError( + f"DynamixelMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." + ) + + start_time = time.perf_counter() + + if self.mock: + import tests.mock_dynamixel_sdk as dxl + else: + import dynamixel_sdk as dxl + + if motor_names is None: + motor_names = self.motor_names + + if isinstance(motor_names, str): + motor_names = [motor_names] + + if isinstance(values, (int, float, np.integer)): + values = [int(values)] * len(motor_names) + + values = np.array(values) + + motor_ids = [] + models = [] + for name in motor_names: + motor_idx, model = self.motors[name] + motor_ids.append(motor_idx) + models.append(model) + + if data_name in CALIBRATION_REQUIRED and self.calibration is not None: + values = self.revert_calibration(values, motor_names) + + values = values.tolist() + + assert_same_address(self.model_ctrl_table, models, data_name) + addr, bytes = self.model_ctrl_table[model][data_name] + group_key = get_group_sync_key(data_name, motor_names) + + init_group = data_name not in self.group_readers + if init_group: + self.group_writers[group_key] = dxl.GroupSyncWrite( + self.port_handler, self.packet_handler, addr, bytes + ) + + for idx, value in zip(motor_ids, values, strict=True): + data = convert_to_bytes(value, bytes, self.mock) + if init_group: + self.group_writers[group_key].addParam(idx, data) + else: + self.group_writers[group_key].changeParam(idx, data) + + comm = self.group_writers[group_key].txPacket() + if comm != dxl.COMM_SUCCESS: + raise ConnectionError( + f"Write failed due to communication error on port {self.port} for group_key {group_key}: " + f"{self.packet_handler.getTxRxResult(comm)}" + ) + + # log the number of seconds it took to write the data to the motors + delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names) + self.logs[delta_ts_name] = time.perf_counter() - start_time + + # TODO(rcadene): should we log the time before sending the write command? + # log the utc time when the write has been completed + ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names) + self.logs[ts_utc_name] = capture_timestamp_utc() + + def disconnect(self): + if not self.is_connected: + raise DeviceNotConnectedError( + f"DynamixelMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." + ) + + if self.port_handler is not None: + self.port_handler.closePort() + self.port_handler = None + + self.packet_handler = None + self.group_readers = {} + self.group_writers = {} + self.is_connected = False + + def __del__(self): + if getattr(self, "is_connected", False): + self.disconnect() + + +def set_operating_mode(arm: DynamixelMotorsBus): + if (arm.read("Torque_Enable") != TorqueMode.DISABLED.value).any(): + raise ValueError("To run set robot preset, the torque must be disabled on all motors.") + + # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't + # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm, + # you could end up with a servo with a position 0 or 4095 at a crucial point See [ + # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11] + all_motors_except_gripper = [name for name in arm.motor_names if name != "gripper"] + if len(all_motors_except_gripper) > 0: + # 4 corresponds to Extended Position on Koch motors + arm.write("Operating_Mode", 4, all_motors_except_gripper) + + # Use 'position control current based' for gripper to be limited by the limit of the current. + # For the follower gripper, it means it can grasp an object without forcing too much even tho, + # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch). + # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger + # to make it move, and it will move back to its original target position when we release the force. + # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288" + arm.write("Operating_Mode", 5, "gripper") diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index 8a34d1d3..450d3efc 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -12,16 +12,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import enum -import time -import traceback from copy import deepcopy import numpy as np -import tqdm +import scservo_sdk as scs -from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError -from lerobot.common.utils.utils import capture_timestamp_utc +from ..motors_bus import ( + CalibrationMode, + JointOutOfRangeError, + MotorsBus, + assert_same_address, + get_group_sync_key, +) PROTOCOL_VERSION = 0 BAUDRATE = 1_000_000 @@ -215,72 +217,7 @@ def convert_to_bytes(value, bytes, mock=False): return data -def get_group_sync_key(data_name, motor_names): - group_key = f"{data_name}_" + "_".join(motor_names) - return group_key - - -def get_result_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - rslt_name = f"{fn_name}_{group_key}" - return rslt_name - - -def get_queue_name(fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - queue_name = f"{fn_name}_{group_key}" - return queue_name - - -def get_log_name(var_name, fn_name, data_name, motor_names): - group_key = get_group_sync_key(data_name, motor_names) - log_name = f"{var_name}_{fn_name}_{group_key}" - return log_name - - -def assert_same_address(model_ctrl_table, motor_models, data_name): - all_addr = [] - all_bytes = [] - for model in motor_models: - addr, bytes = model_ctrl_table[model][data_name] - all_addr.append(addr) - all_bytes.append(bytes) - - if len(set(all_addr)) != 1: - raise NotImplementedError( - f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer." - ) - - if len(set(all_bytes)) != 1: - raise NotImplementedError( - f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer." - ) - - -class TorqueMode(enum.Enum): - ENABLED = 1 - DISABLED = 0 - - -class DriveMode(enum.Enum): - NON_INVERTED = 0 - INVERTED = 1 - - -class CalibrationMode(enum.Enum): - # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] - DEGREE = 0 - # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100] - LINEAR = 1 - - -class JointOutOfRangeError(Exception): - def __init__(self, message="Joint is out of range"): - self.message = message - super().__init__(self.message) - - -class FeetechMotorsBus: +class FeetechMotorsBus(MotorsBus): """ The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the python feetech sdk to communicate with the motors. For more info, see the [feetech SDK Documentation](https://emanual.robotis.com/docs/en/software/feetech/feetech_sdk/sample_code/python_read_write_protocol_2_0/#python-read-write-protocol-20). @@ -319,122 +256,22 @@ class FeetechMotorsBus: ``` """ + model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) + model_resolution = deepcopy(MODEL_RESOLUTION) + def __init__( self, port: str, motors: dict[str, tuple[int, str]], - mock: bool = False, ): - self.port = port - self.motors = motors - self.mock = mock - - self.model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) - self.model_resolution = deepcopy(MODEL_RESOLUTION) - - self.port_handler = None - self.packet_handler = None - self.calibration = None - self.is_connected = False - self.group_readers = {} - self.group_writers = {} - self.logs = {} - - def connect(self): - if self.is_connected: - raise DeviceAlreadyConnectedError( - f"FeetechMotorsBus({self.port}) is already connected. Do not call `motors_bus.connect()` twice." - ) - - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs + super().__init__(port, motors) + def _set_handlers(self): self.port_handler = scs.PortHandler(self.port) self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION) - try: - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - except Exception: - traceback.print_exc() - print( - "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n" - ) - raise - - # Allow to read and write - self.is_connected = True - - self.port_handler.setPacketTimeoutMillis(TIMEOUT_MS) - - def reconnect(self): - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - self.port_handler = scs.PortHandler(self.port) - self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION) - - if not self.port_handler.openPort(): - raise OSError(f"Failed to open port '{self.port}'.") - - self.is_connected = True - - def are_motors_configured(self): - # Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, - # a ConnectionError will be raised anyway. - try: - return (self.motor_indices == self.read("ID")).all() - except ConnectionError as e: - print(e) - return False - - def find_motor_indices(self, possible_ids=None, num_retry=2): - if possible_ids is None: - possible_ids = range(MAX_ID_RANGE) - - indices = [] - for idx in tqdm.tqdm(possible_ids): - try: - present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0] - except ConnectionError: - continue - - if idx != present_idx: - # sanity check - raise OSError( - "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged." - ) - indices.append(idx) - - return indices - - def set_bus_baudrate(self, baudrate): - present_bus_baudrate = self.port_handler.getBaudRate() - if present_bus_baudrate != baudrate: - print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.") - self.port_handler.setBaudRate(baudrate) - - if self.port_handler.getBaudRate() != baudrate: - raise OSError("Failed to write bus baud rate.") - - @property - def motor_names(self) -> list[str]: - return list(self.motors.keys()) - - @property - def motor_models(self) -> list[str]: - return [model for _, model in self.motors.values()] - - @property - def motor_indices(self) -> list[int]: - return [idx for idx, _ in self.motors.values()] - - def set_calibration(self, calibration: dict[str, list]): - self.calibration = calibration + def _set_timeout(self, timeout: int = TIMEOUT_MS): + self.port_handler.setPacketTimeoutMillis(timeout) def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None): if motor_names is None: @@ -502,11 +339,6 @@ class FeetechMotorsBus: return values def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY): - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - return_list = True if not isinstance(motor_ids, list): return_list = False @@ -539,25 +371,7 @@ class FeetechMotorsBus: else: return values[0] - def read(self, data_name, motor_names: str | list[str] | None = None): - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - if not self.is_connected: - raise DeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - + def _read(self, data_name, motor_names: str | list[str] | None = None): motor_ids = [] models = [] for name in motor_names: @@ -602,22 +416,9 @@ class FeetechMotorsBus: if data_name in CALIBRATION_REQUIRED and self.calibration is not None: values = self.apply_calibration(values, motor_names) - # log the number of seconds it took to read the data from the motors - delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # log the utc time at which the data was received - ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() - return values def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY): - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - if not isinstance(motor_ids, list): motor_ids = [motor_ids] if not isinstance(values, list): @@ -641,30 +442,7 @@ class FeetechMotorsBus: f"{self.packet_handler.getTxRxResult(comm)}" ) - def write(self, data_name, values: int | float | np.ndarray, motor_names: str | list[str] | None = None): - if not self.is_connected: - raise DeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. You need to run `motors_bus.connect()`." - ) - - start_time = time.perf_counter() - - if self.mock: - import tests.motors.mock_scservo_sdk as scs - else: - import scservo_sdk as scs - - if motor_names is None: - motor_names = self.motor_names - - if isinstance(motor_names, str): - motor_names = [motor_names] - - if isinstance(values, (int, float, np.integer)): - values = [int(values)] * len(motor_names) - - values = np.array(values) - + def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None: motor_ids = [] models = [] for name in motor_names: @@ -675,8 +453,6 @@ class FeetechMotorsBus: if data_name in CALIBRATION_REQUIRED and self.calibration is not None: values = self.revert_calibration(values, motor_names) - values = values.tolist() - assert_same_address(self.model_ctrl_table, models, data_name) addr, bytes = self.model_ctrl_table[model][data_name] group_key = get_group_sync_key(data_name, motor_names) @@ -700,31 +476,3 @@ class FeetechMotorsBus: f"Write failed due to communication error on port {self.port} for group_key {group_key}: " f"{self.packet_handler.getTxRxResult(comm)}" ) - - # log the number of seconds it took to write the data to the motors - delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names) - self.logs[delta_ts_name] = time.perf_counter() - start_time - - # TODO(rcadene): should we log the time before sending the write command? - # log the utc time when the write has been completed - ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names) - self.logs[ts_utc_name] = capture_timestamp_utc() - - def disconnect(self): - if not self.is_connected: - raise DeviceNotConnectedError( - f"FeetechMotorsBus({self.port}) is not connected. Try running `motors_bus.connect()` first." - ) - - if self.port_handler is not None: - self.port_handler.closePort() - self.port_handler = None - - self.packet_handler = None - self.group_readers = {} - self.group_writers = {} - self.is_connected = False - - def __del__(self): - if getattr(self, "is_connected", False): - self.disconnect() diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index ca95a4da..a6a9297e 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -1,29 +1,279 @@ +#!/usr/bin/env python + +# Copyright 2024 The HuggingFace Inc. team. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# TODO(aliberts): This noqa is for the PortHandler / PacketHandler Protocols +# Add block noqa when feature below is available +# https://github.com/astral-sh/ruff/issues/3711 +# ruff: noqa: N802 + import abc +import enum +import time +import traceback +from typing import Protocol + +import numpy as np +import tqdm + +from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError +from lerobot.common.utils.utils import capture_timestamp_utc + +MAX_ID_RANGE = 252 + + +def get_group_sync_key(data_name: str, motor_names: list[str]): + group_key = f"{data_name}_" + "_".join(motor_names) + return group_key + + +def get_result_name(fn_name: str, data_name: str, motor_names: list[str]): + group_key = get_group_sync_key(data_name, motor_names) + rslt_name = f"{fn_name}_{group_key}" + return rslt_name + + +def get_queue_name(fn_name: str, data_name: str, motor_names: list[str]): + group_key = get_group_sync_key(data_name, motor_names) + queue_name = f"{fn_name}_{group_key}" + return queue_name + + +def get_log_name(var_name: str, fn_name: str, data_name: str, motor_names: list[str]): + group_key = get_group_sync_key(data_name, motor_names) + log_name = f"{var_name}_{fn_name}_{group_key}" + return log_name + + +def assert_same_address(model_ctrl_table: dict[str, dict], motor_models: list[str], data_name: str): + all_addr = [] + all_bytes = [] + for model in motor_models: + addr, bytes = model_ctrl_table[model][data_name] + all_addr.append(addr) + all_bytes.append(bytes) + + if len(set(all_addr)) != 1: + raise NotImplementedError( + f"At least two motor models use a different address for `data_name`='{data_name}' ({list(zip(motor_models, all_addr, strict=False))}). Contact a LeRobot maintainer." + ) + + if len(set(all_bytes)) != 1: + raise NotImplementedError( + f"At least two motor models use a different bytes representation for `data_name`='{data_name}' ({list(zip(motor_models, all_bytes, strict=False))}). Contact a LeRobot maintainer." + ) + + +class TorqueMode(enum.Enum): + ENABLED = 1 + DISABLED = 0 + + +class DriveMode(enum.Enum): + NON_INVERTED = 0 + INVERTED = 1 + + +class CalibrationMode(enum.Enum): + # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180] + DEGREE = 0 + # Joints with liner motions (like gripper of Aloha) are expressed in nominal range of [0, 100] + LINEAR = 1 + + +class JointOutOfRangeError(Exception): + def __init__(self, message="Joint is out of range"): + self.message = message + super().__init__(self.message) + + +class PortHandler(Protocol): + def __init__(self, port_name): + self.is_open: bool + self.baudrate: int + self.packet_start_time: float + self.packet_timeout: float + self.tx_time_per_byte: float + self.is_using: bool + self.port_name: str + self.ser: object + + def openPort(self): ... + def closePort(self): ... + def clearPort(self): ... + def setPortName(self, port_name): ... + def getPortName(self): ... + def setBaudRate(self, baudrate): ... + def getBaudRate(self): ... + def getBytesAvailable(self): ... + def readPort(self, length): ... + def writePort(self, packet): ... + def setPacketTimeout(self, packet_length): ... + def setPacketTimeoutMillis(self, msec): ... + def isPacketTimeout(self): ... + def getCurrentTime(self): ... + def getTimeSinceStart(self): ... + def setupPort(self, cflag_baud): ... + def getCFlagBaud(self, baudrate): ... + + +class PacketHandler(Protocol): + def getTxRxResult(self, result): ... + def getRxPacketError(self, error): ... + def txPacket(self, port, txpacket): ... + def rxPacket(self, port): ... + def txRxPacket(self, port, txpacket): ... + def ping(self, port, id): ... + def action(self, port, id): ... + def readTx(self, port, id, address, length): ... + def readRx(self, port, id, length): ... + def readTxRx(self, port, id, address, length): ... + def read1ByteTx(self, port, id, address): ... + def read1ByteRx(self, port, id): ... + def read1ByteTxRx(self, port, id, address): ... + def read2ByteTx(self, port, id, address): ... + def read2ByteRx(self, port, id): ... + def read2ByteTxRx(self, port, id, address): ... + def read4ByteTx(self, port, id, address): ... + def read4ByteRx(self, port, id): ... + def read4ByteTxRx(self, port, id, address): ... + def writeTxOnly(self, port, id, address, length, data): ... + def writeTxRx(self, port, id, address, length, data): ... + def write1ByteTxOnly(self, port, id, address, data): ... + def write1ByteTxRx(self, port, id, address, data): ... + def write2ByteTxOnly(self, port, id, address, data): ... + def write2ByteTxRx(self, port, id, address, data): ... + def write4ByteTxOnly(self, port, id, address, data): ... + def write4ByteTxRx(self, port, id, address, data): ... + def regWriteTxOnly(self, port, id, address, length, data): ... + def regWriteTxRx(self, port, id, address, length, data): ... + def syncReadTx(self, port, start_address, data_length, param, param_length): ... + def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): ... class MotorsBus(abc.ABC): """The main LeRobot class for implementing motors buses.""" + model_ctrl_table: dict[str, dict] + model_resolution_table: dict[str, int] + def __init__( self, + port: str, motors: dict[str, tuple[int, str]], ): + self.port = port self.motors = motors + self.port_handler: PortHandler | None = None + self.packet_handler: PacketHandler | None = None + + self.group_readers = {} + self.group_writers = {} + self.logs = {} + + self.calibration = None + self.is_connected: bool = False def __len__(self): return len(self.motors) - @abc.abstractmethod + @property + def motor_names(self) -> list[str]: + return list(self.motors) + + @property + def motor_models(self) -> list[str]: + return [model for _, model in self.motors.values()] + + @property + def motor_indices(self) -> list[int]: + return [idx for idx, _ in self.motors.values()] + def connect(self): + if self.is_connected: + raise DeviceAlreadyConnectedError( + f"{self.__name__}({self.port}) is already connected. Do not call `{self.__name__}.connect()` twice." + ) + + self._set_handlers() + + try: + if not self.port_handler.openPort(): + raise OSError(f"Failed to open port '{self.port}'.") + except Exception: + traceback.print_exc() + print( + "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n" + ) + raise + + self._set_timeout() + + # Allow to read and write + self.is_connected = True + + @abc.abstractmethod + def _set_handlers(self): pass @abc.abstractmethod - def reconnect(self): + def _set_timeout(self, timeout: int): pass - @abc.abstractmethod - def set_calibration(self, calibration: dict[str, list]): - pass + def are_motors_configured(self): + """ + Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, a + ConnectionError will be raised anyway. + """ + try: + return (self.motor_indices == self.read("ID")).all() + except ConnectionError as e: + print(e) + return False + + def find_motor_indices(self, possible_ids: list[str] = None, num_retry: int = 2): + if possible_ids is None: + possible_ids = range(MAX_ID_RANGE) + + indices = [] + for idx in tqdm.tqdm(possible_ids): + try: + present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0] + except ConnectionError: + continue + + if idx != present_idx: + # sanity check + raise OSError( + "Motor index used to communicate through the bus is not the same as the one present in the motor memory. The motor memory might be damaged." + ) + indices.append(idx) + + return indices + + def set_baudrate(self, baudrate): + present_bus_baudrate = self.port_handler.getBaudRate() + if present_bus_baudrate != baudrate: + print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.") + self.port_handler.setBaudRate(baudrate) + + if self.port_handler.getBaudRate() != baudrate: + raise OSError("Failed to write bus baud rate.") + + def set_calibration(self, calibration_dict: dict[str, list]): + self.calibration = calibration_dict @abc.abstractmethod def apply_calibration(self): @@ -33,14 +283,84 @@ class MotorsBus(abc.ABC): def revert_calibration(self): pass - @abc.abstractmethod - def read(self): - pass + def read(self, data_name, motor_names: str | list[str] | None = None): + if not self.is_connected: + raise DeviceNotConnectedError( + f"{self.__name__}({self.port}) is not connected. You need to run `{self.__name__}.connect()`." + ) + + start_time = time.perf_counter() + if motor_names is None: + motor_names = self.motor_names + + if isinstance(motor_names, str): + motor_names = [motor_names] + + values = self._read(data_name, motor_names) + + # log the number of seconds it took to read the data from the motors + delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names) + self.logs[delta_ts_name] = time.perf_counter() - start_time + + # log the utc time at which the data was received + ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names) + self.logs[ts_utc_name] = capture_timestamp_utc() + + return values @abc.abstractmethod - def write(self): + def _read(self, data_name, motor_names: str | list[str] | None = None): pass + def write( + self, data_name: str, values: int | float | np.ndarray, motor_names: str | list[str] | None = None + ): + if not self.is_connected: + raise DeviceNotConnectedError( + f"{self.__name__}({self.port}) is not connected. You need to run `{self.__name__}.connect()`." + ) + + start_time = time.perf_counter() + + if motor_names is None: + motor_names = self.motor_names + + if isinstance(motor_names, str): + motor_names = [motor_names] + + if isinstance(values, (int, float, np.integer)): + values = [int(values)] * len(motor_names) + + self._write(data_name, values, motor_names) + + # log the number of seconds it took to write the data to the motors + delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names) + self.logs[delta_ts_name] = time.perf_counter() - start_time + + # TODO(rcadene): should we log the time before sending the write command? + # log the utc time when the write has been completed + ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names) + self.logs[ts_utc_name] = capture_timestamp_utc() + @abc.abstractmethod + def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None: + pass + def disconnect(self): - pass + if not self.is_connected: + raise DeviceNotConnectedError( + f"{self.__name__}({self.port}) is not connected. Try running `{self.__name__}.connect()` first." + ) + + if self.port_handler is not None: + self.port_handler.closePort() + self.port_handler = None + + self.packet_handler = None + self.group_readers = {} + self.group_writers = {} + self.is_connected = False + + def __del__(self): + if self.is_connected: + self.disconnect()