Return models (str) with pings

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()}

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

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,

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