fix(robots): fixes for reachy

src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py

@@ -16,9 +16,13 @@
 
 from ..configs import CameraConfig, ColorMode
 
+REACHY2_CAMERA_FPS = 15
+REACHY2_CAMERA_WIDTH = 640
+REACHY2_CAMERA_HEIGHT = 480
+
 
 @CameraConfig.register_subclass("reachy2_camera")
-@dataclass
+@dataclass(kw_only=True)
 class Reachy2CameraConfig(CameraConfig):
     """Configuration class for Reachy 2 camera devices.
 
@@ -57,10 +61,15 @@ class Reachy2CameraConfig(CameraConfig):
 
     name: str
     image_type: str
+
     color_mode: ColorMode = ColorMode.RGB
     ip_address: str | None = "localhost"
     port: int = 50065
-    # use_depth: bool = False
+
+    # TODO(maractingi):  verify if reachy2 camera supports different fps, width, and height
+    fps: int | None = REACHY2_CAMERA_FPS
+    width: int | None = REACHY2_CAMERA_WIDTH
+    height: int | None = REACHY2_CAMERA_HEIGHT
 
     def __post_init__(self):
         if self.name not in ["teleop", "depth"]:

src/lerobot/cameras/reachy2_camera/reachy2_camera.py

@@ -28,13 +28,17 @@
     os.environ["OPENCV_VIDEOIO_MSMF_ENABLE_HW_TRANSFORMS"] = "0"
 import cv2
 import numpy as np
-from reachy2_sdk.media.camera import CameraView
-from reachy2_sdk.media.camera_manager import CameraManager
+
+try:
+    from reachy2_sdk.media.camera import CameraView
+    from reachy2_sdk.media.camera_manager import CameraManager
+except Exception as e:
+    logging.info(f"Could not import ReachSDK: {e}")
 
 from lerobot.errors import DeviceNotConnectedError
 
 from ..camera import Camera
-from .configuration_reachy2_camera import ColorMode, Reachy2CameraConfig
+from .configuration_reachy2_camera import REACHY2_CAMERA_FPS, ColorMode, Reachy2CameraConfig
 
 logger = logging.getLogger(__name__)
 
@@ -65,7 +69,6 @@ def __init__(self, config: Reachy2CameraConfig):
 
         self.config = config
 
-        self.fps = config.fps
         self.color_mode = config.color_mode
 
         self.cam_manager: CameraManager | None = None
@@ -123,7 +126,7 @@ def find_cameras(ip_address: str = "localhost", port: int = 50065) -> list[dict[
                 "default_profile": {
                     "width": width,
                     "height": height,
-                    "fps": 30,
+                    "fps": REACHY2_CAMERA_FPS,
                 },
             }
             initialized_cameras.append(camera_info)
@@ -153,26 +156,26 @@ def read(self, color_mode: ColorMode | None = None) -> np.ndarray:
         start_time = time.perf_counter()
 
         frame = None
+        if self.config.name == "teleop" and hasattr(self.cam_manager, "teleop"):
+            if self.config.image_type == "left":
+                frame = self.cam_manager.teleop.get_frame(
+                    CameraView.LEFT, size=(self.config.width, self.config.height)
+                )[0]
+            elif self.config.image_type == "right":
+                frame = self.cam_manager.teleop.get_frame(
+                    CameraView.RIGHT, size=(self.config.width, self.config.height)
+                )[0]
+        elif self.config.name == "depth" and hasattr(self.cam_manager, "depth"):
+            if self.config.image_type == "depth":
+                frame = self.cam_manager.depth.get_depth_frame()[0]
+            elif self.config.image_type == "rgb":
+                frame = self.cam_manager.depth.get_frame(size=(self.config.width, self.config.height))[0]
 
-        if self.cam_manager is None:
-            raise DeviceNotConnectedError(f"{self} is not connected.")
-        else:
-            if self.config.name == "teleop" and hasattr(self.cam_manager, "teleop"):
-                if self.config.image_type == "left":
-                    frame = self.cam_manager.teleop.get_frame(CameraView.LEFT, size=(640, 480))[0]
-                elif self.config.image_type == "right":
-                    frame = self.cam_manager.teleop.get_frame(CameraView.RIGHT, size=(640, 480))[0]
-            elif self.config.name == "depth" and hasattr(self.cam_manager, "depth"):
-                if self.config.image_type == "depth":
-                    frame = self.cam_manager.depth.get_depth_frame()[0]
-                elif self.config.image_type == "rgb":
-                    frame = self.cam_manager.depth.get_frame(size=(640, 480))[0]
-
-            if frame is None:
-                return np.empty((0, 0, 3), dtype=np.uint8)
-
-            if self.config.color_mode == "rgb":
-                frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
+        if frame is None:
+            return np.empty((0, 0, 3), dtype=np.uint8)
+
+        if self.config.color_mode == "rgb":
+            frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
 
         read_duration_ms = (time.perf_counter() - start_time) * 1e3
         logger.debug(f"{self} read took: {read_duration_ms:.1f}ms")

src/lerobot/record.py

@@ -67,6 +67,7 @@
     CameraConfig,  # noqa: F401
 )
 from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig  # noqa: F401
+from lerobot.cameras.reachy2_camera.configuration_reachy2_camera import Reachy2CameraConfig  # noqa: F401
 from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig  # noqa: F401
 from lerobot.configs import parser
 from lerobot.configs.policies import PreTrainedConfig
@@ -83,6 +84,7 @@
     hope_jr,
     koch_follower,
     make_robot_from_config,
+    reachy2,
     so100_follower,
     so101_follower,
 )
@@ -93,6 +95,7 @@
     homunculus,
     koch_leader,
     make_teleoperator_from_config,
+    reachy2_teleoperator,
     so100_leader,
     so101_leader,
 )

src/lerobot/robots/reachy2/configuration_reachy2.py

@@ -65,29 +65,20 @@ def __post_init__(self) -> None:
                 name="teleop",
                 image_type="left",
                 ip_address=self.ip_address,
-                fps=15,
-                width=640,
-                height=480,
                 color_mode=ColorMode.RGB,
             )
         if self.with_right_teleop_camera:
             self.cameras["teleop_right"] = Reachy2CameraConfig(
                 name="teleop",
                 image_type="right",
                 ip_address=self.ip_address,
-                fps=15,
-                width=640,
-                height=480,
                 color_mode=ColorMode.RGB,
             )
         if self.with_torso_camera:
             self.cameras["torso_rgb"] = Reachy2CameraConfig(
                 name="depth",
                 image_type="rgb",
                 ip_address=self.ip_address,
-                fps=15,
-                width=640,
-                height=480,
                 color_mode=ColorMode.RGB,
             )
 

src/lerobot/robots/reachy2/robot_reachy2.py

@@ -18,14 +18,18 @@
 from typing import Any
 
 import numpy as np
-from reachy2_sdk import ReachySDK
 
 from lerobot.cameras.utils import make_cameras_from_configs
 
 from ..robot import Robot
 from ..utils import ensure_safe_goal_position
 from .configuration_reachy2 import Reachy2RobotConfig
 
+try:
+    from reachy2_sdk import ReachySDK
+except ImportError:
+    ReachySDK = None
+
 # {lerobot_keys: reachy2_sdk_keys}
 REACHY2_NECK_JOINTS = {
     "neck_yaw.pos": "head.neck.yaw",
@@ -77,6 +81,9 @@ class Reachy2Robot(Robot):
 
     def __init__(self, config: Reachy2RobotConfig):
         super().__init__(config)
+        if ReachySDK is None:
+            raise ImportError("reachy2_sdk is required to use Reachy2Robot")
+        self._sdk = ReachySDK
 
         self.config = config
         self.robot_type = self.config.type
@@ -122,7 +129,7 @@ def is_connected(self) -> bool:
         return self.reachy.is_connected() if self.reachy is not None else False
 
     def connect(self, calibrate: bool = False) -> None:
-        self.reachy = ReachySDK(self.config.ip_address)
+        self.reachy = self._sdk(self.config.ip_address)
         if not self.is_connected:
             raise ConnectionError()
 

src/lerobot/teleoperate.py

@@ -68,6 +68,7 @@
     hope_jr,
     koch_follower,
     make_robot_from_config,
+    reachy2,
     so100_follower,
     so101_follower,
 )
@@ -79,6 +80,7 @@
     homunculus,
     koch_leader,
     make_teleoperator_from_config,
+    reachy2_teleoperator,
     so100_leader,
     so101_leader,
 )

src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py

@@ -17,13 +17,16 @@
 import logging
 import time
 
-from reachy2_sdk import ReachySDK
-
 from ..teleoperator import Teleoperator
 from .config_reachy2_teleoperator import Reachy2TeleoperatorConfig
 
 logger = logging.getLogger(__name__)
 
+try:
+    from reachy2_sdk import ReachySDK
+except ImportError:
+    ReachySDK = None
+
 # {lerobot_keys: reachy2_sdk_keys}
 REACHY2_NECK_JOINTS = {
     "neck_yaw.pos": "head.neck.yaw",
@@ -74,6 +77,9 @@ class Reachy2Teleoperator(Teleoperator):
     name = "reachy2_specific"
 
     def __init__(self, config: Reachy2TeleoperatorConfig):
+        if ReachySDK is None:
+            raise ImportError("reachy2_sdk is required to use Reachy2Teleoperator")
+        self._sdk = ReachySDK
         super().__init__(config)
         self.config = config
         self.reachy: None | ReachySDK = None
@@ -117,7 +123,7 @@ def is_connected(self) -> bool:
         return self.reachy.is_connected() if self.reachy is not None else False
 
     def connect(self, calibrate: bool = True) -> None:
-        self.reachy = ReachySDK(self.config.ip_address)
+        self.reachy = self._sdk(self.config.ip_address)
         if not self.is_connected:
             raise ConnectionError()
         logger.info(f"{self} connected.")
@@ -135,7 +141,10 @@ def configure(self) -> None:
     def get_action(self) -> dict[str, float]:
         start = time.perf_counter()
 
-        if self.reachy and self.is_connected:
+        joint_action = {}
+        vel_action = {}
+
+        if self.is_connected:
             if self.config.use_present_position:
                 joint_action = {
                     k: self.reachy.joints[v].present_position for k, v in self.joints_dict.items()
@@ -152,6 +161,7 @@ def get_action(self) -> dict[str, float]:
                 vel_action = {k: self.reachy.mobile_base.odometry[v] for k, v in REACHY2_VEL.items()}
             else:
                 vel_action = {k: self.reachy.mobile_base.last_cmd_vel[v] for k, v in REACHY2_VEL.items()}
+
         dt_ms = (time.perf_counter() - start) * 1e3
         logger.debug(f"{self} read action: {dt_ms:.1f}ms")
         return {**joint_action, **vel_action}
@@ -160,5 +170,5 @@ def send_feedback(self, feedback: dict[str, float]) -> None:
         raise NotImplementedError
 
     def disconnect(self) -> None:
-        if self.reachy and self.is_connected:
+        if self.is_connected:
             self.reachy.disconnect()