WIP

lerobot/__init__.py

@@ -28,6 +28,8 @@ Example:
         print(lerobot.available_policies)
         print(lerobot.available_policies_per_env)
         print(lerobot.available_robots)
+        print(lerobot.available_cameras)
+        print(lerobot.available_motors)
     ```
 
 When implementing a new dataset loadable with LeRobotDataset follow these steps:
@@ -198,6 +200,17 @@ available_robots = [
     "aloha",
 ]
 
+# lists all available robots from `lerobot/common/robot_devices/cameras`
+available_cameras = [
+    "opencv",
+    "intelrealsense",
+]
+
+# lists all available robots from `lerobot/common/robot_devices/motors`
+available_motors = [
+    "dynamixel",
+]
+
 # keys and values refer to yaml files
 available_policies_per_env = {
     "aloha": ["act"],

lerobot/common/robot_devices/cameras/intelrealsense.py

@@ -5,6 +5,7 @@ This file contains utilities for recording frames from Intel Realsense cameras.
 import argparse
 import concurrent.futures
 import logging
+import math
 import shutil
 import threading
 import time
@@ -156,7 +157,9 @@ class IntelRealSenseCameraConfig:
                 f"`color_mode` is expected to be 'rgb' or 'bgr', but {self.color_mode} is provided."
             )
 
-        if (self.fps or self.width or self.height) and not (self.fps and self.width and self.height):
+        at_least_one_is_not_none = self.fps is not None or self.width is not None or self.height is not None
+        at_least_one_is_none = self.fps is None or self.width is None or self.height is None
+        if at_least_one_is_not_none and at_least_one_is_none:
             raise ValueError(
                 "For `fps`, `width` and `height`, either all of them need to be set, or none of them, "
                 f"but {self.fps=}, {self.width=}, {self.height=} were provided."
@@ -260,7 +263,7 @@ class IntelRealSenseCamera:
 
         self.camera = rs.pipeline()
         try:
-            self.camera.start(config)
+            profile = self.camera.start(config)
             is_camera_open = True
         except RuntimeError:
             is_camera_open = False
@@ -279,6 +282,29 @@ class IntelRealSenseCamera:
 
             raise OSError(f"Can't access IntelRealSenseCamera({self.camera_index}).")
 
+        color_stream = profile.get_stream(rs.stream.color)
+        color_profile = color_stream.as_video_stream_profile()
+        actual_fps = color_profile.fps()
+        actual_width = color_profile.width()
+        actual_height = color_profile.height()
+
+        if self.fps is not None and not math.isclose(self.fps, actual_fps, rel_tol=1e-3):
+            raise OSError(
+                f"Can't set {self.fps=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_fps}."
+            )
+        if self.width is not None and self.width != actual_width:
+            raise OSError(
+                f"Can't set {self.width=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_width}."
+            )
+        if self.height is not None and self.height != actual_height:
+            raise OSError(
+                f"Can't set {self.height=} for IntelRealSenseCamera({self.camera_index}). Actual value is {actual_height}."
+            )
+
+        self.fps = actual_fps
+        self.width = actual_width
+        self.height = actual_height
+
         self.is_connected = True
 
     def read(self, temporary_color: str | None = None) -> np.ndarray | tuple[np.ndarray, np.ndarray]:

tests/conftest.py

@@ -18,8 +18,9 @@ import traceback
 import pytest
 
 from lerobot.common.utils.utils import init_hydra_config
-
-from .utils import DEVICE, ROBOT_CONFIG_PATH_TEMPLATE
+from tests.test_cameras import make_camera
+from tests.test_motors import make_motors_bus
+from tests.utils import DEVICE, ROBOT_CONFIG_PATH_TEMPLATE
 
 
 def pytest_collection_finish():
@@ -41,3 +42,29 @@ def is_robot_available(robot_type):
         traceback.print_exc()
         print(f"\nA {robot_type} robot is not available.")
         return False
+
+
+@pytest.fixture
+def is_camera_available(camera_type):
+    try:
+        camera = make_camera(camera_type)
+        camera.connect()
+        del camera
+        return True
+    except Exception:
+        traceback.print_exc()
+        print(f"\nA {camera_type} camera is not available.")
+        return False
+
+
+@pytest.fixture
+def is_motor_available(motor_type):
+    try:
+        motors_bus = make_motors_bus(motor_type)
+        motors_bus.connect()
+        del motors_bus
+        return True
+    except Exception:
+        traceback.print_exc()
+        print(f"\nA {motor_type} motor is not available.")
+        return False

tests/mock_dynamixel.py

@@ -0,0 +1,48 @@
+from dynamixel_sdk import COMM_SUCCESS
+
+
+class PortHandler:
+    def __init__(self, port):
+        self.port = port
+
+    def openPort(self):  # noqa: N802
+        return True
+
+    def closePort(self):  # noqa: N802
+        pass
+
+    def setPacketTimeoutMillis(self, timeout_ms):  # noqa: N802
+        del timeout_ms  # unused
+
+
+class PacketHandler:
+    def __init__(self, protocol_version):
+        del protocol_version  # unused
+
+
+class GroupSyncRead:
+    def __init__(self, port_handler, packet_handler, address, bytes):
+        pass
+
+    def addParam(self, motor_index):  # noqa: N802
+        pass
+
+    def txRxPacket(self):  # noqa: N802
+        return COMM_SUCCESS
+
+    def getData(self, index, address, bytes):  # noqa: N802
+        return value  # noqa: F821
+
+
+class GroupSyncWrite:
+    def __init__(self, port_handler, packet_handler, address, bytes):
+        pass
+
+    def addParam(self, index, data):  # noqa: N802
+        pass
+
+    def txPacket(self):  # noqa: N802
+        return COMM_SUCCESS
+
+    def changeParam(self, index, data):  # noqa: N802
+        pass

tests/mock_intelrealsense.py

@@ -0,0 +1,110 @@
+import enum
+
+import numpy as np
+
+
+class MockStream(enum.Enum):
+    color = 0
+    depth = 1
+
+
+class MockFormat(enum.Enum):
+    rgb8 = 0
+    z16 = 1
+
+
+class MockConfig:
+    def enable_device(self, device_id: str):
+        self.device_enabled = device_id
+
+    def enable_stream(
+        self, stream_type: MockStream, width=None, height=None, color_format: MockFormat = None, fps=None
+    ):
+        self.stream_type = stream_type
+        # Overwrite default values when possible
+        self.width = 848 if width is None else width
+        self.height = 480 if height is None else height
+        self.color_format = MockFormat.rgb8 if color_format is None else color_format
+        self.fps = 30 if fps is None else fps
+
+
+class MockColorProfile:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def fps(self):
+        return self.config.fps
+
+    def width(self):
+        return self.config.width
+
+    def height(self):
+        return self.config.height
+
+
+class MockColorStream:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def as_video_stream_profile(self):
+        return MockColorProfile(self.config)
+
+
+class MockProfile:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def get_stream(self, color_format: MockFormat):
+        del color_format  # unused
+        return MockColorStream(self.config)
+
+
+class MockPipeline:
+    def __init__(self):
+        self.started = False
+        self.config = None
+
+    def start(self, config: MockConfig):
+        self.started = True
+        self.config = config
+        return MockProfile(self.config)
+
+    def stop(self):
+        if not self.started:
+            raise RuntimeError("You need to start the camera before stop.")
+        self.started = False
+        self.config = None
+
+    def wait_for_frames(self, timeout_ms=50000):
+        del timeout_ms  # unused
+        return MockFrames(self.config)
+
+
+class MockFrames:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def get_color_frame(self):
+        return MockColorFrame(self.config)
+
+    def get_depth_frame(self):
+        return MockDepthFrame(self.config)
+
+
+class MockColorFrame:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def get_data(self):
+        data = np.ones((self.config.height, self.config.width, 3), dtype=np.uint8)
+        # Create a difference between rgb and bgr
+        data[:, :, 0] = 2
+        return data
+
+
+class MockDepthFrame:
+    def __init__(self, config: MockConfig):
+        self.config = config
+
+    def get_data(self):
+        return np.ones((self.config.height, self.config.width), dtype=np.uint16)

tests/test_cameras.py

@@ -1,25 +1,34 @@
 """
-Tests meant to be used locally and launched manually.
+Tests for physical cameras and their mocked versions.
+If the physical camera is not connected to the computer, or not working,
+the test will be skipped.
 
-Example usage:
+Example of running a specific test:
 ```bash
 pytest -sx tests/test_cameras.py::test_camera
 ```
 
-Example of running test on mocked_koch:
+Example of running test on a real OpenCV camera connected to the computer:
 ```bash
