diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index 21f2524c..4bcbf6b0 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -136,6 +136,9 @@ class DynamixelMotorsBus(MotorsBus): def _assert_protocol_is_compatible(self, instruction_name: str) -> None: pass + def _handshake(self) -> None: + self._assert_motors_exist() + def configure_motors(self) -> None: # By default, Dynamixel motors have a 500µs delay response time (corresponding to a value of 250 on # the 'Return_Delay_Time' address). We ensure this is reduced to the minimum of 2µs (value of 0). diff --git a/lerobot/common/motors/feetech/feetech.py b/lerobot/common/motors/feetech/feetech.py index 193f1b4a..8c31401b 100644 --- a/lerobot/common/motors/feetech/feetech.py +++ b/lerobot/common/motors/feetech/feetech.py @@ -22,6 +22,7 @@ from lerobot.common.utils.encoding_utils import decode_sign_magnitude, encode_si from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value from .tables import ( FIRMWARE_MAJOR_VERSION, + FIRMWARE_MINOR_VERSION, MODEL_BAUDRATE_TABLE, MODEL_CONTROL_TABLE, MODEL_ENCODING_TABLE, @@ -150,6 +151,18 @@ class FeetechMotorsBus(MotorsBus): "'Broadcast Ping' is not available with Feetech motors using Protocol 1. Use 'Ping' sequentially instead." ) + def _assert_same_firmware(self) -> None: + firmware_versions = self._read_firmware_version(self.ids) + if len(set(firmware_versions.values())) != 1: + raise RuntimeError( + "Some Motors use different firmware versions. Update their firmware first using Feetech's software. " + "Visit https://www.feetechrc.com/software." + ) + + def _handshake(self) -> None: + self._assert_motors_exist() + self._assert_same_firmware() + def configure_motors(self) -> None: # By default, Feetech motors have a 500µs delay response time (corresponding to a value of 250 on the # 'Return_Delay' address). We ensure this is reduced to the minimum of 2µs (value of 0). @@ -317,9 +330,9 @@ class FeetechMotorsBus(MotorsBus): display_dict = {id_: self.packet_handler.getRxPacketError(err) for id_, err in ids_errors.items()} logger.error(f"Some motors found returned an error status:\n{pformat(display_dict, indent=4)}") - return self._get_model_number(list(ids_status), raise_on_error) + return self._read_model_number(list(ids_status), raise_on_error) - def _get_firmware_version(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, str]: + def _read_firmware_version(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, str]: firmware_versions = {} for id_ in motor_ids: firm_ver_major, comm, error = self._read( @@ -329,7 +342,7 @@ class FeetechMotorsBus(MotorsBus): return firm_ver_minor, comm, error = self._read( - *FIRMWARE_MAJOR_VERSION, id_, raise_on_error=raise_on_error + *FIRMWARE_MINOR_VERSION, id_, raise_on_error=raise_on_error ) if not self._is_comm_success(comm) or self._is_error(error): return @@ -338,7 +351,7 @@ class FeetechMotorsBus(MotorsBus): return firmware_versions - def _get_model_number(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, int]: + def _read_model_number(self, motor_ids: list[int], raise_on_error: bool = False) -> dict[int, int]: model_numbers = {} for id_ in motor_ids: model_nb, comm, error = self._read(*MODEL_NUMBER, id_, raise_on_error=raise_on_error) diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index d0f8ff3e..1ec4a201 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -377,9 +377,13 @@ class MotorsBus(abc.ABC): def _assert_motors_exist(self) -> None: # TODO(aliberts): collect all wrong ids/models and display them at once - found_models = self.broadcast_ping() + found_models = {} + for id_ in self.ids: + model_nb = self.ping(id_) + if model_nb is not None: + found_models[id_] = model_nb expected_models = {m.id: self.model_number_table[m.model] for m in self.motors.values()} - if not found_models or set(found_models) != set(self.ids): + if set(found_models) != set(self.ids): raise RuntimeError( f"{self.__class__.__name__} is supposed to have these motors: ({{id: model_nb}})" f"\n{pformat(expected_models, indent=4, sort_dicts=False)}\n" @@ -403,7 +407,7 @@ class MotorsBus(abc.ABC): def is_connected(self) -> bool: return self.port_handler.is_open - def connect(self, assert_motors_exist: bool = True) -> None: + def connect(self, handshake: bool = True) -> None: if self.is_connected: raise DeviceAlreadyConnectedError( f"{self.__class__.__name__}('{self.port}') is already connected. Do not call `{self.__class__.__name__}.connect()` twice." @@ -412,8 +416,8 @@ class MotorsBus(abc.ABC): try: if not self.port_handler.openPort(): raise OSError(f"Failed to open port '{self.port}'.") - elif assert_motors_exist: - self._assert_motors_exist() + elif handshake: + self._handshake() except (FileNotFoundError, OSError, serial.SerialException) as e: raise ConnectionError( f"\nCould not connect on port '{self.port}'. Make sure you are using the correct port." @@ -423,6 +427,10 @@ class MotorsBus(abc.ABC): self.set_timeout() logger.debug(f"{self.__class__.__name__} connected.") + @abc.abstractmethod + def _handshake(self) -> None: + pass + @classmethod def scan_port(cls, port: str, *args, **kwargs) -> dict[int, list[int]]: bus = cls(port, {}, *args, **kwargs) @@ -690,7 +698,7 @@ class MotorsBus(abc.ABC): return if self._is_error(error): if raise_on_error: - raise RuntimeError(self.packet_handler.getTxRxResult(comm)) + raise RuntimeError(self.packet_handler.getRxPacketError(error)) else: return diff --git a/tests/motors/test_dynamixel.py b/tests/motors/test_dynamixel.py index cb2c11e6..822fd049 100644 --- a/tests/motors/test_dynamixel.py +++ b/tests/motors/test_dynamixel.py @@ -91,7 +91,7 @@ def test_ping(id_, mock_motors, dummy_motors): expected_model_nb = MODEL_NUMBER_TABLE[dummy_motors[f"dummy_{id_}"].model] stub = mock_motors.build_ping_stub(id_, expected_model_nb) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) ping_model_nb = bus.ping(id_) @@ -104,7 +104,7 @@ def test_broadcast_ping(mock_motors, dummy_motors): expected_model_nbs = {id_: MODEL_NUMBER_TABLE[model] for id_, model in models.items()} stub = mock_motors.build_broadcast_ping_stub(expected_model_nbs) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) ping_model_nbs = bus.broadcast_ping() @@ -123,7 +123,7 @@ def test_broadcast_ping(mock_motors, dummy_motors): def test__read(addr, length, id_, value, mock_motors, dummy_motors): stub = mock_motors.build_read_stub(addr, length, id_, value) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) read_value, _, _ = bus._read(addr, length, id_) @@ -136,7 +136,7 @@ def test__read_error(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value, error = (10, 4, 1, 1337, dxl.ERRNUM_DATA_LIMIT) stub = mock_motors.build_read_stub(addr, length, id_, value, error=error) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises( @@ -155,7 +155,7 @@ def test__read_comm(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value = (10, 4, 1, 1337) stub = mock_motors.build_read_stub(addr, length, id_, value, reply=False) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -178,7 +178,7 @@ def test__read_comm(raise_on_error, mock_motors, dummy_motors): def test__write(addr, length, id_, value, mock_motors, dummy_motors): stub = mock_motors.build_write_stub(addr, length, id_, value) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) comm, error = bus._write(addr, length, id_, value) @@ -192,7 +192,7 @@ def test__write_error(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value, error = (10, 4, 1, 1337, dxl.ERRNUM_DATA_LIMIT) stub = mock_motors.build_write_stub(addr, length, id_, value, error=error) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises( @@ -211,7 +211,7 @@ def test__write_comm(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value = (10, 4, 1, 1337) stub = mock_motors.build_write_stub(addr, length, id_, value, reply=False) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -235,7 +235,7 @@ def test__write_comm(raise_on_error, mock_motors, dummy_motors): def test__sync_read(addr, length, ids_values, mock_motors, dummy_motors): stub = mock_motors.build_sync_read_stub(addr, length, ids_values) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) read_values, _ = bus._sync_read(addr, length, list(ids_values)) @@ -248,7 +248,7 @@ def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors): addr, length, ids_values = (10, 4, {1: 1337}) stub = mock_motors.build_sync_read_stub(addr, length, ids_values, reply=False) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -272,7 +272,7 @@ def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors): def test__sync_write(addr, length, ids_values, mock_motors, dummy_motors): stub = mock_motors.build_sync_write_stub(addr, length, ids_values) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) comm = bus._sync_write(addr, length, ids_values) @@ -294,7 +294,7 @@ def test_is_calibrated(mock_motors, dummy_motors, dummy_calibration): motors=dummy_motors, calibration=dummy_calibration, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) is_calibrated = bus.is_calibrated @@ -321,7 +321,7 @@ def test_reset_calibration(mock_motors, dummy_motors): ) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) bus.reset_calibration() @@ -355,7 +355,7 @@ def test_set_half_turn_homings(mock_motors, dummy_motors): write_homing_stubs.append(stub) bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) bus.reset_calibration = MagicMock() bus.set_half_turn_homings() @@ -386,7 +386,7 @@ def test_record_ranges_of_motion(mock_motors, dummy_motors): ) with patch("lerobot.common.motors.motors_bus.enter_pressed", side_effect=[False, True]): bus = DynamixelMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) mins, maxes = bus.record_ranges_of_motion(display_values=False) diff --git a/tests/motors/test_feetech.py b/tests/motors/test_feetech.py index baf6d340..360c13cb 100644 --- a/tests/motors/test_feetech.py +++ b/tests/motors/test_feetech.py @@ -101,7 +101,7 @@ def test_ping(id_, mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) ping_model_nb = bus.ping(id_) @@ -125,7 +125,7 @@ def test_broadcast_ping(mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) ping_model_nbs = bus.broadcast_ping() @@ -148,7 +148,7 @@ def test__read(addr, length, id_, value, mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) read_value, _, _ = bus._read(addr, length, id_) @@ -164,7 +164,7 @@ def test__read_error(raise_on_error, mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")): @@ -184,7 +184,7 @@ def test__read_comm(raise_on_error, mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -210,7 +210,7 @@ def test__write(addr, length, id_, value, mock_motors, dummy_motors): port=mock_motors.port, motors=dummy_motors, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) comm, error = bus._write(addr, length, id_, value) @@ -224,7 +224,7 @@ def test__write_error(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value, error = (10, 4, 1, 1337, scs.ERRBIT_VOLTAGE) stub = mock_motors.build_write_stub(addr, length, id_, value, error=error) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(RuntimeError, match=re.escape("[RxPacketError] Input voltage error!")): @@ -241,7 +241,7 @@ def test__write_comm(raise_on_error, mock_motors, dummy_motors): addr, length, id_, value = (10, 4, 1, 1337) stub = mock_motors.build_write_stub(addr, length, id_, value, reply=False) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -265,7 +265,7 @@ def test__write_comm(raise_on_error, mock_motors, dummy_motors): def test__sync_read(addr, length, ids_values, mock_motors, dummy_motors): stub = mock_motors.build_sync_read_stub(addr, length, ids_values) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) read_values, _ = bus._sync_read(addr, length, list(ids_values)) @@ -278,7 +278,7 @@ def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors): addr, length, ids_values = (10, 4, {1: 1337}) stub = mock_motors.build_sync_read_stub(addr, length, ids_values, reply=False) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) if raise_on_error: with pytest.raises(ConnectionError, match=re.escape("[TxRxResult] There is no status packet!")): @@ -302,7 +302,7 @@ def test__sync_read_comm(raise_on_error, mock_motors, dummy_motors): def test__sync_write(addr, length, ids_values, mock_motors, dummy_motors): stub = mock_motors.build_sync_write_stub(addr, length, ids_values) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) comm = bus._sync_write(addr, length, ids_values) @@ -324,7 +324,7 @@ def test_is_calibrated(mock_motors, dummy_motors, dummy_calibration): motors=dummy_motors, calibration=dummy_calibration, ) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) is_calibrated = bus.is_calibrated @@ -350,7 +350,7 @@ def test_reset_calibration(mock_motors, dummy_motors): ) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) bus.reset_calibration() @@ -386,7 +386,7 @@ def test_set_half_turn_homings(mock_motors, dummy_motors): write_homing_stubs.append(stub) bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) bus.reset_calibration = MagicMock() bus.set_half_turn_homings() @@ -417,7 +417,7 @@ def test_record_ranges_of_motion(mock_motors, dummy_motors): ) with patch("lerobot.common.motors.motors_bus.enter_pressed", side_effect=[False, True]): bus = FeetechMotorsBus(port=mock_motors.port, motors=dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) mins, maxes = bus.record_ranges_of_motion(display_values=False) diff --git a/tests/motors/test_motors_bus.py b/tests/motors/test_motors_bus.py index c98cda7d..f3af8daf 100644 --- a/tests/motors/test_motors_bus.py +++ b/tests/motors/test_motors_bus.py @@ -227,7 +227,7 @@ def test__serialize_data_large_number(): ) def test_read(data_name, id_, value, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] with ( @@ -261,7 +261,7 @@ def test_read(data_name, id_, value, dummy_motors): ) def test_write(data_name, id_, value, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] with ( @@ -296,7 +296,7 @@ def test_write(data_name, id_, value, dummy_motors): ) def test_sync_read_by_str(data_name, id_, value, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] ids = [id_] expected_value = {f"dummy_{id_}": value} @@ -333,7 +333,7 @@ def test_sync_read_by_str(data_name, id_, value, dummy_motors): ) def test_sync_read_by_list(data_name, ids_values, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] ids = list(ids_values) expected_values = {f"dummy_{id_}": val for id_, val in ids_values.items()} @@ -370,7 +370,7 @@ def test_sync_read_by_list(data_name, ids_values, dummy_motors): ) def test_sync_read_by_none(data_name, ids_values, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] ids = list(ids_values) expected_values = {f"dummy_{id_}": val for id_, val in ids_values.items()} @@ -406,7 +406,7 @@ def test_sync_read_by_none(data_name, ids_values, dummy_motors): ) def test_sync_write_by_single_value(data_name, value, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] ids_values = {m.id: value for m in dummy_motors.values()} @@ -441,7 +441,7 @@ def test_sync_write_by_single_value(data_name, value, dummy_motors): ) def test_sync_write_by_value_dict(data_name, ids_values, dummy_motors): bus = MockMotorsBus("/dev/dummy-port", dummy_motors) - bus.connect(assert_motors_exist=False) + bus.connect(handshake=False) addr, length = DUMMY_CTRL_TABLE_2[data_name] values = {f"dummy_{id_}": val for id_, val in ids_values.items()}