Hack feetech firmware bug

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
+from ..motors_bus import Motor, MotorCalibration, MotorsBus, NameOrID, Value, get_address
 from .tables import (
     AVAILABLE_BAUDRATES,
     MODEL_BAUDRATE_TABLE,
@@ -192,3 +192,20 @@ 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