-python -m pytest -sx -k "mocked_koch" tests/test_cameras.py::test_camera
+pytest -sx tests/test_cameras.py::test_camera[opencv]
+```
+
+Example of running test on a mocked version of an OpenCV camera:
+```bash
+pytest -sx -k "mocked_opencv" tests/test_cameras.py::test_camera
 ```
 """
 
 import numpy as np
 import pytest
 
-from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera, save_images_from_cameras
 from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from tests.utils import TEST_ROBOT_TYPES, require_robot
+from tests.utils import TEST_CAMERA_TYPES, require_camera
+
+# Camera indices used for connecting physical cameras
+OPENCV_CAMERA_INDEX = 0
+INTELREALSENSE_CAMERA_INDEX = 128422271614
 
-CAMERA_INDEX = 2
 # Maximum absolute difference between two consecutive images recored by a camera.
 # This value differs with respect to the camera.
 MAX_PIXEL_DIFFERENCE = 25
@@ -29,9 +38,26 @@ def compute_max_pixel_difference(first_image, second_image):
     return np.abs(first_image.astype(float) - second_image.astype(float)).max()
 
 
-@pytest.mark.parametrize("robot_type", TEST_ROBOT_TYPES)
-@require_robot
-def test_camera(request, robot_type):
+def make_camera(camera_type, **kwargs):
+    if camera_type == "opencv":
+        from lerobot.common.robot_devices.cameras.opencv import OpenCVCamera
+
+        camera_index = kwargs.pop("camera_index", OPENCV_CAMERA_INDEX)
+        return OpenCVCamera(camera_index, **kwargs)
+
+    elif camera_type == "intelrealsense":
+        from lerobot.common.robot_devices.cameras.intelrealsense import IntelRealSenseCamera
+
+        camera_index = kwargs.pop("camera_index", INTELREALSENSE_CAMERA_INDEX)
+        return IntelRealSenseCamera(camera_index, **kwargs)
+
+    else:
+        raise ValueError(f"The camera type '{camera_type}' is not valid.")
+
+
+@pytest.mark.parametrize("camera_type", TEST_CAMERA_TYPES)
+@require_camera
+def test_camera(request, camera_type):
     """Test assumes that `camera.read()` returns the same image when called multiple times in a row.
     So the environment should not change (you shouldnt be in front of the camera) and the camera should not be moving.
 
@@ -40,10 +66,9 @@ def test_camera(request, robot_type):
     """
     # TODO(rcadene): measure fps in nightly?
     # TODO(rcadene): test logs
-    # TODO(rcadene): add compatibility with other camera APIs
 
     # Test instantiating
-    camera = OpenCVCamera(CAMERA_INDEX)
+    camera = make_camera(camera_type)
 
     # Test reading, async reading, disconnecting before connecting raises an error
     with pytest.raises(RobotDeviceNotConnectedError):
@@ -57,7 +82,7 @@ def test_camera(request, robot_type):
     del camera
 
     # Test connecting
-    camera = OpenCVCamera(CAMERA_INDEX)
+    camera = make_camera(camera_type)
     camera.connect()
     assert camera.is_connected
     assert camera.fps is not None
@@ -94,12 +119,12 @@ def test_camera(request, robot_type):
     assert camera.thread is None
 
     # Test disconnecting with `__del__`
-    camera = OpenCVCamera(CAMERA_INDEX)
+    camera = make_camera(camera_type)
     camera.connect()
     del camera
 
     # Test acquiring a bgr image
-    camera = OpenCVCamera(CAMERA_INDEX, color_mode="bgr")
+    camera = make_camera(camera_type, color_mode="bgr")
     camera.connect()
     assert camera.color_mode == "bgr"
     bgr_color_image = camera.read()
@@ -110,7 +135,7 @@ def test_camera(request, robot_type):
     # TODO(rcadene): Add a test for a camera that supports fps=60
 
     # Test width and height can be set
-    camera = OpenCVCamera(CAMERA_INDEX, fps=30, width=1280, height=720)
+    camera = make_camera(camera_type, fps=30, width=1280, height=720)
     camera.connect()
     assert camera.fps == 30
     assert camera.width == 1280
@@ -123,13 +148,19 @@ def test_camera(request, robot_type):
     del camera
 
     # Test not supported width and height raise an error
-    camera = OpenCVCamera(CAMERA_INDEX, fps=30, width=0, height=0)
+    camera = make_camera(camera_type, fps=30, width=0, height=0)
     with pytest.raises(OSError):
         camera.connect()
     del camera
 
 
-@pytest.mark.parametrize("robot_type", TEST_ROBOT_TYPES)
-@require_robot
-def test_save_images_from_cameras(tmpdir, request, robot_type):
+@pytest.mark.parametrize("camera_type", TEST_CAMERA_TYPES)
+@require_camera
+def test_save_images_from_cameras(tmpdir, request, camera_type):
+    # TODO(rcadene): refactor
+    if camera_type == "opencv":
+        from lerobot.common.robot_devices.cameras.opencv import save_images_from_cameras
+    elif camera_type == "intelrealsense":
+        from lerobot.common.robot_devices.cameras.intelrealsense import save_images_from_cameras
+
     save_images_from_cameras(tmpdir, record_time_s=1)

tests/test_motors.py

@@ -1,11 +1,24 @@
 """
-Tests meant to be used locally and launched manually.
+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 usage:
+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]
+```
+
+Example of running test on a mocked version of dynamixel motors:
+```bash
+pytest -sx -k "mocked_dynamixel" tests/test_motors.py::test_motors_bus
+```
 """
 
 # TODO(rcadene): measure fps in nightly?
@@ -18,34 +31,46 @@ import time
 import numpy as np
 import pytest
 
-from lerobot import available_robots
+from lerobot import available_motors
 from lerobot.common.robot_devices.motors.utils import MotorsBus
-from lerobot.common.robot_devices.robots.factory import make_robot
 from lerobot.common.robot_devices.utils import RobotDeviceAlreadyConnectedError, RobotDeviceNotConnectedError
-from lerobot.common.utils.utils import init_hydra_config
-from tests.utils import ROBOT_CONFIG_PATH_TEMPLATE, require_robot
+from tests.utils import TEST_MOTOR_TYPES, require_motor
+
+DYNAMIXEL_PORT = "/dev/tty.usbmodem575E0032081"
+DYNAMIXEL_MOTORS = {
+    "shoulder_pan": [1, "xl430-w250"],
+    "shoulder_lift": [2, "xl430-w250"],
+    "elbow_flex": [3, "xl330-m288"],
+    "wrist_flex": [4, "xl330-m288"],
+    "wrist_roll": [5, "xl330-m288"],
+    "gripper": [6, "xl330-m288"],
+}
 
 
-def make_motors_bus(robot_type: str) -> MotorsBus:
-    # Instantiate a robot and return one of its leader arms
-    config_path = ROBOT_CONFIG_PATH_TEMPLATE.format(robot=robot_type)
-    robot_cfg = init_hydra_config(config_path)
-    robot = make_robot(robot_cfg)
-    first_bus_name = list(robot.leader_arms.keys())[0]
-    motors_bus = robot.leader_arms[first_bus_name]
-    return motors_bus
+def make_motors_bus(motor_type: str, **kwargs) -> MotorsBus:
+    if motor_type == "dynamixel":
+        from lerobot.common.robot_devices.motors.dynamixel import DynamixelMotorsBus
+
+        port = kwargs.pop("port", DYNAMIXEL_PORT)
+        motors = kwargs.pop("motors", DYNAMIXEL_MOTORS)
+        return DynamixelMotorsBus(port, motors, **kwargs)
+
+    else:
+        raise ValueError(f"The motor type '{motor_type}' is not valid.")
 
 
-@pytest.mark.parametrize("robot_type", available_robots)
-@require_robot
+# TODO(rcadene): implement mocked version of this test
+@pytest.mark.parametrize("motor_type", available_motors)
+@require_motor
 def test_find_port(request, robot_type):
     from lerobot.common.robot_devices.motors.dynamixel import find_port
 
     find_port()
 
 
-@pytest.mark.parametrize("robot_type", available_robots)
-@require_robot
+# TODO(rcadene): implement mocked version of this test
+@pytest.mark.parametrize("motor_type", available_motors)
+@require_motor
 def test_configure_motors_all_ids_1(request, robot_type):
     input("Are you sure you want to re-configure the motors? Press enter to continue...")
     # This test expect the configuration was already correct.
@@ -63,8 +88,8 @@ def test_configure_motors_all_ids_1(request, robot_type):
     del motors_bus
 
 
-@pytest.mark.parametrize("robot_type", available_robots)
-@require_robot
+@pytest.mark.parametrize("motor_type", TEST_MOTOR_TYPES)
+@require_motor
 def test_motors_bus(request, robot_type):
     motors_bus = make_motors_bus(robot_type)
 

tests/utils.py

@@ -20,7 +20,7 @@ from functools import wraps
 import pytest
 import torch
 
-from lerobot import available_robots
+from lerobot import available_cameras, available_motors, available_robots
 from lerobot.common.utils.import_utils import is_package_available
 
 DEVICE = "cuda" if torch.cuda.is_available() else "cpu"
@@ -31,6 +31,8 @@ DEFAULT_CONFIG_PATH = "lerobot/configs/default.yaml"
 ROBOT_CONFIG_PATH_TEMPLATE = "lerobot/configs/robot/{robot}.yaml"
 
 TEST_ROBOT_TYPES = available_robots + [f"mocked_{robot_type}" for robot_type in available_robots]
+TEST_CAMERA_TYPES = available_cameras + [f"mocked_{camera_type}" for camera_type in available_cameras]
+TEST_MOTOR_TYPES = available_motors + [f"mocked_{motor_type}" for motor_type in available_motors]
 
 
 def require_x86_64_kernel(func):
@@ -178,32 +180,22 @@ def require_robot(func):
         request = kwargs.get("request")
         robot_type = kwargs.get("robot_type")
 
+        if robot_type is None:
+            raise ValueError("The 'robot_type' must be an argument of the test function.")
+
+        if robot_type not in TEST_ROBOT_TYPES:
+            raise ValueError(
+                f"The camera type '{robot_type}' is not valid. Expected one of these '{TEST_ROBOT_TYPES}"
+            )
+
         if request is None:
             raise ValueError("The 'request' fixture must be an argument of the test function.")
 
         # Run test with a monkeypatched version of the robot devices.
         if robot_type.startswith("mocked_"):
             kwargs["robot_type"] = robot_type.replace("mocked_", "")
-
-            monkeypatch = request.getfixturevalue("monkeypatch")
-
-            try:
-                import cv2
-
-                from tests.mock_opencv import MockVideoCapture
-
-                monkeypatch.setattr(cv2, "VideoCapture", MockVideoCapture)
-            except ImportError:
-                traceback.print_exc()
-
-            try:
-                import pyrealsense2 as rs
-
-                # TODO(rcadene):
-                mock_pipeline = None
-                monkeypatch.setattr(rs, "pipeline", mock_pipeline)
-            except ImportError:
-                traceback.print_exc()
+            mock_cameras(request)
+            mock_motors(request)
 
         # Run test with a real robot. Skip test if robot connection fails.
         else:
@@ -214,3 +206,117 @@ def require_robot(func):
         return func(*args, **kwargs)
 
     return wrapper
+
+
+def require_camera(func):
+    @wraps(func)
+    def wrapper(*args, **kwargs):
+        # Access the pytest request context to get the is_camera_available fixture
+        request = kwargs.get("request")
+        camera_type = kwargs.get("camera_type")
+
+        if camera_type is None:
+            raise ValueError("The 'camera_type' must be an argument of the test function.")
+
+        if camera_type not in TEST_CAMERA_TYPES:
+            raise ValueError(
+                f"The camera type '{camera_type}' is not valid. Expected one of these '{TEST_CAMERA_TYPES}"
+            )
+
+        if request is None:
+            raise ValueError("The 'request' fixture must be an argument of the test function.")
+
+        # Run test with a monkeypatched version of the robot devices.
+        if camera_type.startswith("mocked_"):
+            kwargs["camera_type"] = camera_type.replace("mocked_", "")
+            mock_cameras(request)
+
+        # Run test with a real robot. Skip test if robot connection fails.
+        else:
+            # `is_camera_available` is defined in `tests/conftest.py`
+            if not request.getfixturevalue("is_camera_available"):
+                pytest.skip(f"A {camera_type} camera is not available.")
+
+        return func(*args, **kwargs)
+
+    return wrapper
+
+
+def require_motor(func):
+    @wraps(func)
+    def wrapper(*args, **kwargs):
+        # Access the pytest request context to get the is_motor_available fixture
+        request = kwargs.get("request")
+        motor_type = kwargs.get("motor_type")
+
+        if motor_type is None:
+            raise ValueError("The 'motor_type' must be an argument of the test function.")
+
+        if motor_type not in TEST_MOTOR_TYPES:
+            raise ValueError(
+                f"The motor type '{motor_type}' is not valid. Expected one of these '{TEST_MOTOR_TYPES}"
+            )
+
+        if request is None:
+            raise ValueError("The 'request' fixture must be an argument of the test function.")
+
+        # Run test with a monkeypatched version of the robot devices.
+        if motor_type.startswith("mocked_"):
+            kwargs["motor_type"] = motor_type.replace("mocked_", "")
+            mock_motors(request)
+
+        # Run test with a real robot. Skip test if robot connection fails.
+        else:
+            # `is_motor_available` is defined in `tests/conftest.py`
+            if not request.getfixturevalue("is_motor_available"):
+                pytest.skip(f"A {motor_type} motor is not available.")
+
+        return func(*args, **kwargs)
+
+    return wrapper
+
+
+def mock_cameras(request):
+    monkeypatch = request.getfixturevalue("monkeypatch")
+
+    try:
+        import cv2
+
+        from tests.mock_opencv import MockVideoCapture
+
+        monkeypatch.setattr(cv2, "VideoCapture", MockVideoCapture)
+    except ImportError:
+        traceback.print_exc()
+
+    try:
+        import pyrealsense2 as rs
+
+        from tests.mock_intelrealsense import MockConfig, MockFormat, MockPipeline, MockStream
+
+        monkeypatch.setattr(rs, "config", MockConfig)
+        monkeypatch.setattr(rs, "pipeline", MockPipeline)
+        monkeypatch.setattr(rs, "stream", MockStream)
+        monkeypatch.setattr(rs, "format", MockFormat)
+    except ImportError:
+        traceback.print_exc()
+
+
+def mock_motors(request):
+    monkeypatch = request.getfixturevalue("monkeypatch")
+
+    try:
+        import dynamixel_sdk
+
+        from tests.mock_dynamixel import (
+            MockGroupSyncRead,
+            MockGroupSyncWrite,
+            MockPacketHandler,
+            MockPortHandler,
+        )
+
+        monkeypatch.setattr(dynamixel_sdk, "GroupSyncRead", MockGroupSyncRead)
+        monkeypatch.setattr(dynamixel_sdk, "GroupSyncWrite", MockGroupSyncWrite)
+        monkeypatch.setattr(dynamixel_sdk, "PacketHandler", MockPacketHandler)
+        monkeypatch.setattr(dynamixel_sdk, "PortHandler", MockPortHandler)
+    except ImportError:
+        traceback.print_exc()