From 1de75c46c0ffecef82a241577e8cf4796aabe33e Mon Sep 17 00:00:00 2001 From: Simon Alibert Date: Mon, 24 Mar 2025 20:42:43 +0100 Subject: [PATCH] Return models (str) with pings --- lerobot/common/motors/dynamixel/dynamixel.py | 27 ++++++++++++------ lerobot/common/motors/motors_bus.py | 27 ++++++++++++++---- tests/mocks/mock_dynamixel.py | 9 ++---- tests/motors/test_dynamixel.py | 30 +++++++------------- 4 files changed, 54 insertions(+), 39 deletions(-) diff --git a/lerobot/common/motors/dynamixel/dynamixel.py b/lerobot/common/motors/dynamixel/dynamixel.py index c26114ad..30b33896 100644 --- a/lerobot/common/motors/dynamixel/dynamixel.py +++ b/lerobot/common/motors/dynamixel/dynamixel.py @@ -18,20 +18,22 @@ # https://emanual.robotis.com/docs/en/dxl/protocol2/#fast-sync-read-0x8a # -> Need to check compatibility across models +import logging from copy import deepcopy from enum import Enum from ..motors_bus import Motor, MotorsBus -from .tables import MODEL_BAUDRATE_TABLE, MODEL_CONTROL_TABLE, MODEL_RESOLUTION +from .tables import MODEL_BAUDRATE_TABLE, MODEL_CONTROL_TABLE, MODEL_NUMBER, MODEL_RESOLUTION PROTOCOL_VERSION = 2.0 BAUDRATE = 1_000_000 DEFAULT_TIMEOUT_MS = 1000 -MAX_ID_RANGE = 252 CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"] CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"] +logger = logging.getLogger(__name__) + class OperatingMode(Enum): # DYNAMIXEL only controls current(torque) regardless of speed and position. This mode is ideal for a @@ -73,6 +75,7 @@ class DynamixelMotorsBus(MotorsBus): model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE) model_resolution_table = deepcopy(MODEL_RESOLUTION) model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE) + model_number_table = deepcopy(MODEL_NUMBER) calibration_required = deepcopy(CALIBRATION_REQUIRED) default_timeout = DEFAULT_TIMEOUT_MS @@ -135,13 +138,19 @@ class DynamixelMotorsBus(MotorsBus): ] return data - def broadcast_ping( - self, num_retry: int = 0, raise_on_error: bool = False - ) -> dict[int, list[int, int]] | None: - for _ in range(1 + num_retry): + def broadcast_ping(self, num_retry: int = 0, raise_on_error: bool = False) -> dict[int, str] | None: + for n_try in range(1 + num_retry): data_list, comm = self.packet_handler.broadcastPing(self.port_handler) if self._is_comm_success(comm): - return data_list + break + logger.debug(f"Broadcast failed on port '{self.port}' ({n_try=})") + logger.debug(self.packet_handler.getRxPacketError(comm)) - if raise_on_error: - raise ConnectionError(f"Broadcast ping returned a {comm} comm error.") + if not self._is_comm_success(comm): + if raise_on_error: + logger.error(self.packet_handler.getRxPacketError(comm)) + raise ConnectionError + + return data_list if data_list else None + + return {id_: self._model_nb_to_model(data[0]) for id_, data in data_list.items()} diff --git a/lerobot/common/motors/motors_bus.py b/lerobot/common/motors/motors_bus.py index 25b01c5d..a63393ec 100644 --- a/lerobot/common/motors/motors_bus.py +++ b/lerobot/common/motors/motors_bus.py @@ -242,6 +242,7 @@ class MotorsBus(abc.ABC): model_ctrl_table: dict[str, dict] model_resolution_table: dict[str, int] model_baudrate_table: dict[str, dict] + model_number_table: dict[str, int] calibration_required: list[str] default_timeout: int @@ -265,6 +266,7 @@ class MotorsBus(abc.ABC): self._id_to_model_dict = {m.id: m.model for m in self.motors.values()} self._id_to_name_dict = {m.id: name for name, m in self.motors.items()} + self._model_nb_to_model_dict = {v: k for k, v in self.model_number_table.items()} def __len__(self): return len(self.motors) @@ -297,6 +299,9 @@ class MotorsBus(abc.ABC): def ids(self) -> list[int]: return [m.id for m in self.motors.values()] + def _model_nb_to_model(self, motor_nb: int) -> str: + return self._model_nb_to_model_dict[motor_nb] + def _id_to_model(self, motor_id: int) -> str: return self._id_to_model_dict[motor_id] @@ -436,21 +441,33 @@ class MotorsBus(abc.ABC): """ pass - def ping(self, motor: NameOrID, num_retry: int = 0, raise_on_error: bool = False) -> int | None: + def ping(self, motor: NameOrID, num_retry: int = 0, raise_on_error: bool = False) -> str | None: idx = self._get_motor_id(motor) for n_try in range(1 + num_retry): model_number, comm, error = self.packet_handler.ping(self.port_handler, idx) if self._is_comm_success(comm): - return model_number + break logger.debug(f"ping failed for {idx=}: {n_try=} got {comm=} {error=}") - if raise_on_error: - raise ConnectionError(f"Ping motor {motor} returned a {error} error code.") + if not self._is_comm_success(comm): + if raise_on_error: + logger.error(self.packet_handler.getRxPacketError(comm)) + raise ConnectionError + else: + return + if self._is_error(error): + if raise_on_error: + logger.error(self.packet_handler.getTxRxResult(comm)) + raise ConnectionError + else: + return + + return self._model_nb_to_model(model_number) @abc.abstractmethod def broadcast_ping( self, num_retry: int = 0, raise_on_error: bool = False - ) -> dict[int, list[int, int]] | None: + ) -> dict[int, list[int, str]] | None: pass @overload diff --git a/tests/mocks/mock_dynamixel.py b/tests/mocks/mock_dynamixel.py index a7bcf30d..4159af3d 100644 --- a/tests/mocks/mock_dynamixel.py +++ b/tests/mocks/mock_dynamixel.py @@ -441,16 +441,13 @@ class MockMotors(MockSerial): return new_stub def build_broadcast_ping_stub( - self, ids_models_firmwares: dict[int, list[int]] | None = None, num_invalid_try: int = 0 + self, ids_models: dict[int, list[int]] | None = None, num_invalid_try: int = 0 ) -> str: ping_request = MockInstructionPacket.ping(dxl.BROADCAST_ID) - return_packets = b"".join( - MockStatusPacket.ping(idx, model, firm_ver) - for idx, (model, firm_ver) in ids_models_firmwares.items() - ) + return_packets = b"".join(MockStatusPacket.ping(idx, model) for idx, model in ids_models.items()) ping_response = self._build_send_fn(return_packets, num_invalid_try) - stub_name = "Ping_" + "_".join([str(idx) for idx in ids_models_firmwares]) + stub_name = "Ping_" + "_".join([str(idx) for idx in ids_models]) self.stub( name=stub_name, receive_bytes=ping_request, diff --git a/tests/motors/test_dynamixel.py b/tests/motors/test_dynamixel.py index 7fdd2618..73fb40aa 100644 --- a/tests/motors/test_dynamixel.py +++ b/tests/motors/test_dynamixel.py @@ -6,7 +6,7 @@ import dynamixel_sdk as dxl import pytest from lerobot.common.motors import CalibrationMode, Motor -from lerobot.common.motors.dynamixel import DynamixelMotorsBus +from lerobot.common.motors.dynamixel import MODEL_NUMBER, DynamixelMotorsBus from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler @@ -92,15 +92,10 @@ def test_abc_implementation(dummy_motors): DynamixelMotorsBus(port="/dev/dummy-port", motors=dummy_motors) -@pytest.mark.parametrize( - "idx, model_nb", - [ - (1, 1190), - (2, 1200), - (3, 1120), - ], -) -def test_ping(idx, model_nb, mock_motors, dummy_motors): +@pytest.mark.parametrize("idx", [1, 2, 3]) +def test_ping(idx, mock_motors, dummy_motors): + expected_model = dummy_motors[f"dummy_{idx}"].model + model_nb = MODEL_NUMBER[expected_model] stub_name = mock_motors.build_ping_stub(idx, model_nb) motors_bus = DynamixelMotorsBus( port=mock_motors.port, @@ -110,26 +105,23 @@ def test_ping(idx, model_nb, mock_motors, dummy_motors): ping_model_nb = motors_bus.ping(idx) - assert ping_model_nb == model_nb + assert ping_model_nb == expected_model assert mock_motors.stubs[stub_name].called def test_broadcast_ping(mock_motors, dummy_motors): - expected_pings = { - 1: [1060, 50], - 2: [1120, 30], - 3: [1190, 10], - } - stub_name = mock_motors.build_broadcast_ping_stub(expected_pings) + expected_models = {m.id: m.model for m in dummy_motors.values()} + model_nbs = {id_: MODEL_NUMBER[model] for id_, model in expected_models.items()} + stub_name = mock_motors.build_broadcast_ping_stub(model_nbs) motors_bus = DynamixelMotorsBus( port=mock_motors.port, motors=dummy_motors, ) motors_bus.connect() - ping_list = motors_bus.broadcast_ping() + ping_model_nbs = motors_bus.broadcast_ping() - assert ping_list == expected_pings + assert ping_model_nbs == expected_models assert mock_motors.stubs[stub_name].called