Merge branch 'user/aliberts/2025_02_25_refactor_robots' into feat/thread_safe_teleop_keyboard

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Simon Alibert <75076266+aliberts@users.noreply.github.com>

benchmarks/video/README.md

@@ -51,7 +51,7 @@ For a comprehensive list and documentation of these parameters, see the ffmpeg d
 ### Decoding parameters
 **Decoder**
 We tested two video decoding backends from torchvision:
-- `pyav` (default)
+- `pyav`
 - `video_reader` (requires to build torchvision from source)
 
 **Requested timestamps**

examples/7_get_started_with_real_robot.md

@@ -292,6 +292,11 @@ Steps:
    - Scan for devices. All 12 motors should appear.
    - Select the motors one by one and move the arm. Check that the graphical indicator near the top right shows the movement.
 
+** There is a common issue with the Dynamixel XL430-W250 motors where the motors become undiscoverable after upgrading their firmware from Mac and Windows Dynamixel Wizard2 applications.  When this occurs, it is required to do a firmware recovery (Select `DYNAMIXEL Firmware Recovery` and follow the prompts).   There are two known workarounds to conduct this firmware reset:
+  1) Install the Dynamixel Wizard on a linux machine and complete the firmware recovery
+  2) Use the Dynamixel U2D2 in order to perform the reset with Windows or Mac.  This U2D2 can be purchased [here](https://www.robotis.us/u2d2/).
+  For either solution, open DYNAMIXEL Wizard 2.0 and select the appropriate port. You will likely be unable to see the motor in the GUI at this time. Select `Firmware Recovery`, carefully choose the correct model, and wait for the process to complete. Finally, re-scan to confirm the firmware recovery was successful.
+
 **Read and Write with DynamixelMotorsBus**
 
 To get familiar with how `DynamixelMotorsBus` communicates with the motors, you can start by reading data from them. Copy past this code in the same interactive python session:

lerobot/common/datasets/lerobot_dataset.py

@@ -69,6 +69,7 @@ from lerobot.common.datasets.video_utils import (
     VideoFrame,
     decode_video_frames,
     encode_video_frames,
+    get_safe_default_codec,
     get_video_info,
 )
 from lerobot.common.robots.utils import Robot
@@ -462,7 +463,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
             download_videos (bool, optional): Flag to download the videos. Note that when set to True but the
                 video files are already present on local disk, they won't be downloaded again. Defaults to
                 True.
-            video_backend (str | None, optional): Video backend to use for decoding videos. Defaults to torchcodec.
+            video_backend (str | None, optional): Video backend to use for decoding videos. Defaults to torchcodec when available int the platform; otherwise, defaults to 'pyav'.
                 You can also use the 'pyav' decoder used by Torchvision, which used to be the default option, or 'video_reader' which is another decoder of Torchvision.
         """
         super().__init__()
@@ -473,7 +474,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         self.episodes = episodes
         self.tolerance_s = tolerance_s
         self.revision = revision if revision else CODEBASE_VERSION
-        self.video_backend = video_backend if video_backend else "torchcodec"
+        self.video_backend = video_backend if video_backend else get_safe_default_codec()
         self.delta_indices = None
 
         # Unused attributes
@@ -1027,7 +1028,7 @@ class LeRobotDataset(torch.utils.data.Dataset):
         obj.delta_timestamps = None
         obj.delta_indices = None
         obj.episode_data_index = None
-        obj.video_backend = video_backend if video_backend is not None else "torchcodec"
+        obj.video_backend = video_backend if video_backend is not None else get_safe_default_codec()
         return obj
 
 

lerobot/common/datasets/video_utils.py

@@ -13,6 +13,7 @@
 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 # See the License for the specific language governing permissions and
 # limitations under the License.
+import importlib
 import json
 import logging
 import subprocess
@@ -27,14 +28,23 @@ import torch
 import torchvision
 from datasets.features.features import register_feature
 from PIL import Image
-from torchcodec.decoders import VideoDecoder
+
+
+def get_safe_default_codec():
+    if importlib.util.find_spec("torchcodec"):
+        return "torchcodec"
+    else:
+        logging.warning(
+            "'torchcodec' is not available in your platform, falling back to 'pyav' as a default decoder"
+        )
+        return "pyav"
 
 
 def decode_video_frames(
     video_path: Path | str,
     timestamps: list[float],
     tolerance_s: float,
-    backend: str = "torchcodec",
+    backend: str | None = None,
 ) -> torch.Tensor:
     """
     Decodes video frames using the specified backend.
@@ -43,13 +53,15 @@ def decode_video_frames(
         video_path (Path): Path to the video file.
         timestamps (list[float]): List of timestamps to extract frames.
         tolerance_s (float): Allowed deviation in seconds for frame retrieval.
-        backend (str, optional): Backend to use for decoding. Defaults to "torchcodec".
+        backend (str, optional): Backend to use for decoding. Defaults to "torchcodec" when available in the platform; otherwise, defaults to "pyav"..
 
     Returns:
         torch.Tensor: Decoded frames.
 
     Currently supports torchcodec on cpu and pyav.
     """
+    if backend is None:
+        backend = get_safe_default_codec()
     if backend == "torchcodec":
         return decode_video_frames_torchcodec(video_path, timestamps, tolerance_s)
     elif backend in ["pyav", "video_reader"]:
@@ -173,6 +185,12 @@ def decode_video_frames_torchcodec(
     and all subsequent frames until reaching the requested frame. The number of key frames in a video
     can be adjusted during encoding to take into account decoding time and video size in bytes.
     """
+
+    if importlib.util.find_spec("torchcodec"):
+        from torchcodec.decoders import VideoDecoder
+    else:
+        raise ImportError("torchcodec is required but not available.")
+
     # initialize video decoder
     decoder = VideoDecoder(video_path, device=device, seek_mode="approximate")
     loaded_frames = []

lerobot/common/debugging/__init__.py

@@ -0,0 +1,3 @@
+from .motors_bus import visualize_motors_bus
+
+__all__ = ["visualize_motors_bus"]

lerobot/common/debugging/motors_bus.py

@@ -0,0 +1,82 @@
+"""
+Usage example
+
+```python
+from lerobot.common.debugging.motors_bus import visualize_motors_bus
+from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig
+
+cfg = SO100RobotConfig(port="/dev/tty.usbmodem58760430541")
+so100 = SO100Robot(cfg)
+
+visualize_motors_bus(so100.arm)
+```
+"""
+
+import time
+
+from lerobot.common.motors import MotorsBus
+from lerobot.common.motors.feetech.feetech_calibration import (
+    adjusted_to_homing_ticks,
+    adjusted_to_motor_ticks,
+    convert_degrees_to_ticks,
+    convert_ticks_to_degrees,
+)
+
+
+def visualize_motors_bus(motors_bus: MotorsBus):
+    """
+    Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
+    """
+    if not motors_bus.is_connected:
+        motors_bus.connect()
+
+    # Disable torque on all motors so you can move them freely by hand
+    values_dict = {idx: 0 for idx in motors_bus.motor_ids}
+    motors_bus.write("Torque_Enable", values_dict)
+    print("Torque disabled on all joints.")
+
+    try:
+        print("\nPress Ctrl+C to quit.\n")
+        while True:
+            # Read *raw* positions (no calibration).
+            raw_positions = motors_bus.read("Present_Position")
+
+            # # Read *already-homed* positions
+            # homed_positions = motor_bus.read("Present_Position")
+
+            for name, raw_ticks in raw_positions.items():
+                idx = motors_bus.motors[name][0]
+                model = motors_bus.motors[name][1]
+
+                # homed_val = homed_positions[i]  # degrees or % if linear
+
+                # Manually compute "adjusted ticks" from raw ticks
+                manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, motors_bus, idx)
+                # Convert to degrees
+                manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
+
+                # Convert that deg back to ticks
+                manual_ticks = convert_degrees_to_ticks(manual_degs, model)
+                # Then invert them using offset & bus drive mode
+                inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, motors_bus, idx)
+
+                print(
+                    f"{name:15s} | "
+                    f"RAW={raw_ticks:4d} | "
+                    # f"HOMED_FROM_READ={homed_val:7.2f} | "
+                    f"HOMED_TICKS={manual_adjusted:6d} | "
+                    f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
+                    f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
+                    f"INV_TICKS={inv_ticks:4d} "
+                )
+            print("----------------------------------------------------")
+            time.sleep(0.25)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        print("\nExiting. Disconnecting bus...")
+        motors_bus.disconnect()
+
+
+if __name__ == "__main__":
+    visualize_motors_bus()

lerobot/common/motors/__init__.py

@@ -1,3 +1 @@
-from .motors_bus import CalibrationMode, DriveMode, MotorsBus, TorqueMode
-
-__all__ = ["CalibrationMode", "DriveMode", "MotorsBus", "TorqueMode"]
+from .motors_bus import CalibrationMode, DriveMode, Motor, MotorsBus, TorqueMode

lerobot/common/motors/dynamixel/__init__.py

@@ -1,4 +1,3 @@
-from .dynamixel import DynamixelMotorsBus, set_operating_mode
+from .dynamixel import DynamixelMotorsBus
 from .dynamixel_calibration import run_arm_calibration
-
-__all__ = ["DynamixelMotorsBus", "set_operating_mode", "run_arm_calibration"]
+from .tables import *

lerobot/common/motors/dynamixel/dynamixel.py

@@ -12,193 +12,25 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import logging
-import math
+# TODO(aliberts): Should we implement FastSyncRead/Write?
+# https://github.com/ROBOTIS-GIT/DynamixelSDK/pull/643
+# https://github.com/ROBOTIS-GIT/DynamixelSDK/releases/tag/3.8.2
+# https://emanual.robotis.com/docs/en/dxl/protocol2/#fast-sync-read-0x8a
+# -> Need to check compatibility across models
+
 from copy import deepcopy
 
-import numpy as np
-
-from ..motors_bus import (
-    CalibrationMode,
-    JointOutOfRangeError,
-    MotorsBus,
-    TorqueMode,
-    assert_same_address,
-    get_group_sync_key,
-)
+from ..motors_bus import MotorsBus
+from .tables import MODEL_BAUDRATE_TABLE, MODEL_CONTROL_TABLE, MODEL_RESOLUTION
 
 PROTOCOL_VERSION = 2.0
 BAUDRATE = 1_000_000
-TIMEOUT_MS = 1000
-
+DEFAULT_TIMEOUT_MS = 1000
 MAX_ID_RANGE = 252
 
-# The following bounds define the lower and upper joints range (after calibration).
-# For joints in degree (i.e. revolute joints), their nominal range is [-180, 180] degrees
-# which corresponds to a half rotation on the left and half rotation on the right.
-# Some joints might require higher range, so we allow up to [-270, 270] degrees until
-# an error is raised.
-LOWER_BOUND_DEGREE = -270
-UPPER_BOUND_DEGREE = 270
-
-# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
-# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
-# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
-# [-10, 110] until an error is raised.
-LOWER_BOUND_LINEAR = -10
-UPPER_BOUND_LINEAR = 110
-
-HALF_TURN_DEGREE = 180
-
-# https://emanual.robotis.com/docs/en/dxl/x/xl330-m077
-# https://emanual.robotis.com/docs/en/dxl/x/xl330-m288
-# https://emanual.robotis.com/docs/en/dxl/x/xl430-w250
-# https://emanual.robotis.com/docs/en/dxl/x/xm430-w350
-# https://emanual.robotis.com/docs/en/dxl/x/xm540-w270
-# https://emanual.robotis.com/docs/en/dxl/x/xc430-w150
-
-# data_name: (address, size_byte)
-X_SERIES_CONTROL_TABLE = {
-    "Model_Number": (0, 2),
-    "Model_Information": (2, 4),
-    "Firmware_Version": (6, 1),
-    "ID": (7, 1),
-    "Baud_Rate": (8, 1),
-    "Return_Delay_Time": (9, 1),
-    "Drive_Mode": (10, 1),
-    "Operating_Mode": (11, 1),
-    "Secondary_ID": (12, 1),
-    "Protocol_Type": (13, 1),
-    "Homing_Offset": (20, 4),
-    "Moving_Threshold": (24, 4),
-    "Temperature_Limit": (31, 1),
-    "Max_Voltage_Limit": (32, 2),
-    "Min_Voltage_Limit": (34, 2),
-    "PWM_Limit": (36, 2),
-    "Current_Limit": (38, 2),
-    "Acceleration_Limit": (40, 4),
-    "Velocity_Limit": (44, 4),
-    "Max_Position_Limit": (48, 4),
-    "Min_Position_Limit": (52, 4),
-    "Shutdown": (63, 1),
-    "Torque_Enable": (64, 1),
-    "LED": (65, 1),
-    "Status_Return_Level": (68, 1),
-    "Registered_Instruction": (69, 1),
-    "Hardware_Error_Status": (70, 1),
-    "Velocity_I_Gain": (76, 2),
-    "Velocity_P_Gain": (78, 2),
-    "Position_D_Gain": (80, 2),
-    "Position_I_Gain": (82, 2),
-    "Position_P_Gain": (84, 2),
-    "Feedforward_2nd_Gain": (88, 2),
-    "Feedforward_1st_Gain": (90, 2),
-    "Bus_Watchdog": (98, 1),
-    "Goal_PWM": (100, 2),
-    "Goal_Current": (102, 2),
-    "Goal_Velocity": (104, 4),
-    "Profile_Acceleration": (108, 4),
-    "Profile_Velocity": (112, 4),
-    "Goal_Position": (116, 4),
-    "Realtime_Tick": (120, 2),
-    "Moving": (122, 1),
-    "Moving_Status": (123, 1),
-    "Present_PWM": (124, 2),
-    "Present_Current": (126, 2),
-    "Present_Velocity": (128, 4),
-    "Present_Position": (132, 4),
-    "Velocity_Trajectory": (136, 4),
-    "Position_Trajectory": (140, 4),
-    "Present_Input_Voltage": (144, 2),
-    "Present_Temperature": (146, 1),
-}
-
-X_SERIES_BAUDRATE_TABLE = {
-    0: 9_600,
-    1: 57_600,
-    2: 115_200,
-    3: 1_000_000,
-    4: 2_000_000,
-    5: 3_000_000,
-    6: 4_000_000,
-}
-
 CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
 CONVERT_UINT32_TO_INT32_REQUIRED = ["Goal_Position", "Present_Position"]
 
-MODEL_CONTROL_TABLE = {
-    "x_series": X_SERIES_CONTROL_TABLE,
-    "xl330-m077": X_SERIES_CONTROL_TABLE,
-    "xl330-m288": X_SERIES_CONTROL_TABLE,
-    "xl430-w250": X_SERIES_CONTROL_TABLE,
-    "xm430-w350": X_SERIES_CONTROL_TABLE,
-    "xm540-w270": X_SERIES_CONTROL_TABLE,
-    "xc430-w150": X_SERIES_CONTROL_TABLE,
-}
-
-MODEL_RESOLUTION = {
-    "x_series": 4096,
-    "xl330-m077": 4096,
-    "xl330-m288": 4096,
-    "xl430-w250": 4096,
-    "xm430-w350": 4096,
-    "xm540-w270": 4096,
-    "xc430-w150": 4096,
-}
-
-MODEL_BAUDRATE_TABLE = {
-    "x_series": X_SERIES_BAUDRATE_TABLE,
-    "xl330-m077": X_SERIES_BAUDRATE_TABLE,
-    "xl330-m288": X_SERIES_BAUDRATE_TABLE,
-    "xl430-w250": X_SERIES_BAUDRATE_TABLE,
-    "xm430-w350": X_SERIES_BAUDRATE_TABLE,
-    "xm540-w270": X_SERIES_BAUDRATE_TABLE,
-    "xc430-w150": X_SERIES_BAUDRATE_TABLE,
-}
-
-NUM_READ_RETRY = 10
-NUM_WRITE_RETRY = 10
-
-
-def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
-    """This function converts the degree range to the step range for indicating motors rotation.
-    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
-    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
-    """
-    resolutions = [MODEL_RESOLUTION[model] for model in models]
-    steps = degrees / 180 * np.array(resolutions) / 2
-    steps = steps.astype(int)
-    return steps
-
-
-def convert_to_bytes(value, n_bytes: int):
-    import dynamixel_sdk as dxl
-
-    # Note: No need to convert back into unsigned int, since this byte preprocessing
-    # already handles it for us.
-    if n_bytes == 1:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-        ]
-    elif n_bytes == 2:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
-        ]
-    elif n_bytes == 4:
-        data = [
-            dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
-            dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)),
-            dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)),
-        ]
-    else:
-        raise NotImplementedError(
-            f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but "
-            f"{n_bytes} is provided instead."
-        )
-    return data
-
 
 class DynamixelMotorsBus(MotorsBus):
     """
@@ -210,6 +42,8 @@ class DynamixelMotorsBus(MotorsBus):
     model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
     model_resolution_table = deepcopy(MODEL_RESOLUTION)
     model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
+    calibration_required = deepcopy(CALIBRATION_REQUIRED)
+    default_timeout = DEFAULT_TIMEOUT_MS
 
     def __init__(
         self,
@@ -217,405 +51,68 @@ class DynamixelMotorsBus(MotorsBus):
         motors: dict[str, tuple[int, str]],
     ):
         super().__init__(port, motors)
-
-    def _set_handlers(self):
         import dynamixel_sdk as dxl
 
         self.port_handler = dxl.PortHandler(self.port)
         self.packet_handler = dxl.PacketHandler(PROTOCOL_VERSION)
+        self.reader = dxl.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
+        self.writer = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
 
-    def _set_timeout(self, timeout: int = TIMEOUT_MS):
-        self.port_handler.setPacketTimeoutMillis(timeout)
+    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):
+            data_list, comm = self.packet_handler.broadcastPing(self.port_handler)
+            if self._is_comm_success(comm):
+                return data_list
 
-    def _apply_calibration_autocorrect(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function applies the calibration, automatically detects out of range errors for motors values and attempts to correct.
+        if raise_on_error:
+            raise ConnectionError(f"Broadcast ping returned a {comm} comm error.")
 
-        For more info, see docstring of `apply_calibration` and `autocorrect_calibration`.
-        """
-        try:
-            values = self.apply_calibration(values, motor_names)
-        except JointOutOfRangeError as e:
-            print(e)
-            self._autocorrect_calibration(values, motor_names)
-            values = self.apply_calibration(values, motor_names)
-        return values
+    def calibrate_values(self, ids_values: dict[int, int]) -> dict[int, float]:
+        # TODO
+        return ids_values
 
