diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 719e80c78..2abce8181 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -24,7 +24,7 @@ from enum import Enum from lerobot.common.utils.encoding_utils import decode_twos_complement, encode_twos_complement -from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address +from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value from .tables import ( AVAILABLE_BAUDRATES, MODEL_BAUDRATE_TABLE, @@ -192,20 +192,3 @@ class DynamixelMotorsBus(MotorsBus): return data_list if data_list else None return {id_: data[0] for id_, data in data_list.items()} - - def _write(self, data_name: str, motor_id: int, value: int, num_retry: int = 0) -> tuple[int, int]: - model = self._id_to_model(motor_id) - addr, n_bytes = get_address(self.model_ctrl_table, model, data_name) - value = self._encode_value(value, data_name, n_bytes) - data = self._split_int_to_bytes(value, n_bytes) - - for n_try in range(1 + num_retry): - comm, error = self.packet_handler.writeTxRx(self.port_handler, motor_id, addr, n_bytes, data) - if self._is_comm_success(comm): - break - logger.debug( - f"Failed to write '{data_name}' ({addr=} {n_bytes=}) on {motor_id=} with '{value}' ({n_try=})" - ) - logger.debug(self.packet_handler.getRxPacketError(comm)) - - return comm, error diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index 0f73a54b7..f97a8a985 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -19,7 +19,7 @@ from pprint import pformat from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_sign_magnitude -from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address +from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value from .tables import ( AVAILABLE_BAUDRATES, ENCODINGS, @@ -61,6 +61,19 @@ class TorqueMode(Enum): DISABLED = 0 +def patch_setPacketTimeout(self, packet_length): # noqa: N802 + """ + HACK: This patches the PortHandler behavior to set the correct packet timeouts. + + It fixes https://gitee.com/ftservo/SCServoSDK/issues/IBY2S6 + The bug is fixed on the official Feetech SDK repo (https://gitee.com/ftservo/FTServo_Python) + but because that version is not published on PyPI, we rely on the (unofficial) on that is, which needs + patching. + """ + self.packet_start_time = self.getCurrentTime() + self.packet_timeout = (self.tx_time_per_byte * packet_length) + (self.tx_time_per_byte * 3.0) + 50 + + class FeetechMotorsBus(MotorsBus): """ The FeetechMotorsBus class allows to efficiently read and write to the attached motors. It relies on the @@ -88,6 +101,10 @@ class FeetechMotorsBus(MotorsBus): import scservo_sdk as scs self.port_handler = scs.PortHandler(self.port) + # HACK: monkeypatch + self.port_handler.setPacketTimeout = patch_setPacketTimeout.__get__( + self.port_handler, scs.PortHandler + ) self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION) self.sync_reader = scs.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0) self.sync_writer = scs.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0) @@ -261,36 +278,3 @@ class FeetechMotorsBus(MotorsBus): return model_numbers if model_numbers else None return model_numbers - - def _is_eprom(self, address: int) -> bool: - """ - HACK: because of https://gitee.com/ftservo/SCServoSDK/issues/IBY2S6 - When writing to EPROM on for Feetech motors with Lock=0, we need to: - - 1. write several times - - 2. reset serial's buffer - """ - return address < 40 - - def _write(self, data_name: str, motor_id: int, value: int, num_retry: int = 0) -> tuple[int, int]: - model = self._id_to_model(motor_id) - addr, n_bytes = get_address(self.model_ctrl_table, model, data_name) - value = self._encode_value(value, data_name, n_bytes) - data = self._split_int_to_bytes(value, n_bytes) - - if self._is_eprom(addr): # HACK - num_retry = max(num_retry, 5) - - for n_try in range(1 + num_retry): - comm, error = self.packet_handler.writeTxRx(self.port_handler, motor_id, addr, n_bytes, data) - if self._is_comm_success(comm): - break - logger.debug( - f"Failed to write '{data_name}' ({addr=} {n_bytes=}) on {motor_id=} with '{value}' ({n_try=})" - ) - logger.debug(self.packet_handler.getRxPacketError(comm)) - - if self._is_eprom(addr): # HACK - self.port_handler.ser.reset_output_buffer() - self.port_handler.ser.reset_input_buffer() - - return comm, error diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 799d959c5..96cbb7c37 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -468,21 +468,6 @@ class MotorsBus(abc.ABC): if self.port_handler.getBaudRate() != baudrate: raise OSError("Failed to write bus baud rate.") - def reset_calibration(self, motors: NameOrID | list[NameOrID] | None = None) -> None: - if motors is None: - motors = self.names - elif isinstance(motors, (str, int)): - motors = [motors] - elif not isinstance(motors, list): - raise TypeError(motors) - - for motor in motors: - model = self._get_motor_model(motor) - max_res = self.model_resolution_table[model] - 1 - self.write("Homing_Offset", motor, 0, normalize=False) - self.write("Min_Position_Limit", motor, 0, normalize=False) - self.write("Max_Position_Limit", motor, max_res, normalize=False) - @property def is_calibrated(self) -> bool: return self.calibration == self.read_calibration() @@ -517,6 +502,23 @@ class MotorsBus(abc.ABC): self.calibration = calibration_dict + def reset_calibration(self, motors: NameOrID | list[NameOrID] | None = None) -> None: + if motors is None: + motors = self.names + elif isinstance(motors, (str, int)): + motors = [motors] + elif not isinstance(motors, list): + raise TypeError(motors) + + for motor in motors: + model = self._get_motor_model(motor) + max_res = self.model_resolution_table[model] - 1 + self.write("Homing_Offset", motor, 0, normalize=False) + self.write("Min_Position_Limit", motor, 0, normalize=False) + self.write("Max_Position_Limit", motor, max_res, normalize=False) + + self.calibration = {} + def set_half_turn_homings(self, motors: NameOrID | list[NameOrID] | None = None) -> dict[NameOrID, Value]: """ This assumes motors present positions are roughly in the middle of their desired range @@ -892,9 +894,22 @@ class MotorsBus(abc.ABC): f"\n{self.packet_handler.getRxPacketError(error)}" ) - @abc.abstractmethod def _write(self, data_name: str, motor_id: int, value: int, num_retry: int = 0) -> tuple[int, int]: - pass + model = self._id_to_model(motor_id) + addr, n_bytes = get_address(self.model_ctrl_table, model, data_name) + value = self._encode_value(value, data_name, n_bytes) + data = self._split_int_to_bytes(value, n_bytes) + + for n_try in range(1 + num_retry): + comm, error = self.packet_handler.writeTxRx(self.port_handler, motor_id, addr, n_bytes, data) + if self._is_comm_success(comm): + break + logger.debug( + f"Failed to write '{data_name}' ({addr=} {n_bytes=}) on {motor_id=} with '{value}' ({n_try=})" + ) + logger.debug(self.packet_handler.getRxPacketError(comm)) + + return comm, error def disconnect(self, disable_torque: bool = True) -> None: if not self.is_connected: diff --git a/tests/mocks/mock_feetech.py b/tests/mocks/mock_feetech.py index 473433bf8..291052ae3 100644 --- a/tests/mocks/mock_feetech.py +++ b/tests/mocks/mock_feetech.py @@ -7,6 +7,7 @@ import serial from mock_serial import MockSerial from lerobot.common.motors.feetech import SCS_SERIES_CONTROL_TABLE, FeetechMotorsBus +from lerobot.common.motors.feetech.feetech import patch_setPacketTimeout from .mock_serial_patch import WaitableStub @@ -308,6 +309,9 @@ class MockPortHandler(scs.PortHandler): return True + def setPacketTimeout(self, packet_length): # noqa: N802 + return patch_setPacketTimeout(self, packet_length) + class MockMotors(MockSerial): """