From 480d20f65290285a7210dc7471bee6c615bc7b24 Mon Sep 17 00:00:00 2001 From: Steven Palma Date: Fri, 5 Sep 2025 14:50:22 +0200 Subject: [PATCH 1/3] fix(robots): import fixes for reachy --- src/lerobot/cameras/reachy2_camera/reachy2_camera.py | 8 ++++++-- src/lerobot/record.py | 4 ++++ src/lerobot/robots/reachy2/robot_reachy2.py | 6 ++++-- src/lerobot/teleoperate.py | 2 ++ .../reachy2_teleoperator/reachy2_teleoperator.py | 7 ++++--- 5 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py index 0daeb6bbb0..b917c650bf 100644 --- a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py @@ -28,8 +28,12 @@ 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 diff --git a/src/lerobot/record.py b/src/lerobot/record.py index 8eee59558b..f8db9fe28e 100644 --- a/src/lerobot/record.py +++ b/src/lerobot/record.py @@ -67,6 +67,8 @@ 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 from lerobot.datasets.image_writer import safe_stop_image_writer @@ -82,6 +84,7 @@ hope_jr, koch_follower, make_robot_from_config, + reachy2, so100_follower, so101_follower, ) @@ -92,6 +95,7 @@ homunculus, koch_leader, make_teleoperator_from_config, + reachy2_teleoperator, so100_leader, so101_leader, ) diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index ecc488a79e..444bf5dbf7 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -18,7 +18,6 @@ from typing import Any import numpy as np -from reachy2_sdk import ReachySDK from lerobot.cameras.utils import make_cameras_from_configs @@ -76,7 +75,10 @@ class Reachy2Robot(Robot): name = "reachy2" def __init__(self, config: Reachy2RobotConfig): + from reachy2_sdk import ReachySDK + super().__init__(config) + self._sdk = ReachySDK self.config = config self.robot_type = self.config.type @@ -122,7 +124,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() diff --git a/src/lerobot/teleoperate.py b/src/lerobot/teleoperate.py index e7be6967ba..4770fee3f7 100644 --- a/src/lerobot/teleoperate.py +++ b/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, ) diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py index 5a427dd715..43a1d31000 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py @@ -17,8 +17,6 @@ import logging import time -from reachy2_sdk import ReachySDK - from ..teleoperator import Teleoperator from .config_reachy2_teleoperator import Reachy2TeleoperatorConfig @@ -74,6 +72,9 @@ class Reachy2Teleoperator(Teleoperator): name = "reachy2_specific" def __init__(self, config: Reachy2TeleoperatorConfig): + from reachy2_sdk import ReachySDK + + self._sdk = ReachySDK super().__init__(config) self.config = config self.reachy: None | ReachySDK = None @@ -117,7 +118,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.") From 2065ad5df1b853d0a5ca17d459171de646515b68 Mon Sep 17 00:00:00 2001 From: Michel Aractingi Date: Tue, 9 Sep 2025 22:58:33 +0200 Subject: [PATCH 2/3] added default globals for reachy 2 camera width, height, fps; removed redundant condition on is_connected and reachy not None in reachy2 robot --- .../configuration_reachy2_camera.py | 10 ++++- .../cameras/reachy2_camera/reachy2_camera.py | 43 +++++++++---------- .../robots/reachy2/configuration_reachy2.py | 9 ---- .../reachy2_teleoperator.py | 4 +- 4 files changed, 32 insertions(+), 34 deletions(-) diff --git a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py index 5b2303ff29..d5a2a4bb47 100644 --- a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py @@ -16,6 +16,10 @@ from ..configs import CameraConfig, ColorMode +REACHY2_CAMERA_FPS = 15 +REACHY2_CAMERA_WIDTH = 640 +REACHY2_CAMERA_HEIGHT = 480 + @CameraConfig.register_subclass("reachy2_camera") @dataclass @@ -60,7 +64,11 @@ class Reachy2CameraConfig(CameraConfig): 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"]: diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py index b917c650bf..773e7cc13e 100644 --- a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py @@ -38,7 +38,7 @@ 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__) @@ -69,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 @@ -127,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) @@ -157,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") diff --git a/src/lerobot/robots/reachy2/configuration_reachy2.py b/src/lerobot/robots/reachy2/configuration_reachy2.py index aa25351c64..2596f73c56 100644 --- a/src/lerobot/robots/reachy2/configuration_reachy2.py +++ b/src/lerobot/robots/reachy2/configuration_reachy2.py @@ -65,9 +65,6 @@ 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: @@ -75,9 +72,6 @@ def __post_init__(self) -> None: 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: @@ -85,9 +79,6 @@ def __post_init__(self) -> None: name="depth", image_type="rgb", ip_address=self.ip_address, - fps=15, - width=640, - height=480, color_mode=ColorMode.RGB, ) diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py index 43a1d31000..15ebdef481 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py @@ -136,7 +136,7 @@ def configure(self) -> None: def get_action(self) -> dict[str, float]: start = time.perf_counter() - if self.reachy and self.is_connected: + 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() @@ -161,5 +161,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() From e7b8fc011121cd8fd1938b5a46587f5ee65f37a5 Mon Sep 17 00:00:00 2001 From: Michel Aractingi Date: Tue, 9 Sep 2025 23:36:32 +0200 Subject: [PATCH 3/3] fix reachy tests fix tests reachy teleoperator --- .../reachy2_camera/configuration_reachy2_camera.py | 3 ++- src/lerobot/robots/reachy2/robot_reachy2.py | 9 +++++++-- .../reachy2_teleoperator/reachy2_teleoperator.py | 13 +++++++++++-- 3 files changed, 20 insertions(+), 5 deletions(-) diff --git a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py index d5a2a4bb47..4c3225ed2d 100644 --- a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py @@ -22,7 +22,7 @@ @CameraConfig.register_subclass("reachy2_camera") -@dataclass +@dataclass(kw_only=True) class Reachy2CameraConfig(CameraConfig): """Configuration class for Reachy 2 camera devices. @@ -61,6 +61,7 @@ class Reachy2CameraConfig(CameraConfig): name: str image_type: str + color_mode: ColorMode = ColorMode.RGB ip_address: str | None = "localhost" port: int = 50065 diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index 444bf5dbf7..b8ecb7f606 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -25,6 +25,11 @@ 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", @@ -75,9 +80,9 @@ class Reachy2Robot(Robot): name = "reachy2" def __init__(self, config: Reachy2RobotConfig): - from reachy2_sdk import ReachySDK - super().__init__(config) + if ReachySDK is None: + raise ImportError("reachy2_sdk is required to use Reachy2Robot") self._sdk = ReachySDK self.config = config diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py index 15ebdef481..ee0d41e3f3 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py @@ -22,6 +22,11 @@ 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", @@ -72,8 +77,8 @@ class Reachy2Teleoperator(Teleoperator): name = "reachy2_specific" def __init__(self, config: Reachy2TeleoperatorConfig): - from reachy2_sdk import ReachySDK - + if ReachySDK is None: + raise ImportError("reachy2_sdk is required to use Reachy2Teleoperator") self._sdk = ReachySDK super().__init__(config) self.config = config @@ -136,6 +141,9 @@ def configure(self) -> None: def get_action(self) -> dict[str, float]: start = time.perf_counter() + joint_action = {} + vel_action = {} + if self.is_connected: if self.config.use_present_position: joint_action = { @@ -153,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}