-    def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Convert from unsigned int32 joint position range [0, 2**32[ to the universal float32 nominal degree range ]-180.0, 180.0[ with
-        a "zero position" at 0 degree.
+    def uncalibrate_values(self, ids_values: dict[int, float]) -> dict[int, int]:
+        # TODO
+        return ids_values
 
-        Note: We say "nominal degree range" since the motors can take values outside this range. For instance, 190 degrees, if the motor
-        rotate more than a half a turn from the zero position. However, most motors can't rotate more than 180 degrees and will stay in this range.
-
-        Joints values are original in [0, 2**32[ (unsigned int32). Each motor are expected to complete a full rotation
-        when given a goal position that is + or - their resolution. For instance, dynamixel xl330-m077 have a resolution of 4096, and
-        at any position in their original range, let's say the position 56734, they complete a full rotation clockwise by moving to 60830,
-        or anticlockwise by moving to 52638. The position in the original range is arbitrary and might change a lot between each motor.
-        To harmonize between motors of the same model, different robots, or even models of different brands, we propose to work
-        in the centered nominal degree range ]-180, 180[.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Update direction of rotation of the motor to match between leader and follower.
-                # In fact, the motor of the leader for a given joint can be assembled in an
-                # opposite direction in term of rotation than the motor of the follower on the same joint.
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from range [-2**31, 2**31] to
-                # nominal range [-resolution//2, resolution//2] (e.g. [-2048, 2048])
-                values[i] += homing_offset
-
-                # Convert from range [-resolution//2, resolution//2] to
-                # universal float32 centered degree range [-180, 180]
-                # (e.g. 2048 / (4096 // 2) * 180 = 180)
-                values[i] = values[i] / (resolution // 2) * HALF_TURN_DEGREE
-
-                if (values[i] < LOWER_BOUND_DEGREE) or (values[i] > UPPER_BOUND_DEGREE):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [-{HALF_TURN_DEGREE}, {HALF_TURN_DEGREE}] degrees (a full rotation), "
-                        f"with a maximum range of [{LOWER_BOUND_DEGREE}, {UPPER_BOUND_DEGREE}] degrees to account for joints that can rotate a bit more, "
-                        f"but present value is {values[i]} degree. "
-                        "This might be due to a cable connection issue creating an artificial 360 degrees jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Rescale the present position to a nominal range [0, 100] %,
-                # useful for joints with linear motions like Aloha gripper
-                values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
-
-                if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [0, 100] % (a full linear translation), "
-                        f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, "
-                        f"but present value is {values[i]} %. "
-                        "This might be due to a cable connection issue creating an artificial jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-        return values
-
-    def _autocorrect_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """This function automatically detects issues with values of motors after calibration, and correct for these issues.
-
-        Some motors might have values outside of expected maximum bounds after calibration.
-        For instance, for a joint in degree, its value can be outside [-270, 270] degrees, which is totally unexpected given
-        a nominal range of [-180, 180] degrees, which represents half a turn to the left or right starting from zero position.
-
-        Known issues:
-        #1: Motor value randomly shifts of a full turn, caused by hardware/connection errors.
-        #2: Motor internal homing offset is shifted by a full turn, caused by using default calibration (e.g Aloha).
-        #3: motor internal homing offset is shifted by less or more than a full turn, caused by using default calibration
-            or by human error during manual calibration.
-
-        Issues #1 and #2 can be solved by shifting the calibration homing offset by a full turn.
-        Issue #3 will be visually detected by user and potentially captured by the safety feature `max_relative_target`,
-        that will slow down the motor, raise an error asking to recalibrate. Manual recalibrating will solve the issue.
-
-        Note: A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-        """
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Update direction of rotation of the motor to match between leader and follower.
-                # In fact, the motor of the leader for a given joint can be assembled in an
-                # opposite direction in term of rotation than the motor of the follower on the same joint.
-                if drive_mode:
-                    values[i] *= -1
-
-                # Convert from initial range to range [-180, 180] degrees
-                calib_val = (values[i] + homing_offset) / (resolution // 2) * HALF_TURN_DEGREE
-                in_range = (calib_val > LOWER_BOUND_DEGREE) and (calib_val < UPPER_BOUND_DEGREE)
-
-                # Solve this inequality to find the factor to shift the range into [-180, 180] degrees
-                # values[i] = (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE
-                # - HALF_TURN_DEGREE <= (values[i] + homing_offset + resolution * factor) / (resolution // 2) * HALF_TURN_DEGREE <= HALF_TURN_DEGREE
-                # (- (resolution // 2) - values[i] - homing_offset) / resolution <= factor <= ((resolution // 2) - values[i] - homing_offset) / resolution
-                low_factor = (-(resolution // 2) - values[i] - homing_offset) / resolution
-                upp_factor = ((resolution // 2) - values[i] - homing_offset) / resolution
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from initial range to range [0, 100] in %
-                calib_val = (values[i] - start_pos) / (end_pos - start_pos) * 100
-                in_range = (calib_val > LOWER_BOUND_LINEAR) and (calib_val < UPPER_BOUND_LINEAR)
-
-                # Solve this inequality to find the factor to shift the range into [0, 100] %
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos + resolution * factor - start_pos - resolution * factor) * 100
-                # values[i] = (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100
-                # 0 <= (values[i] - start_pos + resolution * factor) / (end_pos - start_pos) * 100 <= 100
-                # (start_pos - values[i]) / resolution <= factor <= (end_pos - values[i]) / resolution
-                low_factor = (start_pos - values[i]) / resolution
-                upp_factor = (end_pos - values[i]) / resolution
-
-            if not in_range:
-                # Get first integer between the two bounds
-                if low_factor < upp_factor:
-                    factor = math.ceil(low_factor)
-
-                    if factor > upp_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-                else:
-                    factor = math.ceil(upp_factor)
-
-                    if factor > low_factor:
-                        raise ValueError(f"No integer found between bounds [{low_factor=}, {upp_factor=}]")
-
-                if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                    out_of_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                    in_range_str = f"{LOWER_BOUND_DEGREE} < {calib_val} < {UPPER_BOUND_DEGREE} degrees"
-                elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                    out_of_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-                    in_range_str = f"{LOWER_BOUND_LINEAR} < {calib_val} < {UPPER_BOUND_LINEAR} %"
-
-                logging.warning(
-                    f"Auto-correct calibration of motor '{name}' by shifting value by {abs(factor)} full turns, "
-                    f"from '{out_of_range_str}' to '{in_range_str}'."
-                )
-
-                # A full turn corresponds to 360 degrees but also to 4096 steps for a motor resolution of 4096.
-                self.calibration["homing_offset"][calib_idx] += resolution * factor
-
-    def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        # TODO(aliberts): remove np
-        """Inverse of `apply_calibration`."""
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                drive_mode = self.calibration["drive_mode"][calib_idx]
-                homing_offset = self.calibration["homing_offset"][calib_idx]
-                _, model = self.motors[name]
-                resolution = self.model_resolution[model]
-
-                # Convert from nominal 0-centered degree range [-180, 180] to
-                # 0-centered resolution range (e.g. [-2048, 2048] for resolution=4096)
-                values[i] = values[i] / HALF_TURN_DEGREE * (resolution // 2)
-
-                # Subtract the homing offsets to come back to actual motor range of values
-                # which can be arbitrary.
-                values[i] -= homing_offset
-
-                # Remove drive mode, which is the rotation direction of the motor, to come back to
-                # actual motor rotation direction which can be arbitrary.
-                if drive_mode:
-                    values[i] *= -1
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from nominal lnear range of [0, 100] % to
-                # actual motor range of values which can be arbitrary.
-                values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos
-
-        values = np.round(values).astype(np.int32)
-        return values
-
-    def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
+    def _is_comm_success(self, comm: int) -> bool:
         import dynamixel_sdk as dxl
 
-        return_list = True
-        if not isinstance(motor_ids, list):
-            return_list = False
-            motor_ids = [motor_ids]
+        return comm == dxl.COMM_SUCCESS
 
-        assert_same_address(self.model_ctrl_table, self.motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = dxl.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes)
-        for idx in motor_ids:
-            group.addParam(idx)
+    @staticmethod
+    def split_int_bytes(value: int, n_bytes: int) -> list[int]:
+        # Validate input
+        if value < 0:
+            raise ValueError(f"Negative values are not allowed: {value}")
 
-        for _ in range(num_retry):
-            comm = group.txRxPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
+        max_value = {1: 0xFF, 2: 0xFFFF, 4: 0xFFFFFFFF}.get(n_bytes)
+        if max_value is None:
+            raise NotImplementedError(f"Unsupported byte size: {n_bytes}. Expected [1, 2, 4].")
 
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
+        if value > max_value:
+            raise ValueError(f"Value {value} exceeds the maximum for {n_bytes} bytes ({max_value}).")
 
-        values = []
-        for idx in motor_ids:
-            value = group.getData(idx, addr, bytes)
-            values.append(value)
-
-        if return_list:
-            return values
-        else:
-            return values[0]
-
-    def _read(self, data_name: str, motor_names: list[str]):
         import dynamixel_sdk as dxl
 
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        if data_name not in self.group_readers:
-            # create new group reader
-            self.group_readers[group_key] = dxl.GroupSyncRead(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-            for idx in motor_ids:
-                self.group_readers[group_key].addParam(idx)
-
-        for _ in range(NUM_READ_RETRY):
-            comm = self.group_readers[group_key].txRxPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
-
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = self.group_readers[group_key].getData(idx, addr, bytes)
-            values.append(value)
-
-        values = np.array(values)
-
-        # Convert to signed int to use range [-2048, 2048] for our motor positions.
-        if data_name in CONVERT_UINT32_TO_INT32_REQUIRED:
-            values = values.astype(np.int32)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self._apply_calibration_autocorrect(values, motor_names)
-
-        return values
-
-    def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
-        import dynamixel_sdk as dxl
-
-        if not isinstance(motor_ids, list):
-            motor_ids = [motor_ids]
-        if not isinstance(values, list):
-            values = [values]
-
-        assert_same_address(self.model_ctrl_table, motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = dxl.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes)
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes)
-            group.addParam(idx, data)
-
-        for _ in range(num_retry):
-            comm = group.txPacket()
-            if comm == dxl.COMM_SUCCESS:
-                break
-
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-    def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None:
-        import dynamixel_sdk as dxl
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.revert_calibration(values, motor_names)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        init_group = data_name not in self.group_readers
-        if init_group:
-            self.group_writers[group_key] = dxl.GroupSyncWrite(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes)
-            if init_group:
-                self.group_writers[group_key].addParam(idx, data)
-            else:
-                self.group_writers[group_key].changeParam(idx, data)
-
-        comm = self.group_writers[group_key].txPacket()
-        if comm != dxl.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-
-def set_operating_mode(bus: DynamixelMotorsBus):
-    if (bus.read("Torque_Enable") != TorqueMode.DISABLED.value).any():
-        raise ValueError("To run set robot preset, the torque must be disabled on all motors.")
-
-    # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
-    # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
-    # you could end up with a servo with a position 0 or 4095 at a crucial point See [
-    # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
-    all_motors_except_gripper = [name for name in bus.motor_names if name != "gripper"]
-    if len(all_motors_except_gripper) > 0:
-        # 4 corresponds to Extended Position on Koch motors
-        bus.write("Operating_Mode", 4, all_motors_except_gripper)
-
-    # Use 'position control current based' for gripper to be limited by the limit of the current.
-    # For the follower gripper, it means it can grasp an object without forcing too much even tho,
-    # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
-    # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
-    # to make it move, and it will move back to its original target position when we release the force.
-    # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
-    bus.write("Operating_Mode", 5, "gripper")
+        # Note: No need to convert back into unsigned int, since this byte preprocessing
+        # already handles it for us.
+        if n_bytes == 1:
+            data = [
+                dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
+            ]
+        elif n_bytes == 2:
+            data = [
+                dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
+                dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
+            ]
+        elif n_bytes == 4:
+            data = [
+                dxl.DXL_LOBYTE(dxl.DXL_LOWORD(value)),
+                dxl.DXL_HIBYTE(dxl.DXL_LOWORD(value)),
+                dxl.DXL_LOBYTE(dxl.DXL_HIWORD(value)),
+                dxl.DXL_HIBYTE(dxl.DXL_HIWORD(value)),
+            ]
+        return data

lerobot/common/motors/dynamixel/dynamixel_calibration.py

@@ -17,12 +17,8 @@
 
 import numpy as np
 
-from ..motors_bus import MotorsBus
-from .dynamixel import (
-    CalibrationMode,
-    TorqueMode,
-    convert_degrees_to_steps,
-)
+from ..motors_bus import CalibrationMode, MotorsBus, TorqueMode
+from .tables import MODEL_RESOLUTION
 
 URL_TEMPLATE = (
     "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
@@ -49,6 +45,17 @@ def apply_drive_mode(position, drive_mode):
     return position
 
 
+def convert_degrees_to_steps(degrees: float | np.ndarray, models: str | list[str]) -> np.ndarray:
+    """This function converts the degree range to the step range for indicating motors rotation.
+    It assumes a motor achieves a full rotation by going from -180 degree position to +180.
+    The motor resolution (e.g. 4096) corresponds to the number of steps needed to achieve a full rotation.
+    """
+    resolutions = [MODEL_RESOLUTION[model] for model in models]
+    steps = degrees / 180 * np.array(resolutions) / 2
+    steps = steps.astype(int)
+    return steps
+
+
 def compute_nearest_rounded_position(position, models):
     delta_turn = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, models)
     nearest_pos = np.round(position.astype(float) / delta_turn) * delta_turn
@@ -89,11 +96,11 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type
     # We arbitrarily chose our zero target position to be a straight horizontal position with gripper upwards and closed.
     # It is easy to identify and all motors are in a "quarter turn" position. Once calibration is done, this position will
     # correspond to every motor angle being 0. If you set all 0 as Goal Position, the arm will move in this position.
-    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.motor_models)
+    zero_target_pos = convert_degrees_to_steps(ZERO_POSITION_DEGREE, arm.models)
 
     # Compute homing offset so that `present_position + homing_offset ~= target_position`.
     zero_pos = arm.read("Present_Position")
-    zero_nearest_pos = compute_nearest_rounded_position(zero_pos, arm.motor_models)
+    zero_nearest_pos = compute_nearest_rounded_position(zero_pos, arm.models)
     homing_offset = zero_target_pos - zero_nearest_pos
 
     # The rotated target position corresponds to a rotation of a quarter turn from the zero position.
@@ -107,7 +114,7 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type
     print("See: " + URL_TEMPLATE.format(robot=robot_type, arm=arm_type, position="rotated"))
     input("Press Enter to continue...")
 
-    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.motor_models)
+    rotated_target_pos = convert_degrees_to_steps(ROTATED_POSITION_DEGREE, arm.models)
 
     # Find drive mode by rotating each motor by a quarter of a turn.
     # Drive mode indicates if the motor rotation direction should be inverted (=1) or not (=0).
@@ -116,7 +123,7 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type
 
     # Re-compute homing offset to take into account drive mode
     rotated_drived_pos = apply_drive_mode(rotated_pos, drive_mode)
-    rotated_nearest_pos = compute_nearest_rounded_position(rotated_drived_pos, arm.motor_models)
+    rotated_nearest_pos = compute_nearest_rounded_position(rotated_drived_pos, arm.models)
     homing_offset = rotated_target_pos - rotated_nearest_pos
 
     print("\nMove arm to rest position")
@@ -125,12 +132,12 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type
     print()
 
     # Joints with rotational motions are expressed in degrees in nominal range of [-180, 180]
-    calib_mode = [CalibrationMode.DEGREE.name] * len(arm.motor_names)
+    calib_mode = [CalibrationMode.DEGREE.name] * len(arm.names)
 
     # TODO(rcadene): make type of joints (DEGREE or LINEAR) configurable from yaml?
-    if robot_type in ["aloha"] and "gripper" in arm.motor_names:
+    if robot_type in ["aloha"] and "gripper" in arm.names:
         # Joints with linear motions (like gripper of Aloha) are expressed in nominal range of [0, 100]
-        calib_idx = arm.motor_names.index("gripper")
+        calib_idx = arm.names.index("gripper")
         calib_mode[calib_idx] = CalibrationMode.LINEAR.name
 
     calib_data = {
@@ -139,6 +146,6 @@ def run_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm_type
         "start_pos": zero_pos.tolist(),
         "end_pos": rotated_pos.tolist(),
         "calib_mode": calib_mode,
-        "motor_names": arm.motor_names,
+        "motor_names": arm.names,
     }
     return calib_data

lerobot/common/motors/dynamixel/tables.py

@@ -0,0 +1,110 @@
+# data_name: (address, size_byte)
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table
+X_SERIES_CONTROL_TABLE = {
+    "Model_Number": (0, 2),
+    "Model_Information": (2, 4),
+    "Firmware_Version": (6, 1),
+    "ID": (7, 1),
+    "Baud_Rate": (8, 1),
+    "Return_Delay_Time": (9, 1),
+    "Drive_Mode": (10, 1),
+    "Operating_Mode": (11, 1),
+    "Secondary_ID": (12, 1),
+    "Protocol_Type": (13, 1),
+    "Homing_Offset": (20, 4),
+    "Moving_Threshold": (24, 4),
+    "Temperature_Limit": (31, 1),
+    "Max_Voltage_Limit": (32, 2),
+    "Min_Voltage_Limit": (34, 2),
+    "PWM_Limit": (36, 2),
+    "Current_Limit": (38, 2),
+    "Acceleration_Limit": (40, 4),
+    "Velocity_Limit": (44, 4),
+    "Max_Position_Limit": (48, 4),
+    "Min_Position_Limit": (52, 4),
+    "Shutdown": (63, 1),
+    "Torque_Enable": (64, 1),
+    "LED": (65, 1),
+    "Status_Return_Level": (68, 1),
+    "Registered_Instruction": (69, 1),
+    "Hardware_Error_Status": (70, 1),
+    "Velocity_I_Gain": (76, 2),
+    "Velocity_P_Gain": (78, 2),
+    "Position_D_Gain": (80, 2),
+    "Position_I_Gain": (82, 2),
+    "Position_P_Gain": (84, 2),
+    "Feedforward_2nd_Gain": (88, 2),
+    "Feedforward_1st_Gain": (90, 2),
+    "Bus_Watchdog": (98, 1),
+    "Goal_PWM": (100, 2),
+    "Goal_Current": (102, 2),
+    "Goal_Velocity": (104, 4),
+    "Profile_Acceleration": (108, 4),
+    "Profile_Velocity": (112, 4),
+    "Goal_Position": (116, 4),
+    "Realtime_Tick": (120, 2),
+    "Moving": (122, 1),
+    "Moving_Status": (123, 1),
+    "Present_PWM": (124, 2),
+    "Present_Current": (126, 2),
+    "Present_Velocity": (128, 4),
+    "Present_Position": (132, 4),
+    "Velocity_Trajectory": (136, 4),
+    "Position_Trajectory": (140, 4),
+    "Present_Input_Voltage": (144, 2),
+    "Present_Temperature": (146, 1),
+}
+
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#baud-rate8
+X_SERIES_BAUDRATE_TABLE = {
+    0: 9_600,
+    1: 57_600,
+    2: 115_200,
+    3: 1_000_000,
+    4: 2_000_000,
+    5: 3_000_000,
+    6: 4_000_000,
+}
+
+# {model: model_resolution}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#specifications
+MODEL_RESOLUTION = {
+    "x_series": 4096,
+    "xl330-m077": 4096,
+    "xl330-m288": 4096,
+    "xl430-w250": 4096,
+    "xm430-w350": 4096,
+    "xm540-w270": 4096,
+    "xc430-w150": 4096,
+}
+
+# {model: model_number}
+# https://emanual.robotis.com/docs/en/dxl/x/{MODEL}/#control-table-of-eeprom-area
+MODELS_TABLE = {
+    "xl330-m077": 1190,
+    "xl330-m288": 1200,
+    "xl430-w250": 1060,
+    "xm430-w350": 1020,
+    "xm540-w270": 1120,
+    "xc430-w150": 1070,
+}
+
+MODEL_CONTROL_TABLE = {
+    "x_series": X_SERIES_CONTROL_TABLE,
+    "xl330-m077": X_SERIES_CONTROL_TABLE,
+    "xl330-m288": X_SERIES_CONTROL_TABLE,
+    "xl430-w250": X_SERIES_CONTROL_TABLE,
+    "xm430-w350": X_SERIES_CONTROL_TABLE,
+    "xm540-w270": X_SERIES_CONTROL_TABLE,
+    "xc430-w150": X_SERIES_CONTROL_TABLE,
+}
+
+MODEL_BAUDRATE_TABLE = {
+    "x_series": X_SERIES_BAUDRATE_TABLE,
+    "xl330-m077": X_SERIES_BAUDRATE_TABLE,
+    "xl330-m288": X_SERIES_BAUDRATE_TABLE,
+    "xl430-w250": X_SERIES_BAUDRATE_TABLE,
+    "xm430-w350": X_SERIES_BAUDRATE_TABLE,
+    "xm540-w270": X_SERIES_BAUDRATE_TABLE,
+    "xc430-w150": X_SERIES_BAUDRATE_TABLE,
+}

lerobot/common/motors/feetech/__init__.py

@@ -1,8 +1,3 @@
 from .feetech import FeetechMotorsBus
 from .feetech_calibration import apply_feetech_offsets_from_calibration, run_full_arm_calibration
-
-__all__ = [
-    "FeetechMotorsBus",
-    "apply_feetech_offsets_from_calibration",
-    "run_full_arm_calibration",
-]
+from .tables import *

lerobot/common/motors/feetech/feetech.py

@@ -14,204 +14,20 @@
 
 from copy import deepcopy
 
-import numpy as np
-
-from ..motors_bus import (
-    CalibrationMode,
-    JointOutOfRangeError,
-    MotorsBus,
-    assert_same_address,
-    get_group_sync_key,
+from ..motors_bus import MotorsBus
+from .tables import (
+    CALIBRATION_REQUIRED,
+    MODEL_BAUDRATE_TABLE,
+    MODEL_CONTROL_TABLE,
+    MODEL_RESOLUTION,
 )
 
 PROTOCOL_VERSION = 0
 BAUDRATE = 1_000_000
-TIMEOUT_MS = 1000
+DEFAULT_TIMEOUT_MS = 1000
 
 MAX_ID_RANGE = 252
 
-# For joints in percentage (i.e. joints that move linearly like the prismatic joint of a gripper),
-# their nominal range is [0, 100] %. For instance, for Aloha gripper, 0% is fully
-# closed, and 100% is fully open. To account for slight calibration issue, we allow up to
-# [-10, 110] until an error is raised.
-LOWER_BOUND_LINEAR = -10
-UPPER_BOUND_LINEAR = 110
-
-HALF_TURN_DEGREE = 180
-
-# See this link for STS3215 Memory Table:
-# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
-# data_name: (address, size_byte)
-SCS_SERIES_CONTROL_TABLE = {
-    "Model": (3, 2),
-    "ID": (5, 1),
-    "Baud_Rate": (6, 1),
-    "Return_Delay": (7, 1),
-    "Response_Status_Level": (8, 1),
-    "Min_Angle_Limit": (9, 2),
-    "Max_Angle_Limit": (11, 2),
-    "Max_Temperature_Limit": (13, 1),
-    "Max_Voltage_Limit": (14, 1),
-    "Min_Voltage_Limit": (15, 1),
-    "Max_Torque_Limit": (16, 2),
-    "Phase": (18, 1),
-    "Unloading_Condition": (19, 1),
-    "LED_Alarm_Condition": (20, 1),
-    "P_Coefficient": (21, 1),
-    "D_Coefficient": (22, 1),
-    "I_Coefficient": (23, 1),
-    "Minimum_Startup_Force": (24, 2),
-    "CW_Dead_Zone": (26, 1),
-    "CCW_Dead_Zone": (27, 1),
-    "Protection_Current": (28, 2),
-    "Angular_Resolution": (30, 1),
-    "Offset": (31, 2),
-    "Mode": (33, 1),
-    "Protective_Torque": (34, 1),
-    "Protection_Time": (35, 1),
-    "Overload_Torque": (36, 1),
-    "Speed_closed_loop_P_proportional_coefficient": (37, 1),
-    "Over_Current_Protection_Time": (38, 1),
-    "Velocity_closed_loop_I_integral_coefficient": (39, 1),
-    "Torque_Enable": (40, 1),
-    "Acceleration": (41, 1),
-    "Goal_Position": (42, 2),
-    "Goal_Time": (44, 2),
-    "Goal_Speed": (46, 2),
-    "Torque_Limit": (48, 2),
-    "Lock": (55, 1),
-    "Present_Position": (56, 2),
-    "Present_Speed": (58, 2),
-    "Present_Load": (60, 2),
-    "Present_Voltage": (62, 1),
-    "Present_Temperature": (63, 1),
-    "Status": (65, 1),
-    "Moving": (66, 1),
-    "Present_Current": (69, 2),
-    # Not in the Memory Table
-    "Maximum_Acceleration": (85, 2),
-}
-
-SCS_SERIES_BAUDRATE_TABLE = {
-    0: 1_000_000,
-    1: 500_000,
-    2: 250_000,
-    3: 128_000,
-    4: 115_200,
-    5: 57_600,
-    6: 38_400,
-    7: 19_200,
-}
-
-CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]
-
-MODEL_CONTROL_TABLE = {
-    "scs_series": SCS_SERIES_CONTROL_TABLE,
-    "sts3215": SCS_SERIES_CONTROL_TABLE,
-}
-
-MODEL_RESOLUTION = {
-    "scs_series": 4096,
-    "sts3215": 4096,
-}
-
-MODEL_BAUDRATE_TABLE = {
-    "scs_series": SCS_SERIES_BAUDRATE_TABLE,
-    "sts3215": SCS_SERIES_BAUDRATE_TABLE,
-}
-
-# High number of retries is needed for feetech compared to dynamixel motors.
-NUM_READ_RETRY = 20
-NUM_WRITE_RETRY = 20
-
-
-def convert_ticks_to_degrees(ticks, model):
-    resolutions = MODEL_RESOLUTION[model]
-    # Convert the ticks to degrees
-    return ticks * (360.0 / resolutions)
-
-
-def convert_degrees_to_ticks(degrees, model):
-    resolutions = MODEL_RESOLUTION[model]
-    # Convert degrees to motor ticks
-    return int(degrees * (resolutions / 360.0))
-
-
-def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motorbus, motor_id: int) -> int:
-    """
-    Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048'
-    becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution).
-    """
-    resolutions = MODEL_RESOLUTION[model]
-
-    # Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
-    ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
-
-    # If above halfway, fold it into negative territory => [-2048..+2047].
-    if ticks > (resolutions // 2):
-        ticks -= resolutions
-
-    # Flip sign if drive_mode is set.
-    drive_mode = 0
-    if motorbus.calibration is not None:
-        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-
-    if drive_mode:
-        ticks *= -1
-
-    return ticks
-
-
-def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motorbus, motor_id: int) -> int:
-    """
-    Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047]
-    and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
-    """
-    # Flip sign if drive_mode was set.
-    drive_mode = 0
-    if motorbus.calibration is not None:
-        drive_mode = motorbus.calibration["drive_mode"][motor_id - 1]
-
-    if drive_mode:
-        adjusted_pos *= -1
-
-    resolutions = MODEL_RESOLUTION[model]
-
-    # Shift by +half-resolution and wrap into [0..res-1].
-    # This undoes the earlier shift by -half-resolution.
-    ticks = (adjusted_pos + (resolutions // 2)) % resolutions
-
-    return ticks
-
-
-def convert_to_bytes(value, n_bytes: int):
-    import scservo_sdk as scs
-
-    # Note: No need to convert back into unsigned int, since this byte preprocessing
-    # already handles it for us.
-    if n_bytes == 1:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-        ]
-    elif n_bytes == 2:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
-        ]
-    elif n_bytes == 4:
-        data = [
-            scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
-            scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
-            scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
-        ]
-    else:
-        raise NotImplementedError(
-            f"Value of the number of bytes to be sent is expected to be in [1, 2, 4], but "
-            f"{n_bytes} is provided instead."
-        )
-    return data
-
 
 class FeetechMotorsBus(MotorsBus):
     """
@@ -222,6 +38,8 @@ class FeetechMotorsBus(MotorsBus):
     model_ctrl_table = deepcopy(MODEL_CONTROL_TABLE)
     model_resolution_table = deepcopy(MODEL_RESOLUTION)
     model_baudrate_table = deepcopy(MODEL_BAUDRATE_TABLE)
+    calibration_required = deepcopy(CALIBRATION_REQUIRED)
+    default_timeout = DEFAULT_TIMEOUT_MS
 
     def __init__(
         self,
@@ -229,224 +47,60 @@ class FeetechMotorsBus(MotorsBus):
         motors: dict[str, tuple[int, str]],
     ):
         super().__init__(port, motors)
-
-    def _set_handlers(self):
         import scservo_sdk as scs
 
         self.port_handler = scs.PortHandler(self.port)
         self.packet_handler = scs.PacketHandler(PROTOCOL_VERSION)
+        self.reader = scs.GroupSyncRead(self.port_handler, self.packet_handler, 0, 0)
+        self.writer = scs.GroupSyncWrite(self.port_handler, self.packet_handler, 0, 0)
 
-    def _set_timeout(self, timeout: int = TIMEOUT_MS):
-        self.port_handler.setPacketTimeoutMillis(timeout)
+    def broadcast_ping(self, num_retry: int | None = None):
+        raise NotImplementedError  # TODO
 
-    def apply_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        if motor_names is None:
-            motor_names = self.motor_names
+    def calibrate_values(self, ids_values: dict[int, int]) -> dict[int, float]:
+        # TODO
+        return ids_values
 
-        # Convert from unsigned int32 original range [0, 2**32] to signed float32 range
-        values = values.astype(np.float32)
+    def uncalibrate_values(self, ids_values: dict[int, float]) -> dict[int, int]:
+        # TODO
+        return ids_values
 
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                motor_idx, model = self.motors[name]
-
-                # Convert raw motor ticks to homed ticks, then convert the homed ticks to degrees
-                values[i] = adjusted_to_homing_ticks(values[i], model, self, motor_idx)
-                values[i] = convert_ticks_to_degrees(values[i], model)
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Rescale the present position to a nominal range [0, 100] %,
-                # useful for joints with linear motions like Aloha gripper
-                values[i] = (values[i] - start_pos) / (end_pos - start_pos) * 100
-
-                if (values[i] < LOWER_BOUND_LINEAR) or (values[i] > UPPER_BOUND_LINEAR):
-                    raise JointOutOfRangeError(
-                        f"Wrong motor position range detected for {name}. "
-                        f"Expected to be in nominal range of [0, 100] % (a full linear translation), "
-                        f"with a maximum range of [{LOWER_BOUND_LINEAR}, {UPPER_BOUND_LINEAR}] % to account for some imprecision during calibration, "
-                        f"but present value is {values[i]} %. "
-                        "This might be due to a cable connection issue creating an artificial jump in motor values. "
-                        "You need to recalibrate by running: `python lerobot/scripts/control_robot.py calibrate`"
-                    )
-
-        return values
-
-    def revert_calibration(self, values: np.ndarray | list, motor_names: list[str] | None):
-        """Inverse of `apply_calibration`."""
-        if motor_names is None:
-            motor_names = self.motor_names
-
-        for i, name in enumerate(motor_names):
-            calib_idx = self.calibration["motor_names"].index(name)
-            calib_mode = self.calibration["calib_mode"][calib_idx]
-
-            if CalibrationMode[calib_mode] == CalibrationMode.DEGREE:
-                motor_idx, model = self.motors[name]
-
-                # Convert degrees to homed ticks, then convert the homed ticks to raw ticks
-                values[i] = convert_degrees_to_ticks(values[i], model)
-                values[i] = adjusted_to_motor_ticks(values[i], model, self, motor_idx)
-
-            elif CalibrationMode[calib_mode] == CalibrationMode.LINEAR:
-                start_pos = self.calibration["start_pos"][calib_idx]
-                end_pos = self.calibration["end_pos"][calib_idx]
-
-                # Convert from nominal lnear range of [0, 100] % to
-                # actual motor range of values which can be arbitrary.
-                values[i] = values[i] / 100 * (end_pos - start_pos) + start_pos
-
-        values = np.round(values).astype(np.int32)
-        return values
-
-    def read_with_motor_ids(self, motor_models, motor_ids, data_name, num_retry=NUM_READ_RETRY):
+    def _is_comm_success(self, comm: int) -> bool:
         import scservo_sdk as scs
 
-        return_list = True
-        if not isinstance(motor_ids, list):
-            return_list = False
-            motor_ids = [motor_ids]
+        return comm == scs.COMM_SUCCESS
 
-        assert_same_address(self.model_ctrl_table, self.motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = scs.GroupSyncRead(self.port_handler, self.packet_handler, addr, bytes)
-        for idx in motor_ids:
-            group.addParam(idx)
+    @staticmethod
+    def split_int_bytes(value: int, n_bytes: int) -> list[int]:
+        # Validate input
+        if value < 0:
+            raise ValueError(f"Negative values are not allowed: {value}")
 
-        for _ in range(num_retry):
-            comm = group.txRxPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
+        max_value = {1: 0xFF, 2: 0xFFFF, 4: 0xFFFFFFFF}.get(n_bytes)
+        if max_value is None:
+            raise NotImplementedError(f"Unsupported byte size: {n_bytes}. Expected [1, 2, 4].")
 
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
+        if value > max_value:
+            raise ValueError(f"Value {value} exceeds the maximum for {n_bytes} bytes ({max_value}).")
 
-        values = []
-        for idx in motor_ids:
-            value = group.getData(idx, addr, bytes)
-            values.append(value)
-
-        if return_list:
-            return values
-        else:
-            return values[0]
-
-    def _read(self, data_name: str, motor_names: list[str]):
         import scservo_sdk as scs
 
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        if data_name not in self.group_readers:
-            # Very Important to flush the buffer!
-            self.port_handler.ser.reset_output_buffer()
-            self.port_handler.ser.reset_input_buffer()
-
-            # Create new group reader
-            self.group_readers[group_key] = scs.GroupSyncRead(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-            for idx in motor_ids:
-                self.group_readers[group_key].addParam(idx)
-
-        for _ in range(NUM_READ_RETRY):
-            comm = self.group_readers[group_key].txRxPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
-
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Read failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-        values = []
-        for idx in motor_ids:
-            value = self.group_readers[group_key].getData(idx, addr, bytes)
-            values.append(value)
-
-        values = np.array(values)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.apply_calibration(values, motor_names)
-
-        return values
-
-    def write_with_motor_ids(self, motor_models, motor_ids, data_name, values, num_retry=NUM_WRITE_RETRY):
-        import scservo_sdk as scs
-
-        if not isinstance(motor_ids, list):
-            motor_ids = [motor_ids]
-        if not isinstance(values, list):
-            values = [values]
-
-        assert_same_address(self.model_ctrl_table, motor_models, data_name)
-        addr, bytes = self.model_ctrl_table[motor_models[0]][data_name]
-        group = scs.GroupSyncWrite(self.port_handler, self.packet_handler, addr, bytes)
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes)
-            group.addParam(idx, data)
-
-        for _ in range(num_retry):
-            comm = group.txPacket()
-            if comm == scs.COMM_SUCCESS:
-                break
-
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port_handler.port_name} for indices {motor_ids}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
-
-    def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None:
-        import scservo_sdk as scs
-
-        motor_ids = []
-        models = []
-        for name in motor_names:
-            motor_idx, model = self.motors[name]
-            motor_ids.append(motor_idx)
-            models.append(model)
-
-        if data_name in CALIBRATION_REQUIRED and self.calibration is not None:
-            values = self.revert_calibration(values, motor_names)
-
-        assert_same_address(self.model_ctrl_table, models, data_name)
-        addr, bytes = self.model_ctrl_table[model][data_name]
-        group_key = get_group_sync_key(data_name, motor_names)
-
-        init_group = data_name not in self.group_readers
-        if init_group:
-            self.group_writers[group_key] = scs.GroupSyncWrite(
-                self.port_handler, self.packet_handler, addr, bytes
-            )
-
-        for idx, value in zip(motor_ids, values, strict=True):
-            data = convert_to_bytes(value, bytes)
-            if init_group:
-                self.group_writers[group_key].addParam(idx, data)
-            else:
-                self.group_writers[group_key].changeParam(idx, data)
-
-        comm = self.group_writers[group_key].txPacket()
-        if comm != scs.COMM_SUCCESS:
-            raise ConnectionError(
-                f"Write failed due to communication error on port {self.port} for group_key {group_key}: "
-                f"{self.packet_handler.getTxRxResult(comm)}"
-            )
+        # Note: No need to convert back into unsigned int, since this byte preprocessing
+        # already handles it for us.
+        if n_bytes == 1:
+            data = [
+                scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
+            ]
+        elif n_bytes == 2:
+            data = [
+                scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
+                scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
+            ]
+        elif n_bytes == 4:
+            data = [
+                scs.SCS_LOBYTE(scs.SCS_LOWORD(value)),
+                scs.SCS_HIBYTE(scs.SCS_LOWORD(value)),
+                scs.SCS_LOBYTE(scs.SCS_HIWORD(value)),
+                scs.SCS_HIBYTE(scs.SCS_HIWORD(value)),
+            ]
+        return data

lerobot/common/motors/feetech/feetech_calibration.py

@@ -13,11 +13,13 @@
 # limitations under the License.
 import numpy as np
 
-from ..motors_bus import MotorsBus, TorqueMode
-from .feetech import (
+from ..motors_bus import (
     CalibrationMode,
-    FeetechMotorsBus,
+    MotorsBus,
+    TorqueMode,
 )
+from .feetech import FeetechMotorsBus
+from .tables import MODEL_RESOLUTION
 
 URL_TEMPLATE = (
     "https://raw.githubusercontent.com/huggingface/lerobot/main/media/{robot}/{arm}_{position}.webp"
@@ -33,7 +35,7 @@ def get_calibration_modes(arm: MotorsBus):
     """Returns calibration modes for each motor (DEGREE for rotational, LINEAR for gripper)."""
     return [
         CalibrationMode.LINEAR.name if name == "gripper" else CalibrationMode.DEGREE.name
-        for name in arm.motor_names
+        for name in arm.names
     ]
 
 
@@ -105,7 +107,7 @@ def single_motor_calibration(arm: MotorsBus, motor_id: int):
         "start_pos": int(start_pos),
         "end_pos": int(end_pos),
         "calib_mode": get_calibration_modes(arm)[motor_id - 1],
-        "motor_name": arm.motor_names[motor_id - 1],
+        "motor_name": arm.names[motor_id - 1],
     }
 
     return calib_dict
@@ -140,19 +142,19 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
     )  # TODO(pepijn): replace with new instruction homing pos (all motors in middle) in tutorial
     input("Press Enter to continue...")
 
-    start_positions = np.zeros(len(arm.motor_indices))
-    end_positions = np.zeros(len(arm.motor_indices))
-    encoder_offsets = np.zeros(len(arm.motor_indices))
+    start_positions = np.zeros(len(arm.ids))
+    end_positions = np.zeros(len(arm.ids))
+    encoder_offsets = np.zeros(len(arm.ids))
 
     modes = get_calibration_modes(arm)
 
-    for i, motor_id in enumerate(arm.motor_indices):
+    for i, motor_id in enumerate(arm.ids):
         if modes[i] == CalibrationMode.DEGREE.name:
             encoder_offsets[i] = calibrate_homing_motor(motor_id, arm)
             start_positions[i] = 0
             end_positions[i] = 0
 
-    for i, motor_id in enumerate(arm.motor_indices):
+    for i, motor_id in enumerate(arm.ids):
         if modes[i] == CalibrationMode.LINEAR.name:
             start_positions[i], end_positions[i] = calibrate_linear_motor(motor_id, arm)
             encoder_offsets[i] = 0
@@ -171,7 +173,7 @@ def run_full_arm_calibration(arm: MotorsBus, robot_type: str, arm_name: str, arm
         "start_pos": start_positions.astype(int).tolist(),
         "end_pos": end_positions.astype(int).tolist(),
         "calib_mode": get_calibration_modes(arm),
-        "motor_names": arm.motor_names,
+        "motor_names": arm.names,
     }
     return calib_dict
 
@@ -251,3 +253,62 @@ def apply_feetech_offsets_from_calibration(motorsbus: FeetechMotorsBus, calibrat
 
     motorsbus.write("Lock", 0)
     print("Offsets have been saved to EEPROM successfully.")
+
+
+def convert_ticks_to_degrees(ticks, model):
+    resolutions = MODEL_RESOLUTION[model]
+    # Convert the ticks to degrees
+    return ticks * (360.0 / resolutions)
+
+
+def convert_degrees_to_ticks(degrees, model):
+    resolutions = MODEL_RESOLUTION[model]
+    # Convert degrees to motor ticks
+    return int(degrees * (resolutions / 360.0))
+
+
+def adjusted_to_homing_ticks(raw_motor_ticks: int, model: str, motor_bus: MotorsBus, motor_id: int) -> int:
+    """
+    Takes a raw reading [0..(res-1)] (e.g. 0..4095) and shifts it so that '2048'
+    becomes 0 in the homed coordinate system ([-2048..+2047] for 4096 resolution).
+    """
+    resolutions = MODEL_RESOLUTION[model]
+
+    # Shift raw ticks by half-resolution so 2048 -> 0, then wrap [0..res-1].
+    ticks = (raw_motor_ticks - (resolutions // 2)) % resolutions
+
+    # If above halfway, fold it into negative territory => [-2048..+2047].
+    if ticks > (resolutions // 2):
+        ticks -= resolutions
+
+    # Flip sign if drive_mode is set.
+    drive_mode = 0
+    if motor_bus.calibration is not None:
+        drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1]
+
+    if drive_mode:
+        ticks *= -1
+
+    return ticks
+
+
+def adjusted_to_motor_ticks(adjusted_pos: int, model: str, motor_bus: MotorsBus, motor_id: int) -> int:
+    """
+    Inverse of adjusted_to_homing_ticks(). Takes a 'homed' position in [-2048..+2047]
+    and recovers the raw [0..(res-1)] ticks with 2048 as midpoint.
+    """
+    # Flip sign if drive_mode was set.
+    drive_mode = 0
+    if motor_bus.calibration is not None:
+        drive_mode = motor_bus.calibration["drive_mode"][motor_id - 1]
+
+    if drive_mode:
+        adjusted_pos *= -1
+
+    resolutions = MODEL_RESOLUTION[model]
+
+    # Shift by +half-resolution and wrap into [0..res-1].
+    # This undoes the earlier shift by -half-resolution.
+    ticks = (adjusted_pos + (resolutions // 2)) % resolutions
+
+    return ticks

lerobot/common/motors/feetech/tables.py

@@ -0,0 +1,80 @@
+# See this link for STS3215 Memory Table:
+# https://docs.google.com/spreadsheets/d/1GVs7W1VS1PqdhA1nW-abeyAHhTUxKUdR/edit?usp=sharing&ouid=116566590112741600240&rtpof=true&sd=true
+# data_name: (address, size_byte)
+SCS_SERIES_CONTROL_TABLE = {
+    "Model": (3, 2),
+    "ID": (5, 1),
+    "Baud_Rate": (6, 1),
+    "Return_Delay": (7, 1),
+    "Response_Status_Level": (8, 1),
+    "Min_Angle_Limit": (9, 2),
+    "Max_Angle_Limit": (11, 2),
+    "Max_Temperature_Limit": (13, 1),
+    "Max_Voltage_Limit": (14, 1),
+    "Min_Voltage_Limit": (15, 1),
+    "Max_Torque_Limit": (16, 2),
+    "Phase": (18, 1),
+    "Unloading_Condition": (19, 1),
+    "LED_Alarm_Condition": (20, 1),
+    "P_Coefficient": (21, 1),
+    "D_Coefficient": (22, 1),
+    "I_Coefficient": (23, 1),
+    "Minimum_Startup_Force": (24, 2),
+    "CW_Dead_Zone": (26, 1),
+    "CCW_Dead_Zone": (27, 1),
+    "Protection_Current": (28, 2),
+    "Angular_Resolution": (30, 1),
+    "Offset": (31, 2),
+    "Mode": (33, 1),
+    "Protective_Torque": (34, 1),
+    "Protection_Time": (35, 1),
+    "Overload_Torque": (36, 1),
+    "Speed_closed_loop_P_proportional_coefficient": (37, 1),
+    "Over_Current_Protection_Time": (38, 1),
+    "Velocity_closed_loop_I_integral_coefficient": (39, 1),
+    "Torque_Enable": (40, 1),
+    "Acceleration": (41, 1),
+    "Goal_Position": (42, 2),
+    "Goal_Time": (44, 2),
+    "Goal_Speed": (46, 2),
+    "Torque_Limit": (48, 2),
+    "Lock": (55, 1),
+    "Present_Position": (56, 2),
+    "Present_Speed": (58, 2),
+    "Present_Load": (60, 2),
+    "Present_Voltage": (62, 1),
+    "Present_Temperature": (63, 1),
+    "Status": (65, 1),
+    "Moving": (66, 1),
+    "Present_Current": (69, 2),
+    # Not in the Memory Table
+    "Maximum_Acceleration": (85, 2),
+}
+
+SCS_SERIES_BAUDRATE_TABLE = {
+    0: 1_000_000,
+    1: 500_000,
+    2: 250_000,
+    3: 128_000,
+    4: 115_200,
+    5: 57_600,
+    6: 38_400,
+    7: 19_200,
+}
+
+MODEL_CONTROL_TABLE = {
+    "scs_series": SCS_SERIES_CONTROL_TABLE,
+    "sts3215": SCS_SERIES_CONTROL_TABLE,
+}
+
+MODEL_RESOLUTION = {
+    "scs_series": 4096,
+    "sts3215": 4096,
+}
+
+MODEL_BAUDRATE_TABLE = {
+    "scs_series": SCS_SERIES_BAUDRATE_TABLE,
+    "sts3215": SCS_SERIES_BAUDRATE_TABLE,
+}
+
+CALIBRATION_REQUIRED = ["Goal_Position", "Present_Position"]

lerobot/common/motors/motors_bus.py

@@ -14,35 +14,32 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-# TODO(aliberts): This noqa is for the PortHandler / PacketHandler Protocols
-# Add block noqa when feature below is available
-# https://github.com/astral-sh/ruff/issues/3711
 # ruff: noqa: N802
+# This noqa is for the Protocols classes: PortHandler, PacketHandler GroupSyncRead/Write
+# TODO(aliberts): Add block noqa when feature below is available
+# https://github.com/astral-sh/ruff/issues/3711
 
 import abc
-import time
-import traceback
+import json
+import logging
+from dataclasses import dataclass
 from enum import Enum
-from typing import Protocol
+from functools import cached_property
+from pathlib import Path
+from pprint import pformat
+from typing import Protocol, TypeAlias, overload
 
-import numpy as np
-import tqdm
+import serial
+from deepdiff import DeepDiff
 
 from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from lerobot.common.utils.utils import capture_timestamp_utc
+
+NameOrID: TypeAlias = str | int
+Value: TypeAlias = int | float
 
 MAX_ID_RANGE = 252
 
-
-def get_group_sync_key(data_name: str, motor_names: list[str]) -> str:
-    group_key = f"{data_name}_" + "_".join(motor_names)
-    return group_key
-
-
-def get_log_name(var_name: str, fn_name: str, data_name: str, motor_names: list[str]) -> str:
-    group_key = get_group_sync_key(data_name, motor_names)
-    log_name = f"{var_name}_{fn_name}_{group_key}"
-    return log_name
+logger = logging.getLogger(__name__)
 
 
 def assert_same_address(model_ctrl_table: dict[str, dict], motor_models: list[str], data_name: str) -> None:
@@ -98,7 +95,7 @@ class PortHandler(Protocol):
         self.tx_time_per_byte: float
         self.is_using: bool
         self.port_name: str
-        self.ser: object
+        self.ser: serial.Serial
 
     def openPort(self): ...
     def closePort(self): ...
@@ -153,6 +150,52 @@ class PacketHandler(Protocol):
     def syncWriteTxOnly(self, port, start_address, data_length, param, param_length): ...
 
 
+class GroupSyncRead(Protocol):
+    def __init__(self, port, ph, start_address, data_length):
+        self.port: str
+        self.ph: PortHandler
+        self.start_address: int
+        self.data_length: int
+        self.last_result: bool
+        self.is_param_changed: bool
+        self.param: list
+        self.data_dict: dict
+
+    def makeParam(self): ...
+    def addParam(self, id): ...
+    def removeParam(self, id): ...
+    def clearParam(self): ...
+    def txPacket(self): ...
+    def rxPacket(self): ...
+    def txRxPacket(self): ...
+    def isAvailable(self, id, address, data_length): ...
+    def getData(self, id, address, data_length): ...
+
+
+class GroupSyncWrite(Protocol):
+    def __init__(self, port, ph, start_address, data_length):
+        self.port: str
+        self.ph: PortHandler
+        self.start_address: int
+        self.data_length: int
+        self.is_param_changed: bool
+        self.param: list
+        self.data_dict: dict
+
+    def makeParam(self): ...
+    def addParam(self, id, data): ...
+    def removeParam(self, id): ...
+    def changeParam(self, id, data): ...
+    def clearParam(self): ...
+    def txPacket(self): ...
+
+
+@dataclass
+class Motor:
+    id: int
+    model: str
+
+
 class MotorsBus(abc.ABC):
     """The main LeRobot class for implementing motors buses.
 
@@ -163,7 +206,7 @@ class MotorsBus(abc.ABC):
     Note: This class may evolve in the future should we add support for other manufacturers SDKs.
 
     A MotorsBus allows to efficiently read and write to the attached motors.
-    It represents a several motors daisy-chained together and connected through a serial port.
+    It represents several motors daisy-chained together and connected through a serial port.
 
     A MotorsBus subclass instance requires a port (e.g. `FeetechMotorsBus(port="/dev/tty.usbmodem575E0031751"`)).
     To find the port, you can run our utility script:
@@ -198,199 +241,337 @@ class MotorsBus(abc.ABC):
     model_ctrl_table: dict[str, dict]
     model_resolution_table: dict[str, int]
     model_baudrate_table: dict[str, dict]
+    calibration_required: list[str]
+    default_timeout: int
 
     def __init__(
         self,
         port: str,
-        motors: dict[str, tuple[int, str]],
+        motors: dict[str, Motor],
     ):
         self.port = port
         self.motors = motors
-        self.port_handler: PortHandler | None = None
-        self.packet_handler: PacketHandler | None = None
+        self._validate_motors()
 
-        self.group_readers = {}
-        self.group_writers = {}
-        self.logs = {}
+        self.port_handler: PortHandler
+        self.packet_handler: PacketHandler
+        self.reader: GroupSyncRead
+        self.writer: GroupSyncWrite
 
         self.calibration = None
-        self.is_connected: bool = False
+
+        self._id_to_model = {m.id: m.model for m in self.motors.values()}
+        self._id_to_name = {m.id: name for name, m in self.motors.items()}
 
     def __len__(self):
         return len(self.motors)
 
-    @property
-    def motor_names(self) -> list[str]:
+    def __repr__(self):
+        return (
+            f"{self.__class__.__name__}(\n"
+            f"    Port: '{self.port}',\n"
+            f"    Motors: \n{pformat(self.motors, indent=8)},\n"
+            ")',\n"
+        )
+
+    @cached_property
+    def _has_different_ctrl_tables(self) -> bool:
+        if len(self.models) < 2:
+            return False
+
+        first_table = self.model_ctrl_table[self.models[0]]
+        return any(DeepDiff(first_table, self.model_ctrl_table[model]) for model in self.models[1:])
+
+    def id_to_model(self, motor_id: int) -> str:
+        return self._id_to_model[motor_id]
+
+    def id_to_name(self, motor_id: int) -> str:
+        return self._id_to_name[motor_id]
+
+    @cached_property
+    def names(self) -> list[str]:
         return list(self.motors)
 
-    @property
-    def motor_models(self) -> list[str]:
-        return [model for _, model in self.motors.values()]
+    @cached_property
+    def models(self) -> list[str]:
+        return [m.model for m in self.motors.values()]
+
+    @cached_property
+    def ids(self) -> list[int]:
+        return [m.id for m in self.motors.values()]
+
+    def _validate_motors(self) -> None:
+        # TODO(aliberts): improve error messages for this (display problematics values)
+        if len(self.ids) != len(set(self.ids)):
+            raise ValueError("Some motors have the same id.")
+
+        if len(self.names) != len(set(self.names)):
+            raise ValueError("Some motors have the same name.")
+
+        if any(model not in self.model_resolution_table for model in self.models):
+            raise ValueError("Some motors models are not available.")
 
     @property
-    def motor_indices(self) -> list[int]:
-        return [idx for idx, _ in self.motors.values()]
+    def is_connected(self) -> bool:
+        return self.port_handler.is_open
 
-    def connect(self):
+    def connect(self) -> None:
         if self.is_connected:
             raise DeviceAlreadyConnectedError(
-                f"{self.__name__}({self.port}) is already connected. Do not call `{self.__name__}.connect()` twice."
+                f"{self.__class__.__name__}('{self.port}') is already connected. Do not call `{self.__class__.__name__}.connect()` twice."
             )
 
-        self._set_handlers()
-
         try:
             if not self.port_handler.openPort():
                 raise OSError(f"Failed to open port '{self.port}'.")
-        except Exception:
-            traceback.print_exc()
-            print(
-                "\nTry running `python lerobot/scripts/find_motors_bus_port.py` to make sure you are using the correct port.\n"
+        except (FileNotFoundError, OSError, serial.SerialException) as e:
+            logger.error(
+                f"\nCould not connect on port '{self.port}'. Make sure you are using the correct port."
+                "\nTry running `python lerobot/scripts/find_motors_bus_port.py`\n"
             )
-            raise
+            raise e
 
-        self._set_timeout()
+        self.set_timeout()
+        logger.debug(f"{self.__class__.__name__} connected.")
 
-        # Allow to read and write
-        self.is_connected = True
+    def set_timeout(self, timeout_ms: int | None = None):
+        timeout_ms = timeout_ms if timeout_ms is not None else self.default_timeout
+        self.port_handler.setPacketTimeoutMillis(timeout_ms)
 
-    @abc.abstractmethod
-    def _set_handlers(self):
-        pass
-
-    @abc.abstractmethod
-    def _set_timeout(self, timeout: int):
-        pass
-
-    def are_motors_configured(self):
+    @property
+    def are_motors_configured(self) -> bool:
         """
         Only check the motor indices and not baudrate, since if the motor baudrates are incorrect, a
         ConnectionError will be raised anyway.
         """
         try:
-            return (self.motor_indices == self.read("ID")).all()
+            # TODO(aliberts): use ping instead
+            return (self.ids == self.read("ID")).all()
         except ConnectionError as e:
-            print(e)
+            logger.error(e)
             return False
 
-    def find_motor_indices(self, possible_ids: list[str] = None, num_retry: int = 2):
-        if possible_ids is None:
-            possible_ids = range(MAX_ID_RANGE)
+    def ping(self, motor: NameOrID, num_retry: int = 0, raise_on_error: bool = False) -> int | 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
+            logger.debug(f"ping failed for {idx=}: {n_try=} got {comm=} {error=}")
 
-        indices = []
-        for idx in tqdm.tqdm(possible_ids):
-            try:
-                present_idx = self.read_with_motor_ids(self.motor_models, [idx], "ID", num_retry=num_retry)[0]
-            except ConnectionError:
-                continue
+        if raise_on_error:
+            raise ConnectionError(f"Ping motor {motor} returned a {error} error code.")
 
-            if idx != present_idx:
-                # sanity check
-                raise OSError(
-                    "Motor index used to communicate through the bus is not the same as the one present in the motor "
-                    "memory. The motor memory might be damaged."
-                )
-            indices.append(idx)
+    @abc.abstractmethod
+    def broadcast_ping(
+        self, num_retry: int = 0, raise_on_error: bool = False
+    ) -> dict[int, list[int, int]] | None: ...
 
-        return indices
-
-    def set_baudrate(self, baudrate):
+    def set_baudrate(self, baudrate) -> None:
         present_bus_baudrate = self.port_handler.getBaudRate()
         if present_bus_baudrate != baudrate:
-            print(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.")
+            logger.info(f"Setting bus baud rate to {baudrate}. Previously {present_bus_baudrate}.")
             self.port_handler.setBaudRate(baudrate)
 
             if self.port_handler.getBaudRate() != baudrate:
                 raise OSError("Failed to write bus baud rate.")
 
-    def set_calibration(self, calibration_dict: dict[str, list]):
-        self.calibration = calibration_dict
+    def set_calibration(self, calibration_fpath: Path) -> None:
+        with open(calibration_fpath) as f:
+            calibration = json.load(f)
+
+        self.calibration = {int(idx): val for idx, val in calibration.items()}
 
     @abc.abstractmethod
-    def apply_calibration(self):
+    def calibrate_values(self, ids_values: dict[int, int]) -> dict[int, float]:
         pass
 
     @abc.abstractmethod
-    def revert_calibration(self):
+    def uncalibrate_values(self, ids_values: dict[int, float]) -> dict[int, int]:
         pass
 
-    def read(self, data_name, motor_names: str | list[str] | None = None):
+    @abc.abstractmethod
+    def _is_comm_success(self, comm: int) -> bool:
+        pass
+
+    @staticmethod
+    @abc.abstractmethod
+    def split_int_bytes(value: int, n_bytes: int) -> list[int]:
+        """
+        Splits an unsigned integer into a list of bytes in little-endian order.
+
+        This function extracts the individual bytes of an integer based on the
+        specified number of bytes (`n_bytes`). The output is a list of integers,
+        each representing a byte (0-255).
+
+        **Byte order:** The function returns bytes in **little-endian format**,
+        meaning the least significant byte (LSB) comes first.
+
+        Args:
+            value (int): The unsigned integer to be converted into a byte list. Must be within
+                the valid range for the specified `n_bytes`.
+            n_bytes (int): The number of bytes to use for conversion. Supported values:
+                - 1 (for values 0 to 255)
+                - 2 (for values 0 to 65,535)
+                - 4 (for values 0 to 4,294,967,295)
+
+        Raises:
+            ValueError: If `value` is negative or exceeds the maximum allowed for `n_bytes`.
+            NotImplementedError: If `n_bytes` is not 1, 2, or 4.
+
+        Returns:
+            list[int]: A list of integers, each representing a byte in **little-endian order**.
+
+        Examples:
+            >>> split_int_bytes(0x12, 1)
+            [18]
+            >>> split_int_bytes(0x1234, 2)
+            [52, 18]  # 0x1234 → 0x34 0x12 (little-endian)
+            >>> split_int_bytes(0x12345678, 4)
+            [120, 86, 52, 18]  # 0x12345678 → 0x78 0x56 0x34 0x12
+        """
+        pass
+
+    def get_motor_id(self, motor: NameOrID) -> int:
+        if isinstance(motor, str):
+            return self.motors[motor].id
+        elif isinstance(motor, int):
+            return motor
+        else:
+            raise TypeError(f"'{motor}' should be int, str.")
+
+    @overload
+    def read(self, data_name: str, motors: None = ..., num_retry: int = ...) -> dict[str, Value]: ...
+    @overload
+    def read(
+        self, data_name: str, motors: NameOrID | list[NameOrID], num_retry: int = ...
+    ) -> dict[NameOrID, Value]: ...
+    def read(
+        self, data_name: str, motors: NameOrID | list[NameOrID] | None = None, num_retry: int = 0
+    ) -> dict[NameOrID, Value]:
         if not self.is_connected:
             raise DeviceNotConnectedError(
-                f"{self.__name__}({self.port}) is not connected. You need to run `{self.__name__}.connect()`."
+                f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
             )
 
-        start_time = time.perf_counter()
-        if motor_names is None:
-            motor_names = self.motor_names
+        id_key_map: dict[int, NameOrID] = {}
+        if motors is None:
+            id_key_map = {m.id: name for name, m in self.motors.items()}
+        elif isinstance(motors, (str, int)):
+            id_key_map = {self.get_motor_id(motors): motors}
+        elif isinstance(motors, list):
+            id_key_map = {self.get_motor_id(m): m for m in motors}
+        else:
+            raise TypeError(motors)
 
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
+        motor_ids = list(id_key_map)
+        if self._has_different_ctrl_tables:
+            models = [self.id_to_model(idx) for idx in motor_ids]
+            assert_same_address(self.model_ctrl_table, models, data_name)
 
-        values = self._read(data_name, motor_names)
+        model = self.id_to_model(next(iter(motor_ids)))
+        addr, n_bytes = self.model_ctrl_table[model][data_name]
 
-        # log the number of seconds it took to read the data from the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "read", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
+        comm, ids_values = self._read(motor_ids, addr, n_bytes, num_retry)
+        if not self._is_comm_success(comm):
+            raise ConnectionError(
+                f"Failed to read {data_name} on port {self.port} for ids {motor_ids}:"
+                f"{self.packet_handler.getTxRxResult(comm)}"
+            )
 
-        # log the utc time at which the data was received
-        ts_utc_name = get_log_name("timestamp_utc", "read", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
+        if data_name in self.calibration_required and self.calibration is not None:
+            ids_values = self.calibrate_values(ids_values)
 
-        return values
+        return {id_key_map[idx]: val for idx, val in ids_values.items()}
 
-    @abc.abstractmethod
-    def _read(self, data_name: str, motor_names: list[str]):
-        pass
+    def _read(
+        self, motor_ids: list[str], address: int, n_bytes: int, num_retry: int = 0
+    ) -> tuple[int, dict[int, int]]:
+        self.reader.clearParam()
+        self.reader.start_address = address
+        self.reader.data_length = n_bytes
 
-    def write(
-        self, data_name: str, values: int | float | np.ndarray, motor_names: str | list[str] | None = None
-    ) -> None:
+        # FIXME(aliberts, pkooij): We should probably not have to do this.
+        # Let's try to see if we can do with better comm status handling instead.
+        # self.port_handler.ser.reset_output_buffer()
+        # self.port_handler.ser.reset_input_buffer()
+
+        for idx in motor_ids:
+            self.reader.addParam(idx)
+
+        for n_try in range(1 + num_retry):
+            comm = self.reader.txRxPacket()
+            if self._is_comm_success(comm):
+                break
+            logger.debug(f"ids={list(motor_ids)} @{address} ({n_bytes} bytes) {n_try=} got {comm=}")
+
+        values = {idx: self.reader.getData(idx, address, n_bytes) for idx in motor_ids}
+        return comm, values
+
+    # TODO(aliberts, pkooij): Implementing something like this could get much faster read times.
+    # Note: this could be at the cost of increase latency between the moment the data is produced by the
+    # motors and the moment it is used by a policy
+    # def _async_read(self, motor_ids: list[str], address: int, n_bytes: int):
+    #     self.reader.rxPacket()
+    #     self.reader.txPacket()
+    #     for idx in motor_ids:
+    #         value = self.reader.getData(idx, address, n_bytes)
+
+    def write(self, data_name: str, values: Value | dict[NameOrID, Value], num_retry: int = 0) -> None:
         if not self.is_connected:
             raise DeviceNotConnectedError(
-                f"{self.__name__}({self.port}) is not connected. You need to run `{self.__name__}.connect()`."
+                f"{self.__class__.__name__}('{self.port}') is not connected. You need to run `{self.__class__.__name__}.connect()`."
             )
 
-        start_time = time.perf_counter()
+        if isinstance(values, int):
+            ids_values = {id_: values for id_ in self.ids}
+        elif isinstance(values, dict):
+            ids_values = {self.get_motor_id(motor): val for motor, val in values.items()}
+        else:
+            raise ValueError(f"'values' is expected to be a single value or a dict. Got {values}")
 
-        if motor_names is None:
-            motor_names = self.motor_names
+        if self._has_different_ctrl_tables:
+            models = [self.id_to_model(idx) for idx in ids_values]
+            assert_same_address(self.model_ctrl_table, models, data_name)
 
-        if isinstance(motor_names, str):
-            motor_names = [motor_names]
+        if data_name in self.calibration_required and self.calibration is not None:
+            ids_values = self.uncalibrate_values(ids_values)
 
-        if isinstance(values, (int, float, np.integer)):
-            values = [int(values)] * len(motor_names)
+        model = self.id_to_model(next(iter(ids_values)))
+        addr, n_bytes = self.model_ctrl_table[model][data_name]
 
-        self._write(data_name, values, motor_names)
+        comm = self._write(ids_values, addr, n_bytes, num_retry)
+        if not self._is_comm_success(comm):
+            raise ConnectionError(
+                f"Failed to write {data_name} on port {self.port} for ids {list(ids_values)}:"
+                f"{self.packet_handler.getTxRxResult(comm)}"
+            )
 
-        # log the number of seconds it took to write the data to the motors
-        delta_ts_name = get_log_name("delta_timestamp_s", "write", data_name, motor_names)
-        self.logs[delta_ts_name] = time.perf_counter() - start_time
+    def _write(self, ids_values: dict[int, int], address: int, n_bytes: int, num_retry: int = 0) -> int:
+        self.writer.clearParam()
+        self.writer.start_address = address
+        self.writer.data_length = n_bytes
 
-        # TODO(rcadene): should we log the time before sending the write command?
-        # log the utc time when the write has been completed
-        ts_utc_name = get_log_name("timestamp_utc", "write", data_name, motor_names)
-        self.logs[ts_utc_name] = capture_timestamp_utc()
+        for idx, value in ids_values.items():
+            data = self.split_int_bytes(value, n_bytes)
+            self.writer.addParam(idx, data)
 
-    @abc.abstractmethod
-    def _write(self, data_name: str, values: list[int], motor_names: list[str]) -> None:
-        pass
+        for n_try in range(1 + num_retry):
+            comm = self.writer.txPacket()
+            if self._is_comm_success(comm):
+                break
+            logger.debug(f"ids={list(ids_values)} @{address} ({n_bytes} bytes) {n_try=} got {comm=}")
+
+        return comm
 
     def disconnect(self) -> None:
         if not self.is_connected:
             raise DeviceNotConnectedError(
-                f"{self.__name__}({self.port}) is not connected. Try running `{self.__name__}.connect()` first."
+                f"{self.__class__.__name__}('{self.port}') is not connected. Try running `{self.__class__.__name__}.connect()` first."
             )
 
-        if self.port_handler is not None:
-            self.port_handler.closePort()
-            self.port_handler = None
-
-        self.packet_handler = None
-        self.group_readers = {}
-        self.group_writers = {}
-        self.is_connected = False
+        self.port_handler.closePort()
+        logger.debug(f"{self.__class__.__name__} disconnected.")
 
     def __del__(self):
         if self.is_connected:

lerobot/common/policies/act/modeling_act.py

@@ -119,9 +119,7 @@ class ACTPolicy(PreTrainedPolicy):
         batch = self.normalize_inputs(batch)
         if self.config.image_features:
             batch = dict(batch)  # shallow copy so that adding a key doesn't modify the original
-            batch["observation.images"] = torch.stack(
-                [batch[key] for key in self.config.image_features], dim=-4
-            )
+            batch["observation.images"] = [batch[key] for key in self.config.image_features]
 
         # If we are doing temporal ensembling, do online updates where we keep track of the number of actions
         # we are ensembling over.
@@ -149,9 +147,8 @@ class ACTPolicy(PreTrainedPolicy):
         batch = self.normalize_inputs(batch)
         if self.config.image_features:
             batch = dict(batch)  # shallow copy so that adding a key doesn't modify the original
-            batch["observation.images"] = torch.stack(
-                [batch[key] for key in self.config.image_features], dim=-4
-            )
+            batch["observation.images"] = [batch[key] for key in self.config.image_features]
+
         batch = self.normalize_targets(batch)
         actions_hat, (mu_hat, log_sigma_x2_hat) = self.model(batch)
 
@@ -413,11 +410,10 @@ class ACT(nn.Module):
                 "actions must be provided when using the variational objective in training mode."
             )
 
-        batch_size = (
-            batch["observation.images"]
-            if "observation.images" in batch
-            else batch["observation.environment_state"]
-        ).shape[0]
+        if "observation.images" in batch:
+            batch_size = batch["observation.images"][0].shape[0]
+        else:
+            batch_size = batch["observation.environment_state"].shape[0]
 
         # Prepare the latent for input to the transformer encoder.
         if self.config.use_vae and "action" in batch:
@@ -490,20 +486,21 @@ class ACT(nn.Module):
             all_cam_features = []
             all_cam_pos_embeds = []
 
-            for cam_index in range(batch["observation.images"].shape[-4]):
-                cam_features = self.backbone(batch["observation.images"][:, cam_index])["feature_map"]
-                # TODO(rcadene, alexander-soare): remove call to `.to` to speedup forward ; precompute and use
-                # buffer
+            # For a list of images, the H and W may vary but H*W is constant.
+            for img in batch["observation.images"]:
+                cam_features = self.backbone(img)["feature_map"]
                 cam_pos_embed = self.encoder_cam_feat_pos_embed(cam_features).to(dtype=cam_features.dtype)
-                cam_features = self.encoder_img_feat_input_proj(cam_features)  # (B, C, h, w)
+                cam_features = self.encoder_img_feat_input_proj(cam_features)
+
+                # Rearrange features to (sequence, batch, dim).
+                cam_features = einops.rearrange(cam_features, "b c h w -> (h w) b c")
+                cam_pos_embed = einops.rearrange(cam_pos_embed, "b c h w -> (h w) b c")
+
                 all_cam_features.append(cam_features)
                 all_cam_pos_embeds.append(cam_pos_embed)
-            # Concatenate camera observation feature maps and positional embeddings along the width dimension,
-            # and move to (sequence, batch, dim).
-            all_cam_features = torch.cat(all_cam_features, axis=-1)
-            encoder_in_tokens.extend(einops.rearrange(all_cam_features, "b c h w -> (h w) b c"))
-            all_cam_pos_embeds = torch.cat(all_cam_pos_embeds, axis=-1)
-            encoder_in_pos_embed.extend(einops.rearrange(all_cam_pos_embeds, "b c h w -> (h w) b c"))
+
+            encoder_in_tokens.extend(torch.cat(all_cam_features, axis=0))
+            encoder_in_pos_embed.extend(torch.cat(all_cam_pos_embeds, axis=0))
 
         # Stack all tokens along the sequence dimension.
         encoder_in_tokens = torch.stack(encoder_in_tokens, axis=0)

lerobot/common/robots/koch/robot_koch.py

@@ -27,7 +27,6 @@ from lerobot.common.motors import TorqueMode
 from lerobot.common.motors.dynamixel import (
     DynamixelMotorsBus,
     run_arm_calibration,
-    set_operating_mode,
 )
 
 from ..robot import Robot
@@ -103,7 +102,22 @@ class KochRobot(Robot):
         self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
         self.calibrate()
 
-        set_operating_mode(self.arm)
+        # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
+        # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
+        # you could end up with a servo with a position 0 or 4095 at a crucial point See [
+        # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
+        all_motors_except_gripper = [name for name in self.arm.motor_names if name != "gripper"]
+        if len(all_motors_except_gripper) > 0:
+            # 4 corresponds to Extended Position on Koch motors
+            self.arm.write("Operating_Mode", 4, all_motors_except_gripper)
+
+        # Use 'position control current based' for gripper to be limited by the limit of the current.
+        # For the follower gripper, it means it can grasp an object without forcing too much even tho,
+        # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
+        # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
+        # to make it move, and it will move back to its original target position when we release the force.
+        # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
+        self.arm.write("Operating_Mode", 5, "gripper")
 
         # Set better PID values to close the gap between recorded states and actions
         # TODO(rcadene): Implement an automatic procedure to set optimal PID values for each motor

lerobot/common/robots/manipulator.py

@@ -580,7 +580,7 @@ class ManipulatorRobot:
             # Used when record_data=True
             follower_goal_pos[name] = goal_pos
 
-            goal_pos = goal_pos.numpy().astype(np.int32)
+            goal_pos = goal_pos.numpy().astype(np.float32)
             self.follower_arms[name].write("Goal_Position", goal_pos)
             self.logs[f"write_follower_{name}_goal_pos_dt_s"] = time.perf_counter() - before_fwrite_t
 
@@ -702,7 +702,7 @@ class ManipulatorRobot:
             action_sent.append(goal_pos)
 
             # Send goal position to each follower
-            goal_pos = goal_pos.numpy().astype(np.int32)
+            goal_pos = goal_pos.numpy().astype(np.float32)
             self.follower_arms[name].write("Goal_Position", goal_pos)
 
         return torch.cat(action_sent)

lerobot/common/teleoperators/keyboard/teleop_keyboard.py

@@ -18,7 +18,7 @@ import logging
 import os
 import sys
 import time
-from threading import Lock
+from queue import Queue
 
 import numpy as np
 
@@ -29,7 +29,6 @@ from .configuration_keyboard import KeyboardTeleopConfig
 
 PYNPUT_AVAILABLE = True
 try:
-    # Only import if there's a valid X server or if we're not on a Pi
     if ("DISPLAY" not in os.environ) and ("linux" in sys.platform):
         logging.info("No DISPLAY set. Skipping pynput import.")
         raise ImportError("pynput blocked intentionally due to no display.")
@@ -57,15 +56,14 @@ class KeyboardTeleop(Teleoperator):
         self.config = config
         self.robot_type = config.type
 
-        self.pressed_keys_lock = Lock()
-        self.pressed_keys = {}
+        self.event_queue = Queue()
+        self.current_pressed = {}
         self.listener = None
         self.is_connected = False
         self.logs = {}
 
     @property
     def action_feature(self) -> dict:
-        # TODO(Steven): Set this from config? (when deciding the keys we want to capture)
         return {
             "dtype": "float32",
             "shape": (len(self.arm),),
@@ -100,17 +98,20 @@ class KeyboardTeleop(Teleoperator):
 
     def on_press(self, key):
         if hasattr(key, "char"):
-            with self.pressed_keys_lock:
-                self.pressed_keys[key.char] = True
+            self.event_queue.put((key.char, True))
 
     def on_release(self, key):
         if hasattr(key, "char"):
-            with self.pressed_keys_lock:
-                self.pressed_keys[key.char] = False
+            self.event_queue.put((key.char, False))
         if key == keyboard.Key.esc:
             logging.info("ESC pressed, disconnecting.")
             self.disconnect()
 
+    def _drain_pressed_keys(self):
+        while not self.event_queue.empty():
+            key_char, is_pressed = self.event_queue.get_nowait()
+            self.current_pressed[key_char] = is_pressed
+
     def get_action(self) -> np.ndarray:
         before_read_t = time.perf_counter()
 
@@ -119,10 +120,10 @@ class KeyboardTeleop(Teleoperator):
                 "KeyboardTeleop is not connected. You need to run `connect()` before `get_action()`."
             )
 
-        with self.pressed_keys_lock:
-            pressed_keys_snapshot = self.pressed_keys
+        self._drain_pressed_keys()
 
-        action = {key for key, val in pressed_keys_snapshot.items() if val}
+        # Generate action based on current key states
+        action = {key for key, val in self.current_pressed.items() if val}
         self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t
 
         return np.array(list(action))

lerobot/common/teleoperators/koch/configuration_koch.py

@@ -28,13 +28,3 @@ class KochTeleopConfig(TeleoperatorConfig):
     # Sets the arm in torque mode with the gripper motor set to this angle. This makes it possible
     # to squeeze the gripper and have it spring back to an open position on its own.
     gripper_open_degree: float = 35.156
-
-    mock: bool = False
-
-    # motors
-    shoulder_pan: tuple = (1, "xl330-m077")
-    shoulder_lift: tuple = (2, "xl330-m077")
-    elbow_flex: tuple = (3, "xl330-m077")
-    wrist_flex: tuple = (4, "xl330-m077")
-    wrist_roll: tuple = (5, "xl330-m077")
-    gripper: tuple = (6, "xl330-m077")

lerobot/common/teleoperators/koch/teleop_koch.py

@@ -25,7 +25,6 @@ from lerobot.common.motors import TorqueMode
 from lerobot.common.motors.dynamixel import (
     DynamixelMotorsBus,
     run_arm_calibration,
-    set_operating_mode,
 )
 
 from ..teleoperator import Teleoperator
@@ -50,12 +49,12 @@ class KochTeleop(Teleoperator):
         self.arm = DynamixelMotorsBus(
             port=self.config.port,
             motors={
-                "shoulder_pan": config.shoulder_pan,
-                "shoulder_lift": config.shoulder_lift,
-                "elbow_flex": config.elbow_flex,
-                "wrist_flex": config.wrist_flex,
-                "wrist_roll": config.wrist_roll,
-                "gripper": config.gripper,
+                "shoulder_pan": (1, "xl330-m077"),
+                "shoulder_lift": (2, "xl330-m077"),
+                "elbow_flex": (3, "xl330-m077"),
+                "wrist_flex": (4, "xl330-m077"),
+                "wrist_roll": (5, "xl330-m077"),
+                "gripper": (6, "xl330-m077"),
             },
         )
 
@@ -88,7 +87,22 @@ class KochTeleop(Teleoperator):
         self.arm.write("Torque_Enable", TorqueMode.DISABLED.value)
         self.calibrate()
 
-        set_operating_mode(self.arm)
+        # Use 'extended position mode' for all motors except gripper, because in joint mode the servos can't
+        # rotate more than 360 degrees (from 0 to 4095) And some mistake can happen while assembling the arm,
+        # you could end up with a servo with a position 0 or 4095 at a crucial point See [
+        # https://emanual.robotis.com/docs/en/dxl/x/x_series/#operating-mode11]
+        all_motors_except_gripper = [name for name in self.arm.motor_names if name != "gripper"]
+        if len(all_motors_except_gripper) > 0:
+            # 4 corresponds to Extended Position on Koch motors
+            self.arm.write("Operating_Mode", 4, all_motors_except_gripper)
+
+        # Use 'position control current based' for gripper to be limited by the limit of the current.
+        # For the follower gripper, it means it can grasp an object without forcing too much even tho,
+        # it's goal position is a complete grasp (both gripper fingers are ordered to join and reach a touch).
+        # For the leader gripper, it means we can use it as a physical trigger, since we can force with our finger
+        # to make it move, and it will move back to its original target position when we release the force.
+        # 5 corresponds to Current Controlled Position on Koch gripper motors "xl330-m077, xl330-m288"
+        self.arm.write("Operating_Mode", 5, "gripper")
 
         # Enable torque on the gripper and move it to 45 degrees so that we can use it as a trigger.
         logging.info("Activating torque.")

lerobot/configs/default.py

@@ -20,6 +20,7 @@ from lerobot.common import (
     policies,  # noqa: F401
 )
 from lerobot.common.datasets.transforms import ImageTransformsConfig
+from lerobot.common.datasets.video_utils import get_safe_default_codec
 
 
 @dataclass
@@ -35,7 +36,7 @@ class DatasetConfig:
     image_transforms: ImageTransformsConfig = field(default_factory=ImageTransformsConfig)
     revision: str | None = None
     use_imagenet_stats: bool = True
-    video_backend: str = "pyav"
+    video_backend: str = field(default_factory=get_safe_default_codec)
 
 
 @dataclass

lerobot/scripts/calibration_visualization_so100.py

@@ -1,100 +0,0 @@
-"""
-usage:
-
-```python
-python lerobot/scripts/calibration_visualization_so100.py \
-    --teleop.type=so100 \
-    --teleop.port=/dev/tty.usbmodem58760430541
-
-python lerobot/scripts/calibration_visualization_so100.py \
-    --robot.type=so100 \
-    --robot.port=/dev/tty.usbmodem585A0084711
-```
-"""
-
-import time
-from dataclasses import dataclass
-
-import draccus
-
-from lerobot.common.motors.feetech.feetech import (
-    adjusted_to_homing_ticks,
-    adjusted_to_motor_ticks,
-    convert_degrees_to_ticks,
-    convert_ticks_to_degrees,
-)
-from lerobot.common.robots import RobotConfig
-from lerobot.common.robots.so100 import SO100Robot, SO100RobotConfig  # noqa: F401
-from lerobot.common.teleoperators import TeleoperatorConfig
-from lerobot.common.teleoperators.so100 import SO100Teleop, SO100TeleopConfig  # noqa: F401
-
-
-@dataclass
-class DebugFeetechConfig:
-    teleop: TeleoperatorConfig | None = None
-    robot: RobotConfig | None = None
-
-    def __post_init__(self):
-        if bool(self.teleop) == bool(self.robot):
-            raise ValueError("Select a single device.")
-
-
-@draccus.wrap()
-def debug_feetech_positions(cfg: DebugFeetechConfig):
-    """
-    Reads each joint's (1) raw ticks, (2) homed ticks, (3) degrees, and (4) invert-adjusted ticks.
-    """
-    device = SO100Teleop(cfg.teleop) if cfg.teleop else SO100Robot(cfg.robot)
-    device.connect()
-
-    # Disable torque on all motors so you can move them freely by hand
-    device.arm.write("Torque_Enable", 0, motor_names=device.arm.motor_names)
-    print("Torque disabled on all joints.")
-
-    try:
-        print("\nPress Ctrl+C to quit.\n")
-        while True:
-            # Read *raw* positions (no calibration).
-            raw_positions = device.arm.read_with_motor_ids(
-                device.arm.motor_models, device.arm.motor_indices, data_name="Present_Position"
-            )
-
-            # Read *already-homed* positions
-            homed_positions = device.arm.read("Present_Position")
-
-            for i, name in enumerate(device.arm.motor_names):
-                motor_idx, model = device.arm.motors[name]
-
-                raw_ticks = raw_positions[i]  # 0..4095
-                homed_val = homed_positions[i]  # degrees or % if linear
-
-                # Manually compute "adjusted ticks" from raw ticks
-                manual_adjusted = adjusted_to_homing_ticks(raw_ticks, model, device.arm, motor_idx)
-                # Convert to degrees
-                manual_degs = convert_ticks_to_degrees(manual_adjusted, model)
-
-                # Convert that deg back to ticks
-                manual_ticks = convert_degrees_to_ticks(manual_degs, model)
-                # Then invert them using offset & bus drive mode
-                inv_ticks = adjusted_to_motor_ticks(manual_ticks, model, device.arm, motor_idx)
-
-                print(
-                    f"{name:15s} | "
-                    f"RAW={raw_ticks:4d} | "
-                    f"HOMED_FROM_READ={homed_val:7.2f} | "
-                    f"HOMED_TICKS={manual_adjusted:6d} | "
-                    f"MANUAL_ADJ_DEG={manual_degs:7.2f} | "
-                    f"MANUAL_ADJ_TICKS={manual_ticks:6d} | "
-                    f"INV_TICKS={inv_ticks:4d} "
-                )
-            print("----------------------------------------------------")
-            time.sleep(0.25)
-    except KeyboardInterrupt:
-        pass
-    finally:
-        print("\nExiting. Disconnecting bus...")
-        device.disconnect()
-
-
-if __name__ == "__main__":
-    debug_feetech_positions()

pyproject.toml

@@ -53,7 +53,7 @@ dependencies = [
     "einops>=0.8.0",
     "flask>=3.0.3",
     "gdown>=5.1.0",
-    "gymnasium==0.29.1",                                                 # TODO(rcadene, aliberts): Make gym 1.0.0 work
+    "gymnasium==0.29.1", # TODO(rcadene, aliberts): Make gym 1.0.0 work
     "h5py>=3.10.0",
     "huggingface-hub[hf-transfer,cli]>=0.27.1 ; python_version < '4.0'",
     "imageio[ffmpeg]>=2.34.0",
@@ -69,7 +69,7 @@ dependencies = [
     "rerun-sdk>=0.21.0",
     "termcolor>=2.4.0",
     "torch>=2.2.1",
-    "torchcodec>=0.2.1",
+    "torchcodec>=0.2.1; sys_platform != 'win32' and (sys_platform != 'linux' or (platform_machine != 'aarch64' and platform_machine != 'arm64' and platform_machine != 'armv7l'))",
     "torchvision>=0.21.0",
     "wandb>=0.16.3",
     "zarr>=2.17.0",
@@ -92,7 +92,7 @@ stretch = [
     "pyrealsense2>=2.55.1.6486 ; sys_platform != 'darwin'",
     "pynput>=1.7.7",
 ]
-test = ["pytest>=8.1.0", "pytest-cov>=5.0.0", "pyserial>=3.5"]
+test = ["pytest>=8.1.0", "pytest-cov>=5.0.0", "pyserial>=3.5", "mock-serial>=0.0.1 ; sys_platform != 'win32'"]
 umi = ["imagecodecs>=2024.1.1"]
 video_benchmark = ["scikit-image>=0.23.2", "pandas>=2.2.2"]
 xarm = ["gym-xarm>=0.1.1 ; python_version < '4.0'"]
@@ -108,6 +108,9 @@ exclude = ["tests/artifacts/**/*.safetensors"]
 [tool.ruff.lint]
 select = ["E4", "E7", "E9", "F", "I", "N", "B", "C4", "SIM"]
 
+[tool.ruff.lint.per-file-ignores]
+"__init__.py" = ["F401", "F403"]
+
 [tool.bandit]
 exclude_dirs = [
     "tests",

tests/mocks/mock_dynamixel.py

@@ -0,0 +1,515 @@
+import abc
+import random
+import threading
+import time
+from typing import Callable
+
+import dynamixel_sdk as dxl
+import serial
+from mock_serial.mock_serial import MockSerial, Stub
+
+from lerobot.common.motors.dynamixel import X_SERIES_CONTROL_TABLE, DynamixelMotorsBus
+
+# https://emanual.robotis.com/docs/en/dxl/crc/
+DXL_CRC_TABLE = [
+    0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011,
+    0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022,
+    0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072,
+    0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041,
+    0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2,
+    0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1,
+    0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1,
+    0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082,
+    0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192,
+    0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1,
+    0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1,
+    0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2,
+    0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151,
+    0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162,
+    0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132,
+    0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101,
+    0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312,
+    0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321,
+    0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371,
+    0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342,
+    0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1,
+    0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2,
+    0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2,
+    0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381,
+    0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291,
+    0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2,
+    0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2,
+    0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1,
+    0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252,
+    0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261,
+    0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231,
+    0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202
+]  # fmt: skip
+
+# https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction
+INSTRUCTION_TYPES = {
+    "Ping": 0x01,	               # Checks whether the Packet has arrived at a device with the same ID as the specified packet ID
+    "Read": 0x02,	               # Read data from the Device
+    "Write": 0x03,	               # Write data to the Device
+    "Reg_Write": 0x04,	           # Register the Instruction Packet in standby status; Packet can later be executed using the Action command
+    "Action": 0x05,	               # Executes a Packet that was registered beforehand using Reg Write
+    "Factory_Reset": 0x06,	       # Resets the Control Table to its initial factory default settings
+    "Reboot": 0x08,	               # Reboot the Device
+    "Clear": 0x10,	               # Reset certain information stored in memory
+    "Control_Table_Backup": 0x20,  # Store current Control Table status data to a Backup or to restore backup EEPROM data.
+    "Status": 0x55,	               # Return packet sent following the execution of an Instruction Packet
+    "Sync_Read": 0x82,	           # Read data from multiple devices with the same Address with the same length at once
+    "Sync_Write": 0x83,	           # Write data to multiple devices with the same Address with the same length at once
+    "Fast_Sync_Read": 0x8A,	       # Read data from multiple devices with the same Address with the same length at once
+    "Bulk_Read": 0x92,	           # Read data from multiple devices with different Addresses with different lengths at once
+    "Bulk_Write": 0x93,	           # Write data to multiple devices with different Addresses with different lengths at once
+    "Fast_Bulk_Read": 0x9A,	       # Read data from multiple devices with different Addresses with different lengths at once
+}  # fmt: skip
+
+# https://emanual.robotis.com/docs/en/dxl/protocol2/#error
+ERROR_TYPE = {
+    "Success": 0x00, 	        # No error
+    "Result_Fail": 0x01, 	    # Failed to process the sent Instruction Packet
+    "Instruction_Error": 0x02,  # An undefined Instruction has been usedAction has been used without Reg Write
+    "CRC_Error": 0x03, 	        # The CRC of the sent Packet does not match the expected value
+    "Data_Range_Error": 0x04,   # Data to be written to the specified Address is outside the range of the minimum/maximum value
+    "Data_Length_Error": 0x05,  # Attempted to write Data that is shorter than the required data length of the specified Address
+                                #     (ex: when you attempt to only use 2 bytes of a register that has been defined as 4 bytes)
+    "Data_Limit_Error": 0x06,   # Data to be written to the specified Address is outside of the configured Limit value
+    "Access_Error": 0x07,	    # Attempted to write a value to an Address that is Read Only or has not been defined
+                                # Attempted to read a value from an Address that is Write Only or has not been defined
+                                # Attempted to write a value to an EEPROM register while Torque was Enabled.
+}  # fmt: skip
+
+
+class MockDynamixelPacketv2(abc.ABC):
+    @classmethod
+    def build(cls, dxl_id: int, params: list[int], length: list[int], *args, **kwargs) -> bytes:
+        packet = cls._build(dxl_id, params, length, *args, **kwargs)
+        packet = cls._add_stuffing(packet)
+        packet = cls._add_crc(packet)
+        return bytes(packet)
+
+    @abc.abstractclassmethod
+    def _build(cls, dxl_id: int, params: list[int], length: int, *args, **kwargs) -> list[int]:
+        pass
+
+    @staticmethod
+    def _add_stuffing(packet: list[int]) -> list[int]:
+        """
+        Byte stuffing is a method of adding additional data to generated instruction packets to ensure that
+        the packets are processed successfully. When the byte pattern "0xFF 0xFF 0xFD" appears in a packet,
+        byte stuffing adds 0xFD to the end of the pattern to convert it to “0xFF 0xFF 0xFD 0xFD” to ensure
+        that it is not interpreted as the header at the start of another packet.
+
+        Source: https://emanual.robotis.com/docs/en/dxl/protocol2/#transmission-process
+
+        Args:
+            packet (list[int]): The raw packet without stuffing.
+
+        Returns:
+            list[int]: The packet stuffed if it contained a "0xFF 0xFF 0xFD" byte sequence in its data bytes.
+        """
+        packet_length_in = dxl.DXL_MAKEWORD(packet[dxl.PKT_LENGTH_L], packet[dxl.PKT_LENGTH_H])
+        packet_length_out = packet_length_in
+
+        temp = [0] * dxl.TXPACKET_MAX_LEN
+
+        # FF FF FD XX ID LEN_L LEN_H
+        temp[dxl.PKT_HEADER0 : dxl.PKT_HEADER0 + dxl.PKT_LENGTH_H + 1] = packet[
+            dxl.PKT_HEADER0 : dxl.PKT_HEADER0 + dxl.PKT_LENGTH_H + 1
+        ]
+
+        index = dxl.PKT_INSTRUCTION
+
+        for i in range(0, packet_length_in - 2):  # except CRC
+            temp[index] = packet[i + dxl.PKT_INSTRUCTION]
+            index = index + 1
+            if (
+                packet[i + dxl.PKT_INSTRUCTION] == 0xFD
+                and packet[i + dxl.PKT_INSTRUCTION - 1] == 0xFF
+                and packet[i + dxl.PKT_INSTRUCTION - 2] == 0xFF
+            ):
+                # FF FF FD
+                temp[index] = 0xFD
+                index = index + 1
+                packet_length_out = packet_length_out + 1
+
+        temp[index] = packet[dxl.PKT_INSTRUCTION + packet_length_in - 2]
+        temp[index + 1] = packet[dxl.PKT_INSTRUCTION + packet_length_in - 1]
+        index = index + 2
+
+        if packet_length_in != packet_length_out:
+            packet = [0] * index
+
+        packet[0:index] = temp[0:index]
+
+        packet[dxl.PKT_LENGTH_L] = dxl.DXL_LOBYTE(packet_length_out)
+        packet[dxl.PKT_LENGTH_H] = dxl.DXL_HIBYTE(packet_length_out)
+
+        return packet
+
+    @staticmethod
+    def _add_crc(packet: list[int]) -> list[int]:
+        """Computes and add CRC to the packet.
+
+        https://emanual.robotis.com/docs/en/dxl/crc/
+        https://en.wikipedia.org/wiki/Cyclic_redundancy_check
+
+        Args:
+            packet (list[int]): The raw packet without CRC (but with placeholders for it).
+
+        Returns:
+            list[int]: The raw packet with a valid CRC.
+        """
+        crc = 0
+        for j in range(len(packet) - 2):
+            i = ((crc >> 8) ^ packet[j]) & 0xFF
+            crc = ((crc << 8) ^ DXL_CRC_TABLE[i]) & 0xFFFF
+
+        packet[-2] = dxl.DXL_LOBYTE(crc)
+        packet[-1] = dxl.DXL_HIBYTE(crc)
+
+        return packet
+
+
+class MockInstructionPacket(MockDynamixelPacketv2):
+    """
+    Helper class to build valid Dynamixel Protocol 2.0 Instruction Packets.
+
+    Protocol 2.0 Instruction Packet structure
+    (from https://emanual.robotis.com/docs/en/dxl/protocol2/#instruction-packet)
+
+    | Header              | Packet ID | Length      | Instruction | Params            | CRC         |
+    | ------------------- | --------- | ----------- | ----------- | ----------------- | ----------- |
+    | 0xFF 0xFF 0xFD 0x00 | ID        | Len_L Len_H | Instr       | Param 1 … Param N | CRC_L CRC_H |
+
+    """
+
+    @classmethod
+    def _build(cls, dxl_id: int, params: list[int], length: int, instruct_type: str) -> list[int]:
+        instruct_value = INSTRUCTION_TYPES[instruct_type]
+        return [
+            0xFF, 0xFF, 0xFD, 0x00,  # header
+            dxl_id,                  # servo id
+            dxl.DXL_LOBYTE(length),  # length_l
+            dxl.DXL_HIBYTE(length),  # length_h
+            instruct_value,          # instruction type
+            *params,                 # data bytes
+            0x00, 0x00               # placeholder for CRC
+        ]  # fmt: skip
+
+    @classmethod
+    def ping(
+        cls,
+        dxl_id: int,
+    ) -> bytes:
+        """
+        Builds a "Ping" broadcast instruction.
+        (from https://emanual.robotis.com/docs/en/dxl/protocol2/#ping-0x01)
+
+        No parameters required.
+        """
+        params, length = [], 3
+        return cls.build(dxl_id=dxl_id, params=params, length=length, instruct_type="Ping")
+
+    @classmethod
+    def sync_read(
+        cls,
+        dxl_ids: list[int],
+        start_address: int,
+        data_length: int,
+    ) -> bytes:
+        """
+        Builds a "Sync_Read" broadcast instruction.
+        (from https://emanual.robotis.com/docs/en/dxl/protocol2/#sync-read-0x82)
+
+        The parameters for Sync_Read (Protocol 2.0) are:
+            param[0]   = start_address L
+            param[1]   = start_address H
+            param[2]   = data_length L
+            param[3]   = data_length H
+            param[4+]  = motor IDs to read from
+
+        And 'length' = (number_of_params + 7), where:
+            +1 is for instruction byte,
+            +2 is for the address bytes,
+            +2 is for the length bytes,
+            +2 is for the CRC at the end.
+        """
+        params = [
+            dxl.DXL_LOBYTE(start_address),
+            dxl.DXL_HIBYTE(start_address),
+            dxl.DXL_LOBYTE(data_length),
+            dxl.DXL_HIBYTE(data_length),
+            *dxl_ids,
+        ]
+        length = len(dxl_ids) + 7
+        return cls.build(dxl_id=dxl.BROADCAST_ID, params=params, length=length, instruct_type="Sync_Read")
+
+    @classmethod
+    def sync_write(
+        cls,
+        ids_values: dict[int],
+        start_address: int,
+        data_length: int,
+    ) -> bytes:
+        """
+        Builds a "Sync_Write" broadcast instruction.
+        (from https://emanual.robotis.com/docs/en/dxl/protocol2/#sync-write-0x83)
+
+        The parameters for Sync_Write (Protocol 2.0) are:
+            param[0]   = start_address L
+            param[1]   = start_address H
+            param[2]   = data_length L
+            param[3]   = data_length H
+            param[5]   = [1st motor] ID
+            param[5+1] = [1st motor] 1st Byte
+            param[5+2] = [1st motor] 2nd Byte
+            ...
+            param[5+X] = [1st motor] X-th Byte
+            param[6]   = [2nd motor] ID
+            param[6+1] = [2nd motor] 1st Byte
+            param[6+2] = [2nd motor] 2nd Byte
+            ...
+            param[6+X] = [2nd motor] X-th Byte
+
+        And 'length' = ((number_of_params * 1 + data_length) + 7), where:
+            +1 is for instruction byte,
+            +2 is for the address bytes,
+            +2 is for the length bytes,
+            +2 is for the CRC at the end.
+        """
+        data = []
+        for idx, value in ids_values.items():
+            split_value = DynamixelMotorsBus.split_int_bytes(value, data_length)
+            data += [idx, *split_value]
+        params = [
+            dxl.DXL_LOBYTE(start_address),
+            dxl.DXL_HIBYTE(start_address),
+            dxl.DXL_LOBYTE(data_length),
+            dxl.DXL_HIBYTE(data_length),
+            *data,
+        ]
+        length = len(ids_values) * (1 + data_length) + 7
+        return cls.build(dxl_id=dxl.BROADCAST_ID, params=params, length=length, instruct_type="Sync_Write")
+
+
+class MockStatusPacket(MockDynamixelPacketv2):
+    """
+    Helper class to build valid Dynamixel Protocol 2.0 Status Packets.
+
+    Protocol 2.0 Status Packet structure
+    (from https://emanual.robotis.com/docs/en/dxl/protocol2/#status-packet)
+
+    | Header              | Packet ID | Length      | Instruction | Error | Params            | CRC         |
+    | ------------------- | --------- | ----------- | ----------- | ----- | ----------------- | ----------- |
+    | 0xFF 0xFF 0xFD 0x00 | ID        | Len_L Len_H | 0x55        | Err   | Param 1 … Param N | CRC_L CRC_H |
+    """
+
+    @classmethod
+    def _build(cls, dxl_id: int, params: list[int], length: int, error: str = "Success") -> list[int]:
+        err_byte = ERROR_TYPE[error]
+        return [
+            0xFF, 0xFF, 0xFD, 0x00,  # header
+            dxl_id,                  # servo id
+            dxl.DXL_LOBYTE(length),  # length_l
+            dxl.DXL_HIBYTE(length),  # length_h
+            0x55,                    # instruction = 'status'
+            err_byte,                # error
+            *params,                 # data bytes
+            0x00, 0x00               # placeholder for CRC
+        ]  # fmt: skip
+
+    @classmethod
+    def ping(cls, dxl_id: int, model_nb: int = 1190, firm_ver: int = 50) -> bytes:
+        """Builds a 'Ping' status packet.
+
+        https://emanual.robotis.com/docs/en/dxl/protocol2/#ping-0x01
+
+        Args:
+            dxl_id (int): ID of the servo responding.
+            model_nb (int, optional): Desired 'model number' to be returned in the packet. Defaults to 1190
+                which corresponds to a XL330-M077-T.
+            firm_ver (int, optional): Desired 'firmware version' to be returned in the packet.
+                Defaults to 50.
+
+        Returns:
+            bytes: The raw 'Ping' status packet ready to be sent through serial.
+        """
+        params = [dxl.DXL_LOBYTE(model_nb), dxl.DXL_HIBYTE(model_nb), firm_ver]
+        length = 7
+        return cls.build(dxl_id, params=params, length=length)
+
+    @classmethod
+    def present_position(cls, dxl_id: int, pos: int | None = None, min_max_range: tuple = (0, 4095)) -> bytes:
+        """Builds a 'Present_Position' status packet.
+
+        Args:
+            dxl_id (int): ID of the servo responding.
+            pos (int | None, optional): Desired 'Present_Position' to be returned in the packet. If None, it
+                will use a random value in the min_max_range. Defaults to None.
+            min_max_range (tuple, optional): Min/max range to generate the position values used for when 'pos'
+                is None. Note that the bounds are included in the range. Defaults to (0, 4095).
+
+        Returns:
+            bytes: The raw 'Present_Position' status packet ready to be sent through serial.
+        """
+        pos = random.randint(*min_max_range) if pos is None else pos
+        params = [dxl.DXL_LOBYTE(pos), dxl.DXL_HIBYTE(pos), 0, 0]
+        length = 8
+        return cls.build(dxl_id, params=params, length=length)
+
+
+class MockPortHandler(dxl.PortHandler):
+    """
+    This class overwrite the 'setupPort' method of the Dynamixel PortHandler because it can specify
+    baudrates that are not supported with a serial port on MacOS.
+    """
+
+    def setupPort(self, cflag_baud):  # noqa: N802
+        if self.is_open:
+            self.closePort()
+
+        self.ser = serial.Serial(
+            port=self.port_name,
+            # baudrate=self.baudrate,  <- This will fail on MacOS
+            # parity = serial.PARITY_ODD,
+            # stopbits = serial.STOPBITS_TWO,
+            bytesize=serial.EIGHTBITS,
+            timeout=0,
+        )
+        self.is_open = True
+        self.ser.reset_input_buffer()
+        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+
+        return True
+
+
+class WaitableStub(Stub):
+    """
+    In some situations, a test might be checking if a stub has been called before `MockSerial` thread had time
+    to read, match, and call the stub. In these situations, the test can fail randomly.
+
+    Use `wait_called()` or `wait_calls()` to block until the stub is called, avoiding race conditions.
+    """
+
+    def __init__(self, **kwargs):
+        super().__init__(**kwargs)
+        self._event = threading.Event()
+
+    def call(self):
+        self._event.set()
+        return super().call()
+
+    def wait_called(self, timeout: float = 1.0):
+        return self._event.wait(timeout)
+
+    def wait_calls(self, min_calls: int = 1, timeout: float = 1.0):
+        start = time.perf_counter()
+        while time.perf_counter() - start < timeout:
+            if self.calls >= min_calls:
+                return self.calls
+            time.sleep(0.005)
+        raise TimeoutError(f"Stub not called {min_calls} times within {timeout} seconds.")
+
+
+class MockMotors(MockSerial):
+    """
+    This class will simulate physical motors by responding with valid status packets upon receiving some
+    instruction packets. It is meant to test MotorsBus classes.
+    """
+
+    ctrl_table = X_SERIES_CONTROL_TABLE
+
+    def __init__(self):
+        super().__init__()
+
+    @property
+    def stubs(self) -> dict[str, WaitableStub]:
+        return super().stubs
+
+    def stub(self, *, name=None, **kwargs):
+        new_stub = WaitableStub(**kwargs)
+        self._MockSerial__stubs[name or new_stub.receive_bytes] = new_stub
+        return new_stub
+
+    def build_broadcast_ping_stub(
+        self, ids_models_firmwares: 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()
+        )
+        ping_response = self._build_send_fn(return_packets, num_invalid_try)
+
+        stub_name = "Ping_" + "_".join([str(idx) for idx in ids_models_firmwares])
+        self.stub(
+            name=stub_name,
+            receive_bytes=ping_request,
+            send_fn=ping_response,
+        )
+        return stub_name
+
+    def build_ping_stub(
+        self, dxl_id: int, model_nb: int, firm_ver: int = 50, num_invalid_try: int = 0
+    ) -> str:
+        ping_request = MockInstructionPacket.ping(dxl_id)
+        return_packet = MockStatusPacket.ping(dxl_id, model_nb, firm_ver)
+        ping_response = self._build_send_fn(return_packet, num_invalid_try)
+
+        stub_name = f"Ping_{dxl_id}"
+        self.stub(
+            name=stub_name,
+            receive_bytes=ping_request,
+            send_fn=ping_response,
+        )
+        return stub_name
+
+    def build_sync_read_stub(
+        self, data_name: str, ids_values: dict[int, int] | None = None, num_invalid_try: int = 0
+    ) -> str:
+        """
+        'data_name' supported:
+            - Present_Position
+        """
+        address, length = self.ctrl_table[data_name]
+        sync_read_request = MockInstructionPacket.sync_read(list(ids_values), address, length)
+        if data_name != "Present_Position":
+            raise NotImplementedError
+
+        return_packets = b"".join(
+            MockStatusPacket.present_position(idx, pos) for idx, pos in ids_values.items()
+        )
+        sync_read_response = self._build_send_fn(return_packets, num_invalid_try)
+
+        stub_name = f"Sync_Read_{data_name}_" + "_".join([str(idx) for idx in ids_values])
+        self.stub(
+            name=stub_name,
+            receive_bytes=sync_read_request,
+            send_fn=sync_read_response,
+        )
+        return stub_name
+
+    def build_sync_write_stub(
+        self, data_name: str, ids_values: dict[int, int] | None = None, num_invalid_try: int = 0
+    ) -> str:
+        address, length = self.ctrl_table[data_name]
+        sync_read_request = MockInstructionPacket.sync_write(ids_values, address, length)
+        stub_name = f"Sync_Write_{data_name}_" + "_".join([str(idx) for idx in ids_values])
+        self.stub(
+            name=stub_name,
+            receive_bytes=sync_read_request,
+            send_fn=self._build_send_fn(b"", num_invalid_try),
+        )
+        return stub_name
+
+    @staticmethod
+    def _build_send_fn(packet: bytes, num_invalid_try: int = 0) -> Callable[[int], bytes]:
+        def send_fn(_call_count: int) -> bytes:
+            if num_invalid_try >= _call_count:
+                return b""
+            return packet
+
+        return send_fn

tests/mocks/mock_dynamixel_sdk.py

@@ -1,133 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-# ruff: noqa: N802, E741
-
-"""
-Mocked classes and functions from dynamixel_sdk to allow for testing DynamixelMotorsBus code.
-
-Warning: These mocked versions are minimalist. They do not exactly mock every behaviors
-from the original classes and functions (e.g. return types might be None instead of boolean).
-"""
-
-DEFAULT_BAUDRATE = 9_600
-COMM_SUCCESS = 0  # tx or rx packet communication success
-
-
-def convert_to_bytes(value, bytes):
-    # TODO(rcadene): remove need to mock `convert_to_bytes` by implemented the inverse transform
-    # `convert_bytes_to_value`
-    del bytes  # unused
-    return value
-
-
-def get_default_motor_values(motor_index):
-    return {
-        # Key (int) are from X_SERIES_CONTROL_TABLE
-        7: motor_index,  # ID
-        8: DEFAULT_BAUDRATE,  # Baud_rate
-        10: 0,  # Drive_Mode
-        64: 0,  # Torque_Enable
-        # Set 2560 since calibration values for Aloha gripper is between start_pos=2499 and end_pos=3144
-        # For other joints, 2560 will be autocorrected to be in calibration range
-        132: 2560,  # Present_Position
-    }
-
-
-# Macro for Control Table Value
-def DXL_MAKEWORD(a, b):
-    return (a & 0xFF) | ((b & 0xFF) << 8)
-
-
-def DXL_MAKEDWORD(a, b):
-    return (a & 0xFFFF) | (b & 0xFFFF) << 16
-
-
-def DXL_LOWORD(l):
-    return l & 0xFFFF
-
-
-def DXL_HIWORD(l):
-    return (l >> 16) & 0xFFFF
-
-
-def DXL_LOBYTE(w):
-    return w & 0xFF
-
-
-def DXL_HIBYTE(w):
-    return (w >> 8) & 0xFF
-
-
-class PortHandler:
-    def __init__(self, port):
-        self.port = port
-        # factory default baudrate
-        self.baudrate = DEFAULT_BAUDRATE
-
-    def openPort(self):  # noqa: N802
-        return True
-
-    def closePort(self):  # noqa: N802
-        pass
-
-    def setPacketTimeoutMillis(self, timeout_ms):  # noqa: N802
-        del timeout_ms  # unused
-
-    def getBaudRate(self):  # noqa: N802
-        return self.baudrate
-
-    def setBaudRate(self, baudrate):  # noqa: N802
-        self.baudrate = baudrate
-
-
-class PacketHandler:
-    def __init__(self, protocol_version):
-        del protocol_version  # unused
-        # Use packet_handler.data to communicate across Read and Write
-        self.data = {}
-
-
-class GroupSyncRead:
-    def __init__(self, port_handler, packet_handler, address, bytes):
-        self.packet_handler = packet_handler
-
-    def addParam(self, motor_index):  # noqa: N802
-        # Initialize motor default values
-        if motor_index not in self.packet_handler.data:
-            self.packet_handler.data[motor_index] = get_default_motor_values(motor_index)
-
-    def txRxPacket(self):  # noqa: N802
-        return COMM_SUCCESS
-
-    def getData(self, index, address, bytes):  # noqa: N802
-        return self.packet_handler.data[index][address]
-
-
-class GroupSyncWrite:
-    def __init__(self, port_handler, packet_handler, address, bytes):
-        self.packet_handler = packet_handler
-        self.address = address
-
-    def addParam(self, index, data):  # noqa: N802
-        # Initialize motor default values
-        if index not in self.packet_handler.data:
-            self.packet_handler.data[index] = get_default_motor_values(index)
-        self.changeParam(index, data)
-
-    def txPacket(self):  # noqa: N802
-        return COMM_SUCCESS
-
-    def changeParam(self, index, data):  # noqa: N802
-        self.packet_handler.data[index][self.address] = data

tests/mocks/mock_feetech.py

@@ -0,0 +1,374 @@
+import abc
+import random
+import threading
+import time
+from typing import Callable
+
+import scservo_sdk as scs
+import serial
+from mock_serial.mock_serial import MockSerial, Stub
+
+from lerobot.common.motors.feetech import SCS_SERIES_CONTROL_TABLE, FeetechMotorsBus
+
+# https://files.waveshare.com/upload/2/27/Communication_Protocol_User_Manual-EN%28191218-0923%29.pdf
+INSTRUCTION_TYPES = {
+    "Ping": 0x01,	        # Checks whether the Packet has arrived at a device with the same ID as the specified packet ID
+    "Read": 0x02,	        # Read data from the Device
+    "Write": 0x03,	        # Write data to the Device
+    "Reg_Write": 0x04,	    # Register the Instruction Packet in standby status; Packet can later be executed using the Action command
+    "Action": 0x05,	        # Executes a Packet that was registered beforehand using Reg Write
+    "Factory_Reset": 0x06,  # Resets the Control Table to its initial factory default settings
+    "Sync_Read": 0x82,	           # Read data from multiple devices with the same Address with the same length at once
+    "Sync_Write": 0x83,	    # Write data to multiple devices with the same Address with the same length at once
+}  # fmt: skip
+
+ERROR_TYPE = {
+    "Success": 0x00,
+    "Voltage": 0x01,
+    "Angle": 0x02,
+    "Overheat": 0x04,
+    "Overele": 0x08,
+    "Overload": 0x20,
+}
+
+
+class MockFeetechPacket(abc.ABC):
+    @classmethod
+    def build(cls, scs_id: int, params: list[int], length: int, *args, **kwargs) -> bytes:
+        packet = cls._build(scs_id, params, length, *args, **kwargs)
+        packet = cls._add_checksum(packet)
+        return bytes(packet)
+
+    @abc.abstractclassmethod
+    def _build(cls, scs_id: int, params: list[int], length: int, *args, **kwargs) -> list[int]:
+        pass
+
+    @staticmethod
+    def _add_checksum(packet: list[int]) -> list[int]:
+        checksum = 0
+        for idx in range(2, len(packet) - 1):  # except header & checksum
+            checksum += packet[idx]
+
+        packet[-1] = scs.SCS_LOBYTE(~checksum)
+
+        return packet
+
+
+class MockInstructionPacket(MockFeetechPacket):
+    """
+    Helper class to build valid Feetech Instruction Packets.
+
+    Instruction Packet structure
+    (from https://files.waveshare.com/upload/2/27/Communication_Protocol_User_Manual-EN%28191218-0923%29.pdf)
+
+    | Header    | Packet ID | Length | Instruction | Params            | Checksum |
+    | --------- | --------- | ------ | ----------- | ----------------- | -------- |
+    | 0xFF 0xFF | ID        | Len    | Instr       | Param 1 … Param N | Sum      |
+
+    """
+
+    @classmethod
+    def _build(cls, scs_id: int, params: list[int], length: int, instruct_type: str) -> list[int]:
+        instruct_value = INSTRUCTION_TYPES[instruct_type]
+        return [
+            0xFF, 0xFF,      # header
+            scs_id,          # servo id
+            length,          # length
+            instruct_value,  # instruction type
+            *params,         # data bytes
+            0x00,            # placeholder for checksum
+        ]  # fmt: skip
+
+    @classmethod
+    def ping(
+        cls,
+        scs_id: int,
+    ) -> bytes:
+        """
+        Builds a "Ping" broadcast instruction.
+
+        No parameters required.
+        """
+        params, length = [], 2
+        return cls.build(scs_id=scs_id, params=params, length=length, instruct_type="Ping")
+
+    @classmethod
+    def sync_read(
+        cls,
+        scs_ids: list[int],
+        start_address: int,
+        data_length: int,
+    ) -> bytes:
+        """
+        Builds a "Sync_Read" broadcast instruction.
+
+        The parameters for Sync Read are:
+            param[0]   = start_address
+            param[1]   = data_length
+            param[2+]  = motor IDs to read from
+
+        And 'length' = (number_of_params + 4), where:
+            +1 is for instruction byte,
+            +1 is for the address byte,
+            +1 is for the length bytes,
+            +1 is for the checksum at the end.
+        """
+        params = [start_address, data_length, *scs_ids]
+        length = len(scs_ids) + 4
+        return cls.build(scs_id=scs.BROADCAST_ID, params=params, length=length, instruct_type="Sync_Read")
+
+    @classmethod
+    def sync_write(
+        cls,
+        ids_values: dict[int],
+        start_address: int,
+        data_length: int,
+    ) -> bytes:
+        """
+        Builds a "Sync_Write" broadcast instruction.
+
+        The parameters for Sync_Write are:
+            param[0]   = start_address
+            param[1]   = data_length
+            param[2]   = [1st motor] ID
+            param[2+1] = [1st motor] 1st Byte
+            param[2+2] = [1st motor] 2nd Byte
+            ...
+            param[5+X] = [1st motor] X-th Byte
+            param[6]   = [2nd motor] ID
+            param[6+1] = [2nd motor] 1st Byte
+            param[6+2] = [2nd motor] 2nd Byte
+            ...
+            param[6+X] = [2nd motor] X-th Byte
+
+        And 'length' = ((number_of_params * 1 + data_length) + 4), where:
+            +1 is for instruction byte,
+            +1 is for the address byte,
+            +1 is for the length bytes,
+            +1 is for the checksum at the end.
+        """
+        data = []
+        for idx, value in ids_values.items():
+            split_value = FeetechMotorsBus.split_int_bytes(value, data_length)
+            data += [idx, *split_value]
+        params = [start_address, data_length, *data]
+        length = len(ids_values) * (1 + data_length) + 4
+        return cls.build(scs_id=scs.BROADCAST_ID, params=params, length=length, instruct_type="Sync_Write")
+
+
+class MockStatusPacket(MockFeetechPacket):
+    """
+    Helper class to build valid Feetech Status Packets.
+
+    Status Packet structure
+    (from https://files.waveshare.com/upload/2/27/Communication_Protocol_User_Manual-EN%28191218-0923%29.pdf)
+
+    | Header    | Packet ID | Length | Error | Params            | Checksum |
+    | --------- | --------- | ------ | ----- | ----------------- | -------- |
+    | 0xFF 0xFF | ID        | Len    | Err   | Param 1 … Param N | Sum      |
+
+    """
+
+    @classmethod
+    def _build(cls, scs_id: int, params: list[int], length: int, error: str = "Success") -> list[int]:
+        err_byte = ERROR_TYPE[error]
+        return [
+            0xFF, 0xFF,  # header
+            scs_id,      # servo id
+            length,      # length
+            err_byte,    # status
+            *params,     # data bytes
+            0x00,        # placeholder for checksum
+        ]  # fmt: skip
+
+    @classmethod
+    def ping(cls, scs_id: int, model_nb: int = 1190, firm_ver: int = 50) -> bytes:
+        """Builds a 'Ping' status packet.
+
+        Args:
+            scs_id (int): ID of the servo responding.
+            model_nb (int, optional): Desired 'model number' to be returned in the packet. Defaults to 1190
+                which corresponds to a XL330-M077-T.
+            firm_ver (int, optional): Desired 'firmware version' to be returned in the packet.
+                Defaults to 50.
+
+        Returns:
+            bytes: The raw 'Ping' status packet ready to be sent through serial.
+        """
+        # raise NotImplementedError
+        params = [scs.SCS_LOBYTE(model_nb), scs.SCS_HIBYTE(model_nb), firm_ver]
+        length = 2
+        return cls.build(scs_id, params=params, length=length)
+
+    @classmethod
+    def present_position(cls, scs_id: int, pos: int | None = None, min_max_range: tuple = (0, 4095)) -> bytes:
+        """Builds a 'Present_Position' status packet.
+
+        Args:
+            scs_id (int): List of the servos ids
+            pos (int | None, optional): Desired 'Present_Position' to be returned in the packet. If None, it
+                will use a random value in the min_max_range. Defaults to None.
+            min_max_range (tuple, optional): Min/max range to generate the position values used for when 'pos'
+                is None. Note that the bounds are included in the range. Defaults to (0, 4095).
+
+        Returns:
+            bytes: The raw 'Present_Position' status packet ready to be sent through serial.
+        """
+        pos = random.randint(*min_max_range) if pos is None else pos
+        params = [scs.SCS_LOBYTE(pos), scs.SCS_HIBYTE(pos)]
+        length = 4
+        return cls.build(scs_id, params=params, length=length)
+
+
+class MockPortHandler(scs.PortHandler):
+    """
+    This class overwrite the 'setupPort' method of the Feetech PortHandler because it can specify
+    baudrates that are not supported with a serial port on MacOS.
+    """
+
+    def setupPort(self, cflag_baud):  # noqa: N802
+        if self.is_open:
+            self.closePort()
+
+        self.ser = serial.Serial(
+            port=self.port_name,
+            # baudrate=self.baudrate,  <- This will fail on MacOS
+            # parity = serial.PARITY_ODD,
+            # stopbits = serial.STOPBITS_TWO,
+            bytesize=serial.EIGHTBITS,
+            timeout=0,
+        )
+        self.is_open = True
+        self.ser.reset_input_buffer()
+        self.tx_time_per_byte = (1000.0 / self.baudrate) * 10.0
+
+        return True
+
+
+class WaitableStub(Stub):
+    """
+    In some situations, a test might be checking if a stub has been called before `MockSerial` thread had time
+    to read, match, and call the stub. In these situations, the test can fail randomly.
+
+    Use `wait_called()` or `wait_calls()` to block until the stub is called, avoiding race conditions.
+    """
+
+    def __init__(self, **kwargs):
+        super().__init__(**kwargs)
+        self._event = threading.Event()
+
+    def call(self):
+        self._event.set()
+        return super().call()
+
+    def wait_called(self, timeout: float = 1.0):
+        return self._event.wait(timeout)
+
+    def wait_calls(self, min_calls: int = 1, timeout: float = 1.0):
+        start = time.perf_counter()
+        while time.perf_counter() - start < timeout:
+            if self.calls >= min_calls:
+                return self.calls
+            time.sleep(0.005)
+        raise TimeoutError(f"Stub not called {min_calls} times within {timeout} seconds.")
+
+
+class MockMotors(MockSerial):
+    """
+    This class will simulate physical motors by responding with valid status packets upon receiving some
+    instruction packets. It is meant to test MotorsBus classes.
+    """
+
+    ctrl_table = SCS_SERIES_CONTROL_TABLE
+
+    def __init__(self):
+        super().__init__()
+
+    @property
+    def stubs(self) -> dict[str, WaitableStub]:
+        return super().stubs
+
+    def stub(self, *, name=None, **kwargs):
+        new_stub = WaitableStub(**kwargs)
+        self._MockSerial__stubs[name or new_stub.receive_bytes] = new_stub
+        return new_stub
+
+    def build_broadcast_ping_stub(
+        self, ids_models_firmwares: dict[int, list[int]] | None = None, num_invalid_try: int = 0
+    ) -> str:
+        ping_request = MockInstructionPacket.ping(scs.BROADCAST_ID)
+        return_packets = b"".join(
+            MockStatusPacket.ping(idx, model, firm_ver)
+            for idx, (model, firm_ver) in ids_models_firmwares.items()
+        )
+        ping_response = self._build_send_fn(return_packets, num_invalid_try)
+
+        stub_name = "Ping_" + "_".join([str(idx) for idx in ids_models_firmwares])
+        self.stub(
+            name=stub_name,
+            receive_bytes=ping_request,
+            send_fn=ping_response,
+        )
+        return stub_name
+
+    def build_ping_stub(
+        self, scs_id: int, model_nb: int, firm_ver: int = 50, num_invalid_try: int = 0
+    ) -> str:
+        ping_request = MockInstructionPacket.ping(scs_id)
+        return_packet = MockStatusPacket.ping(scs_id, model_nb, firm_ver)
+        ping_response = self._build_send_fn(return_packet, num_invalid_try)
+
+        stub_name = f"Ping_{scs_id}"
+        self.stub(
+            name=stub_name,
+            receive_bytes=ping_request,
+            send_fn=ping_response,
+        )
+        return stub_name
+
+    def build_sync_read_stub(
+        self, data_name: str, ids_values: dict[int, int] | None = None, num_invalid_try: int = 0
+    ) -> str:
+        """
+        'data_name' supported:
+            - Present_Position
+        """
+        address, length = self.ctrl_table[data_name]
+        sync_read_request = MockInstructionPacket.sync_read(list(ids_values), address, length)
+        if data_name != "Present_Position":
+            raise NotImplementedError
+
+        return_packets = b"".join(
+            MockStatusPacket.present_position(idx, pos) for idx, pos in ids_values.items()
+        )
+        sync_read_response = self._build_send_fn(return_packets, num_invalid_try)
+
+        stub_name = f"Sync_Read_{data_name}_" + "_".join([str(idx) for idx in ids_values])
+        self.stub(
+            name=stub_name,
+            receive_bytes=sync_read_request,
+            send_fn=sync_read_response,
+        )
+        return stub_name
+
+    def build_sync_write_stub(
+        self, data_name: str, ids_values: dict[int, int] | None = None, num_invalid_try: int = 0
+    ) -> str:
+        address, length = self.ctrl_table[data_name]
+        sync_read_request = MockInstructionPacket.sync_write(ids_values, address, length)
+        stub_name = f"Sync_Write_{data_name}_" + "_".join([str(idx) for idx in ids_values])
+        self.stub(
+            name=stub_name,
+            receive_bytes=sync_read_request,
+            send_fn=self._build_send_fn(b"", num_invalid_try),
+        )
+        return stub_name
+
+    @staticmethod
+    def _build_send_fn(packet: bytes, num_invalid_try: int = 0) -> Callable[[int], bytes]:
+        def send_fn(_call_count: int) -> bytes:
+            if num_invalid_try >= _call_count:
+                return b""
+            return packet
+
+        return send_fn

tests/mocks/mock_scservo_sdk.py

@@ -1,151 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-
-# ruff: noqa: N802, E741
-
-"""
-Mocked classes and functions from scservo_sdk to allow for testing FeetechMotorsBus code.
-
-Warning: These mocked versions are minimalist. They do not exactly mock every behaviors
-from the original classes and functions (e.g. return types might be None instead of boolean).
-"""
-
-DEFAULT_BAUDRATE = 1_000_000
-COMM_SUCCESS = 0  # tx or rx packet communication success
-
-
-def convert_to_bytes(value, bytes):
-    # TODO(rcadene): remove need to mock `convert_to_bytes` by implemented the inverse transform
-    # `convert_bytes_to_value`
-    del bytes  # unused
-    return value
-
-
-def get_default_motor_values(motor_index):
-    return {
-        # Key (int) are from SCS_SERIES_CONTROL_TABLE
-        5: motor_index,  # ID
-        6: DEFAULT_BAUDRATE,  # Baud_rate
-        10: 0,  # Drive_Mode
-        21: 32,  # P_Coefficient
-        22: 32,  # D_Coefficient
-        23: 0,  # I_Coefficient
-        40: 0,  # Torque_Enable
-        41: 254,  # Acceleration
-        31: -2047,  # Offset
-        33: 0,  # Mode
-        55: 1,  # Lock
-        # Set 2560 since calibration values for Aloha gripper is between start_pos=2499 and end_pos=3144
-        # For other joints, 2560 will be autocorrected to be in calibration range
-        56: 2560,  # Present_Position
-        58: 0,  # Present_Speed
-        69: 0,  # Present_Current
-        85: 150,  # Maximum_Acceleration
-    }
-
-
-# Macro for Control Table Value
-def SCS_MAKEWORD(a, b):
-    return (a & 0xFF) | ((b & 0xFF) << 8)
-
-
-def SCS_MAKEDWORD(a, b):
-    return (a & 0xFFFF) | (b & 0xFFFF) << 16
-
-
-def SCS_LOWORD(l):
-    return l & 0xFFFF
-
-
-def SCS_HIWORD(l):
-    return (l >> 16) & 0xFFFF
-
-
-def SCS_LOBYTE(w):
-    return w & 0xFF
-
-
-def SCS_HIBYTE(w):
-    return (w >> 8) & 0xFF
-
-
-class PortHandler:
-    def __init__(self, port):
-        self.port = port
-        # factory default baudrate
-        self.baudrate = DEFAULT_BAUDRATE
-        self.ser = SerialMock()
-
-    def openPort(self):  # noqa: N802
-        return True
-
-    def closePort(self):  # noqa: N802
-        pass
-
-    def setPacketTimeoutMillis(self, timeout_ms):  # noqa: N802
-        del timeout_ms  # unused
-
-    def getBaudRate(self):  # noqa: N802
-        return self.baudrate
-
-    def setBaudRate(self, baudrate):  # noqa: N802
-        self.baudrate = baudrate
-
-
-class PacketHandler:
-    def __init__(self, protocol_version):
-        del protocol_version  # unused
-        # Use packet_handler.data to communicate across Read and Write
-        self.data = {}
-
-
-class GroupSyncRead:
-    def __init__(self, port_handler, packet_handler, address, bytes):
-        self.packet_handler = packet_handler
-
-    def addParam(self, motor_index):  # noqa: N802
-        # Initialize motor default values
-        if motor_index not in self.packet_handler.data:
-            self.packet_handler.data[motor_index] = get_default_motor_values(motor_index)
-
-    def txRxPacket(self):  # noqa: N802
-        return COMM_SUCCESS
-
-    def getData(self, index, address, bytes):  # noqa: N802
-        return self.packet_handler.data[index][address]
-
-
-class GroupSyncWrite:
-    def __init__(self, port_handler, packet_handler, address, bytes):
-        self.packet_handler = packet_handler
-        self.address = address
-
-    def addParam(self, index, data):  # noqa: N802
-        if index not in self.packet_handler.data:
-            self.packet_handler.data[index] = get_default_motor_values(index)
-        self.changeParam(index, data)
-
-    def txPacket(self):  # noqa: N802
-        return COMM_SUCCESS
-
-    def changeParam(self, index, data):  # noqa: N802
-        self.packet_handler.data[index][self.address] = data
-
-
-class SerialMock:
-    def reset_output_buffer(self):
-        pass
-
-    def reset_input_buffer(self):
-        pass

tests/motors/test_dynamixel.py

@@ -1,47 +1,289 @@
 import sys
+from typing import Generator
 from unittest.mock import patch
 
+import dynamixel_sdk as dxl
 import pytest
 
-from tests.mocks import mock_dynamixel_sdk
+from lerobot.common.motors import Motor
+from lerobot.common.motors.dynamixel import DynamixelMotorsBus
+from tests.mocks.mock_dynamixel import MockMotors, MockPortHandler
 
 
 @pytest.fixture(autouse=True)
-def patch_dynamixel_sdk():
-    with patch.dict(sys.modules, {"dynamixel_sdk": mock_dynamixel_sdk}):
+def patch_port_handler():
+    if sys.platform == "darwin":
+        with patch.object(dxl, "PortHandler", MockPortHandler):
+            yield
+    else:
         yield
 
 
-def test_patch_sdk():
-    assert "dynamixel_sdk" in sys.modules  # Should be patched
-    assert sys.modules["dynamixel_sdk"] is mock_dynamixel_sdk  # Should match the mock
+@pytest.fixture
+def mock_motors() -> Generator[MockMotors, None, None]:
+    motors = MockMotors()
+    motors.open()
+    yield motors
+    motors.close()
 
 
-def test_abc_implementation():
-    from lerobot.common.motors.dynamixel import DynamixelMotorsBus
-
-    # Instantiation should raise an error if the class doesn't implements abstract methods/properties
-    DynamixelMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "xl330-m077")})
+@pytest.fixture
+def dummy_motors() -> dict[str, Motor]:
+    return {
+        "dummy_1": Motor(id=1, model="xl430-w250"),
+        "dummy_2": Motor(id=2, model="xm540-w270"),
+        "dummy_3": Motor(id=3, model="xl330-m077"),
+    }
 
 
-def test_configure_motors_all_ids_1():
-    from lerobot.common.motors.dynamixel import DynamixelMotorsBus
+@pytest.mark.skipif(sys.platform != "darwin", reason=f"No patching needed on {sys.platform=}")
+def test_autouse_patch():
+    """Ensures that the autouse fixture correctly patches dxl.PortHandler with MockPortHandler."""
+    assert dxl.PortHandler is MockPortHandler
 
-    # see X_SERIES_BAUDRATE_TABLE
-    smaller_baudrate = 9_600
-    smaller_baudrate_value = 0
 
-    # This test expect the configuration was already correct.
-    motors_bus = DynamixelMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "xl330-m077")})
+@pytest.mark.parametrize(
+    "value, n_bytes, expected",
+    [
+        (0x12,       1, [0x12]),
+        (0x1234,     2, [0x34, 0x12]),
+        (0x12345678, 4, [0x78, 0x56, 0x34, 0x12]),
+        (0,          1, [0x00]),
+        (0,          2, [0x00, 0x00]),
+        (0,          4, [0x00, 0x00, 0x00, 0x00]),
+        (255,        1, [0xFF]),
+        (65535,      2, [0xFF, 0xFF]),
+        (4294967295, 4, [0xFF, 0xFF, 0xFF, 0xFF]),
+    ],
+    ids=[
+        "1 byte",
+        "2 bytes",
+        "4 bytes",
+        "0 with 1 byte",
+        "0 with 2 bytes",
+        "0 with 4 bytes",
+        "max single byte",
+        "max two bytes",
+        "max four bytes",
+    ],
+)  # fmt: skip
+def test_split_int_bytes(value, n_bytes, expected):
+    assert DynamixelMotorsBus.split_int_bytes(value, n_bytes) == expected
+
+
+def test_split_int_bytes_invalid_n_bytes():
+    with pytest.raises(NotImplementedError):
+        DynamixelMotorsBus.split_int_bytes(100, 3)
+
+
+def test_split_int_bytes_negative_numbers():
+    with pytest.raises(ValueError):
+        neg = DynamixelMotorsBus.split_int_bytes(-1, 1)
+        print(neg)
+
+
+def test_split_int_bytes_large_number():
+    with pytest.raises(ValueError):
+        DynamixelMotorsBus.split_int_bytes(2**32, 4)  # 4-byte max is 0xFFFFFFFF
+
+
+def test_abc_implementation(dummy_motors):
+    """Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
+    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):
+    stub_name = mock_motors.build_ping_stub(idx, model_nb)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
     motors_bus.connect()
-    motors_bus.write("Baud_Rate", [smaller_baudrate_value] * len(motors_bus))
 
-    motors_bus.set_baudrate(smaller_baudrate)
-    motors_bus.write("ID", [1] * len(motors_bus))
-    del motors_bus
+    ping_model_nb = motors_bus.ping(idx)
 
-    # Test configure
-    motors_bus = DynamixelMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "xl330-m077")})
+    assert ping_model_nb == model_nb
+    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)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
     motors_bus.connect()
-    assert motors_bus.are_motors_configured()
-    del motors_bus
+
+    ping_list = motors_bus.broadcast_ping()
+
+    assert ping_list == expected_pings
+    assert mock_motors.stubs[stub_name].called
+
+
+@pytest.mark.parametrize(
+    "motors",
+    [
+        None,
+        [1, 2, 3],
+        ["dummy_1", "dummy_2", "dummy_3"],
+        [1, "dummy_2", 3],
+    ],
+    ids=["None", "by ids", "by names", "mixed"],
+)
+def test_read_all_motors(motors, mock_motors, dummy_motors):
+    expected_positions = {
+        1: 1337,
+        2: 42,
+        3: 4016,
+    }
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_positions)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    positions_read = motors_bus.read("Present_Position", motors=motors)
+
+    motors = ["dummy_1", "dummy_2", "dummy_3"] if motors is None else motors
+    assert mock_motors.stubs[stub_name].called
+    assert positions_read == dict(zip(motors, expected_positions.values(), strict=True))
+
+
+@pytest.mark.parametrize(
+    "idx, pos",
+    [
+        [1, 1337],
+        [2, 42],
+        [3, 4016],
+    ],
+)
+def test_read_single_motor_by_name(idx, pos, mock_motors, dummy_motors):
+    expected_position = {idx: pos}
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_position)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    pos_dict = motors_bus.read("Present_Position", f"dummy_{idx}")
+
+    assert mock_motors.stubs[stub_name].called
+    assert pos_dict == {f"dummy_{idx}": pos}
+
+
+@pytest.mark.parametrize(
+    "idx, pos",
+    [
+        [1, 1337],
+        [2, 42],
+        [3, 4016],
+    ],
+)
+def test_read_single_motor_by_id(idx, pos, mock_motors, dummy_motors):
+    expected_position = {idx: pos}
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_position)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    pos_dict = motors_bus.read("Present_Position", idx)
+
+    assert mock_motors.stubs[stub_name].called
+    assert pos_dict == {idx: pos}
+
+
+@pytest.mark.parametrize(
+    "num_retry, num_invalid_try, pos",
+    [
+        [0, 2, 1337],
+        [2, 3, 42],
+        [3, 2, 4016],
+        [2, 1, 999],
+    ],
+)
+def test_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy_motors):
+    expected_position = {1: pos}
+    stub_name = mock_motors.build_sync_read_stub(
+        "Present_Position", expected_position, num_invalid_try=num_invalid_try
+    )
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    if num_retry >= num_invalid_try:
+        pos_dict = motors_bus.read("Present_Position", 1, num_retry=num_retry)
+        assert pos_dict == {1: pos}
+    else:
+        with pytest.raises(ConnectionError):
+            _ = motors_bus.read("Present_Position", 1, num_retry=num_retry)
+
+    expected_calls = min(1 + num_retry, 1 + num_invalid_try)
+    assert mock_motors.stubs[stub_name].calls == expected_calls
+
+
+@pytest.mark.parametrize(
+    "motors",
+    [
+        [1, 2, 3],
+        ["dummy_1", "dummy_2", "dummy_3"],
+        [1, "dummy_2", 3],
+    ],
+    ids=["by ids", "by names", "mixed"],
+)
+def test_write_all_motors(motors, mock_motors, dummy_motors):
+    goal_positions = {
+        1: 1337,
+        2: 42,
+        3: 4016,
+    }
+    stub_name = mock_motors.build_sync_write_stub("Goal_Position", goal_positions)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    values = dict(zip(motors, goal_positions.values(), strict=True))
+    motors_bus.write("Goal_Position", values)
+
+    assert mock_motors.stubs[stub_name].wait_called()
+
+
+@pytest.mark.parametrize(
+    "data_name, value",
+    [
+        ["Torque_Enable", 0],
+        ["Torque_Enable", 1],
+    ],
+)
+def test_write_all_motors_single_value(data_name, value, mock_motors, dummy_motors):
+    values = {m.id: value for m in dummy_motors.values()}
+    stub_name = mock_motors.build_sync_write_stub(data_name, values)
+    motors_bus = DynamixelMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    motors_bus.write(data_name, value)
+
+    assert mock_motors.stubs[stub_name].wait_called()

tests/motors/test_feetech.py

@@ -1,47 +1,291 @@
 import sys
+from typing import Generator
 from unittest.mock import patch
 
 import pytest
+import scservo_sdk as scs
 
-from tests.mocks import mock_scservo_sdk
+from lerobot.common.motors import Motor
+from lerobot.common.motors.feetech import FeetechMotorsBus
+from tests.mocks.mock_feetech import MockMotors, MockPortHandler
 
 
 @pytest.fixture(autouse=True)
-def patch_scservo_sdk():
-    with patch.dict(sys.modules, {"scservo_sdk": mock_scservo_sdk}):
+def patch_port_handler():
+    if sys.platform == "darwin":
+        with patch.object(scs, "PortHandler", MockPortHandler):
+            yield
+    else:
         yield
 
 
-def test_patch_sdk():
-    assert "scservo_sdk" in sys.modules  # Should be patched
-    assert sys.modules["scservo_sdk"] is mock_scservo_sdk  # Should match the mock
+@pytest.fixture
+def mock_motors() -> Generator[MockMotors, None, None]:
+    motors = MockMotors()
+    motors.open()
+    yield motors
+    motors.close()
 
 
-def test_abc_implementation():
-    from lerobot.common.motors.feetech import FeetechMotorsBus
-
-    # Instantiation should raise an error if the class doesn't implements abstract methods/properties
-    FeetechMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "sts3215")})
+@pytest.fixture
+def dummy_motors() -> dict[str, Motor]:
+    return {
+        "dummy_1": Motor(id=1, model="sts3215"),
+        "dummy_2": Motor(id=2, model="sts3215"),
+        "dummy_3": Motor(id=3, model="sts3215"),
+    }
 
 
-def test_configure_motors_all_ids_1():
-    from lerobot.common.motors.feetech import FeetechMotorsBus
+@pytest.mark.skipif(sys.platform != "darwin", reason=f"No patching needed on {sys.platform=}")
+def test_autouse_patch():
+    """Ensures that the autouse fixture correctly patches scs.PortHandler with MockPortHandler."""
+    assert scs.PortHandler is MockPortHandler
 
-    # see SCS_SERIES_BAUDRATE_TABLE
-    smaller_baudrate = 19_200
-    smaller_baudrate_value = 7
 
-    # This test expect the configuration was already correct.
-    motors_bus = FeetechMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "sts3215")})
+@pytest.mark.parametrize(
+    "value, n_bytes, expected",
+    [
+        (0x12,       1, [0x12]),
+        (0x1234,     2, [0x34, 0x12]),
+        (0x12345678, 4, [0x78, 0x56, 0x34, 0x12]),
+        (0,          1, [0x00]),
+        (0,          2, [0x00, 0x00]),
+        (0,          4, [0x00, 0x00, 0x00, 0x00]),
+        (255,        1, [0xFF]),
+        (65535,      2, [0xFF, 0xFF]),
+        (4294967295, 4, [0xFF, 0xFF, 0xFF, 0xFF]),
+    ],
+    ids=[
+        "1 byte",
+        "2 bytes",
+        "4 bytes",
+        "0 with 1 byte",
+        "0 with 2 bytes",
+        "0 with 4 bytes",
+        "max single byte",
+        "max two bytes",
+        "max four bytes",
+    ],
+)  # fmt: skip
+def test_split_int_bytes(value, n_bytes, expected):
+    assert FeetechMotorsBus.split_int_bytes(value, n_bytes) == expected
+
+
+def test_split_int_bytes_invalid_n_bytes():
+    with pytest.raises(NotImplementedError):
+        FeetechMotorsBus.split_int_bytes(100, 3)
+
+
+def test_split_int_bytes_negative_numbers():
+    with pytest.raises(ValueError):
+        neg = FeetechMotorsBus.split_int_bytes(-1, 1)
+        print(neg)
+
+
+def test_split_int_bytes_large_number():
+    with pytest.raises(ValueError):
+        FeetechMotorsBus.split_int_bytes(2**32, 4)  # 4-byte max is 0xFFFFFFFF
+
+
+def test_abc_implementation(dummy_motors):
+    """Instantiation should raise an error if the class doesn't implement abstract methods/properties."""
+    FeetechMotorsBus(port="/dev/dummy-port", motors=dummy_motors)
+
+
+@pytest.mark.skip("TODO")
+@pytest.mark.parametrize(
+    "idx, model_nb",
+    [
+        [1, 1190],
+        [2, 1200],
+        [3, 1120],
+    ],
+)
+def test_ping(idx, model_nb, mock_motors, dummy_motors):
+    stub_name = mock_motors.build_ping_stub(idx, model_nb)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
     motors_bus.connect()
-    motors_bus.write("Baud_Rate", [smaller_baudrate_value] * len(motors_bus))
 
-    motors_bus.set_baudrate(smaller_baudrate)
-    motors_bus.write("ID", [1] * len(motors_bus))
-    del motors_bus
+    ping_model_nb = motors_bus.ping(idx)
 
-    # Test configure
-    motors_bus = FeetechMotorsBus(port="/dev/dummy-port", motors={"dummy": (1, "sts3215")})
+    assert ping_model_nb == model_nb
+    assert mock_motors.stubs[stub_name].called
+
+
+@pytest.mark.skip("TODO")
+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)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
     motors_bus.connect()
-    assert motors_bus.are_motors_configured()
-    del motors_bus
+
+    ping_list = motors_bus.broadcast_ping()
+
+    assert ping_list == expected_pings
+    assert mock_motors.stubs[stub_name].called
+
+
+@pytest.mark.parametrize(
+    "motors",
+    [
+        None,
+        [1, 2, 3],
+        ["dummy_1", "dummy_2", "dummy_3"],
+        [1, "dummy_2", 3],
+    ],
+    ids=["None", "by ids", "by names", "mixed"],
+)
+def test_read_all_motors(motors, mock_motors, dummy_motors):
+    expected_positions = {
+        1: 1337,
+        2: 42,
+        3: 4016,
+    }
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_positions)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    positions_read = motors_bus.read("Present_Position", motors=motors)
+
+    motors = ["dummy_1", "dummy_2", "dummy_3"] if motors is None else motors
+    assert mock_motors.stubs[stub_name].called
+    assert positions_read == dict(zip(motors, expected_positions.values(), strict=True))
+
+
+@pytest.mark.parametrize(
+    "idx, pos",
+    [
+        [1, 1337],
+        [2, 42],
+        [3, 4016],
+    ],
+)
+def test_read_single_motor_by_name(idx, pos, mock_motors, dummy_motors):
+    expected_position = {idx: pos}
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_position)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    pos_dict = motors_bus.read("Present_Position", f"dummy_{idx}")
+
+    assert mock_motors.stubs[stub_name].called
+    assert pos_dict == {f"dummy_{idx}": pos}
+
+
+@pytest.mark.parametrize(
+    "idx, pos",
+    [
+        [1, 1337],
+        [2, 42],
+        [3, 4016],
+    ],
+)
+def test_read_single_motor_by_id(idx, pos, mock_motors, dummy_motors):
+    expected_position = {idx: pos}
+    stub_name = mock_motors.build_sync_read_stub("Present_Position", expected_position)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    pos_dict = motors_bus.read("Present_Position", idx)
+
+    assert mock_motors.stubs[stub_name].called
+    assert pos_dict == {idx: pos}
+
+
+@pytest.mark.parametrize(
+    "num_retry, num_invalid_try, pos",
+    [
+        [0, 2, 1337],
+        [2, 3, 42],
+        [3, 2, 4016],
+        [2, 1, 999],
+    ],
+)
+def test_read_num_retry(num_retry, num_invalid_try, pos, mock_motors, dummy_motors):
+    expected_position = {1: pos}
+    stub_name = mock_motors.build_sync_read_stub(
+        "Present_Position", expected_position, num_invalid_try=num_invalid_try
+    )
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    if num_retry >= num_invalid_try:
+        pos_dict = motors_bus.read("Present_Position", 1, num_retry=num_retry)
+        assert pos_dict == {1: pos}
+    else:
+        with pytest.raises(ConnectionError):
+            _ = motors_bus.read("Present_Position", 1, num_retry=num_retry)
+
+    expected_calls = min(1 + num_retry, 1 + num_invalid_try)
+    assert mock_motors.stubs[stub_name].calls == expected_calls
+
+
+@pytest.mark.parametrize(
+    "motors",
+    [
+        [1, 2, 3],
+        ["dummy_1", "dummy_2", "dummy_3"],
+        [1, "dummy_2", 3],
+    ],
+    ids=["by ids", "by names", "mixed"],
+)
+def test_write_all_motors(motors, mock_motors, dummy_motors):
+    goal_positions = {
+        1: 1337,
+        2: 42,
+        3: 4016,
+    }
+    stub_name = mock_motors.build_sync_write_stub("Goal_Position", goal_positions)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    values = dict(zip(motors, goal_positions.values(), strict=True))
+    motors_bus.write("Goal_Position", values)
+
+    assert mock_motors.stubs[stub_name].wait_called()
+
+
+@pytest.mark.parametrize(
+    "data_name, value",
+    [
+        ["Torque_Enable", 0],
+        ["Torque_Enable", 1],
+    ],
+)
+def test_write_all_motors_single_value(data_name, value, mock_motors, dummy_motors):
+    values = {m.id: value for m in dummy_motors.values()}
+    stub_name = mock_motors.build_sync_write_stub(data_name, values)
+    motors_bus = FeetechMotorsBus(
+        port=mock_motors.port,
+        motors=dummy_motors,
+    )
+    motors_bus.connect()
+
+    motors_bus.write(data_name, value)
+
+    assert mock_motors.stubs[stub_name].wait_called()

tests/motors/test_motors_bus.py

@@ -1,123 +0,0 @@
-# Copyright 2024 The HuggingFace Inc. team. All rights reserved.
-#
-# Licensed under the Apache License, Version 2.0 (the "License");
-# you may not use this file except in compliance with the License.
-# You may obtain a copy of the License at
-#
-#     http://www.apache.org/licenses/LICENSE-2.0
-#
-# Unless required by applicable law or agreed to in writing, software
-# distributed under the License is distributed on an "AS IS" BASIS,
-# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-# See the License for the specific language governing permissions and
-# limitations under the License.
-"""
-Tests for physical motors and their mocked versions.
-If the physical motors are not connected to the computer, or not working,
-the test will be skipped.
-
-Example of running a specific test:
-```bash
-pytest -sx tests/test_motors.py::test_find_port
-pytest -sx tests/test_motors.py::test_motors_bus
-```
-
-Example of running test on real dynamixel motors connected to the computer:
-```bash
-pytest -sx 'tests/test_motors.py::test_motors_bus[dynamixel-False]'
-```
-
-Example of running test on a mocked version of dynamixel motors:
-```bash
-pytest -sx 'tests/test_motors.py::test_motors_bus[dynamixel-True]'
-```
-"""
-
-# TODO(rcadene): measure fps in nightly?
-# TODO(rcadene): test logs
-# TODO(rcadene): test calibration
-# TODO(rcadene): add compatibility with other motors bus
-
-import time
-
-import numpy as np
-import pytest
-
-from lerobot.common.errors import DeviceAlreadyConnectedError, DeviceNotConnectedError
-from lerobot.scripts.find_motors_bus_port import find_port
-from tests.utils import TEST_MOTOR_TYPES, make_motors_bus, require_motor
-
-
-@pytest.mark.parametrize("motor_type, mock", TEST_MOTOR_TYPES)
-@require_motor
-def test_find_port(request, motor_type, mock):
-    if mock:
-        request.getfixturevalue("patch_builtins_input")
-        with pytest.raises(OSError):
-            find_port()
-    else:
-        find_port()
-
-
-@pytest.mark.parametrize("motor_type, mock", TEST_MOTOR_TYPES)
-@require_motor
-def test_motors_bus(request, motor_type, mock):
-    if mock:
-        request.getfixturevalue("patch_builtins_input")
-
-    motors_bus = make_motors_bus(motor_type, mock=mock)
-
-    # Test reading and writing before connecting raises an error
-    with pytest.raises(DeviceNotConnectedError):
-        motors_bus.read("Torque_Enable")
-    with pytest.raises(DeviceNotConnectedError):
-        motors_bus.write("Torque_Enable", 1)
-    with pytest.raises(DeviceNotConnectedError):
-        motors_bus.disconnect()
-
-    # Test deleting the object without connecting first
-    del motors_bus
-
-    # Test connecting
-    motors_bus = make_motors_bus(motor_type, mock=mock)
-    motors_bus.connect()
-
-    # Test connecting twice raises an error
-    with pytest.raises(DeviceAlreadyConnectedError):
-        motors_bus.connect()
-
-    # Test disabling torque and reading torque on all motors
-    motors_bus.write("Torque_Enable", 0)
-    values = motors_bus.read("Torque_Enable")
-    assert isinstance(values, np.ndarray)
-    assert len(values) == len(motors_bus.motors)
-    assert (values == 0).all()
-
-    # Test writing torque on a specific motor
-    motors_bus.write("Torque_Enable", 1, "gripper")
-
-    # Test reading torque from this specific motor. It is now 1
-    values = motors_bus.read("Torque_Enable", "gripper")
-    assert len(values) == 1
-    assert values[0] == 1
-
-    # Test reading torque from all motors. It is 1 for the specific motor,
-    # and 0 on the others.
-    values = motors_bus.read("Torque_Enable")
-    gripper_index = motors_bus.motor_names.index("gripper")
-    assert values[gripper_index] == 1
-    assert values.sum() == 1  # gripper is the only motor to have torque 1
-
-    # Test writing torque on all motors and it is 1 for all.
-    motors_bus.write("Torque_Enable", 1)
-    values = motors_bus.read("Torque_Enable")
-    assert (values == 1).all()
-
-    # Test ordering the motors to move slightly (+1 value among 4096) and this move
-    # can be executed and seen by the motor position sensor
-    values = motors_bus.read("Present_Position")
-    motors_bus.write("Goal_Position", values + 1)
-    # Give time for the motors to move to the goal position
-    time.sleep(1)
-    new_values = motors_bus.read("Present_Position")
-    assert (new_values == values).all()