Add handshake, fix feetech _read_firmware_version

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).

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)

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