diff --git a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py index 5b2303ff29..4c3225ed2d 100644 --- a/src/lerobot/cameras/reachy2_camera/configuration_reachy2_camera.py +++ b/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"]: diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py index 0daeb6bbb0..773e7cc13e 100644 --- a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py +++ b/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") diff --git a/src/lerobot/record.py b/src/lerobot/record.py index de397bb847..f8db9fe28e 100644 --- a/src/lerobot/record.py +++ b/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, ) 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/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index ecc488a79e..b8ecb7f606 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 @@ -26,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", @@ -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() 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..ee0d41e3f3 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/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()