diff --git a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py index e3d0f4c7b..09a66b09e 100644 --- a/src/lerobot/cameras/reachy2_camera/reachy2_camera.py +++ b/src/lerobot/cameras/reachy2_camera/reachy2_camera.py @@ -139,14 +139,14 @@ class Reachy2Camera(Camera): 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)[0] + 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)[0] + 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()[0] + frame = self.cam_manager.depth.get_frame(size=(640, 480))[0] if frame is None: return None diff --git a/src/lerobot/robots/reachy2/configuration_reachy2.py b/src/lerobot/robots/reachy2/configuration_reachy2.py index 350a02eaa..3a2743787 100644 --- a/src/lerobot/robots/reachy2/configuration_reachy2.py +++ b/src/lerobot/robots/reachy2/configuration_reachy2.py @@ -29,19 +29,35 @@ class Reachy2RobotConfig(RobotConfig): # the number of motors in your follower arms. max_relative_target: int | None = None ip_address: str | None = "localhost" + + # Tag for external commands control + # Set to True if you use an external commands system to control the robot, + # such as the official teleoperation application: https://github.com/pollen-robotics/Reachy2Teleoperation use_external_commands: bool = False + + # Robot parts + # Set to False to not add the corresponding joints part to the robot list of joints. + # By default, all parts are set to True. with_mobile_base: bool = True with_l_arm: bool = True with_r_arm: bool = True with_neck: bool = True with_antennas: bool = True + # Robot cameras + # Set to True if you want to use the corresponding cameras in the observations. + # By default, only the teleop cameras are used. + with_left_teleop_camera: bool = True + with_right_teleop_camera: bool = True + with_torso_camera: bool = False + mock: bool = False def __post_init__(self): - # cameras - self.cameras: dict[str, CameraConfig] = { - "teleop_left": Reachy2CameraConfig( + # Add cameras + self.cameras: dict[str, CameraConfig] = {} + if self.with_left_teleop_camera: + self.cameras["teleop_left"] = Reachy2CameraConfig( name="teleop", image_type="left", ip_address=self.ip_address, @@ -50,8 +66,9 @@ class Reachy2RobotConfig(RobotConfig): height=480, color_mode=ColorMode.RGB, rotation=Cv2Rotation.NO_ROTATION - ), - "teleop_right": Reachy2CameraConfig( + ) + if self.with_right_teleop_camera: + self.cameras["teleop_right"] = Reachy2CameraConfig( name="teleop", image_type="right", ip_address=self.ip_address, @@ -60,128 +77,17 @@ class Reachy2RobotConfig(RobotConfig): height=480, color_mode=ColorMode.RGB, rotation=Cv2Rotation.NO_ROTATION - ), - } + ) + if self.with_torso_camera: + self.cameras["torso_rgb"] = Reachy2CameraConfig( + name="depth", + image_type="rgb", + ip_address=self.ip_address, + fps=30, + width=640, + height=480, + color_mode=ColorMode.RGB, + rotation=Cv2Rotation.NO_ROTATION + ) + super().__post_init__() - - -# # cameras -# cameras: dict[str, CameraConfig] = field( -# default_factory=lambda: { -# "teleop_left": Reachy2CameraConfig( -# name="teleop", -# image_type="left", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "teleop_right": Reachy2CameraConfig( -# name="teleop", -# image_type="right", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "torso_rgb": Reachy2CameraConfig( -# name="depth", -# image_type="rgb", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "torso_depth": Reachy2CameraConfig( -# name="depth", -# image_type="depth", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION, -# use_depth=True -# ) - -# # REAL ROBOT -# "teleop_left": Reachy2CameraConfig( -# name="teleop", -# image_type="left", -# ip_address="192.168.0.199", -# # ip_address="172.18.131.66", -# fps=30, -# width=960, -# height=720, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "teleop_right": Reachy2CameraConfig( -# name="teleop", -# image_type="right", -# ip_address="192.168.0.199", -# # ip_address="172.18.131.66", -# fps=30, -# width=960, -# height=720, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "torso_rgb": Reachy2CameraConfig( -# name="depth", -# image_type="rgb", -# ip_address="172.18.131.66", -# fps=30, -# width=1280, -# height=720, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), - -# # REAL ROBOT REDUCED IMAGE SIZE -# "teleop_left": Reachy2CameraConfig( -# name="teleop", -# image_type="left", -# ip_address="192.168.0.199", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "teleop_right": Reachy2CameraConfig( -# name="teleop", -# image_type="right", -# ip_address="192.168.0.199", -# fps=30, -# width=640, -# height=480, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), - -# # Reduced size for testing -# "teleop_left": Reachy2CameraConfig( -# name="teleop", -# image_type="left", -# ip_address="172.18.131.66", -# fps=30, -# width=480, -# height=360, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# "teleop_right": Reachy2CameraConfig( -# name="teleop", -# image_type="right", -# ip_address="172.18.131.66", -# fps=30, -# width=480, -# height=360, -# color_mode=ColorMode.RGB, -# rotation=Cv2Rotation.NO_ROTATION -# ), -# } -# ) diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index 7e9575e44..481a66ca4 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -86,8 +86,7 @@ class Reachy2Robot(Robot): self.logs = {} - self.joints_dict: dict[str, str] = {} - self.generate_joints_dict() + self.joints_dict: dict[str, str] = self._generate_joints_dict() @property def observation_features(self) -> dict: @@ -142,15 +141,17 @@ class Reachy2Robot(Robot): def calibrate(self) -> None: pass - def generate_joints_dict(self) -> dict[str, str]: + def _generate_joints_dict(self) -> dict[str, str]: + self.joints = {} if self.config.with_neck: - self.joints_dict.update(REACHY2_NECK_JOINTS) + self.joints.update(REACHY2_NECK_JOINTS) if self.config.with_l_arm: - self.joints_dict.update(REACHY2_L_ARM_JOINTS) + self.joints.update(REACHY2_L_ARM_JOINTS) if self.config.with_r_arm: - self.joints_dict.update(REACHY2_R_ARM_JOINTS) + self.joints.update(REACHY2_R_ARM_JOINTS) if self.config.with_antennas: - self.joints_dict.update(REACHY2_ANTENNAS_JOINTS) + self.joints.update(REACHY2_ANTENNAS_JOINTS) + return self.joints def _get_state(self) -> dict: pos_dict = {k: self.reachy.joints[v].present_position for k, v in self.joints_dict.items()} diff --git a/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py b/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py index 6fdf796b0..c77bc0ed4 100644 --- a/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py @@ -85,18 +85,19 @@ class Reachy2FakeTeleoperator(Teleoperator): self.config = config self.reachy: None | ReachySDK = None - self.joints_dict: dict[str, str] = {} - self.generate_joints_dict() + self.joints_dict: dict[str, str] = self._generate_joints_dict() - def generate_joints_dict(self) -> dict[str, str]: + def _generate_joints_dict(self) -> dict[str, str]: + self.joints = {} if self.config.with_neck: - self.joints_dict.update(REACHY2_NECK_JOINTS) + self.joints.update(REACHY2_NECK_JOINTS) if self.config.with_l_arm: - self.joints_dict.update(REACHY2_L_ARM_JOINTS) + self.joints.update(REACHY2_L_ARM_JOINTS) if self.config.with_r_arm: - self.joints_dict.update(REACHY2_R_ARM_JOINTS) + self.joints.update(REACHY2_R_ARM_JOINTS) if self.config.with_antennas: - self.joints_dict.update(REACHY2_ANTENNAS_JOINTS) + self.joints.update(REACHY2_ANTENNAS_JOINTS) + return self.joints @property def action_features(self) -> dict[str, type]: @@ -138,7 +139,7 @@ class Reachy2FakeTeleoperator(Teleoperator): def get_action(self) -> dict[str, float]: start = time.perf_counter() - joint_action = {k: self.reachy.joints[v].goal_position for k, v in REACHY2_JOINTS.items()} + joint_action = {k: self.reachy.joints[v].goal_position for k, v in self.joints_dict.items()} if not self.config.with_mobile_base: dt_ms = (time.perf_counter() - start) * 1e3 logger.debug(f"{self} read action: {dt_ms:.1f}ms")