From ddaca00801ee11bcee4fc6b051e00ea93678170c Mon Sep 17 00:00:00 2001 From: glannuzel Date: Tue, 29 Jul 2025 15:12:05 +0200 Subject: [PATCH] Try adding mobile_base velocity --- src/lerobot/robots/reachy2/robot_reachy2.py | 74 ++++++++----------- .../reachy2_fake_teleoperator.py | 25 ++++--- 2 files changed, 45 insertions(+), 54 deletions(-) diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index 932bacc9b..a2d001696 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -25,13 +25,12 @@ from typing import Any # from stretch_body.robot_params import RobotParams from lerobot.cameras.utils import make_cameras_from_configs -from lerobot.constants import OBS_IMAGES, OBS_STATE from ..robot import Robot from .configuration_reachy2 import Reachy2RobotConfig # {lerobot_keys: reachy2_sdk_keys} -REACHY2_MOTORS = { +REACHY2_JOINTS = { "neck_yaw.pos": "head.neck.yaw", "neck_pitch.pos": "head.neck.pitch", "neck_roll.pos": "head.neck.roll", @@ -53,9 +52,12 @@ REACHY2_MOTORS = { "l_gripper.pos": "l_arm.gripper", "l_antenna.pos": "head.l_antenna", "r_antenna.pos": "head.r_antenna", - # "mobile_base.vx": "mobile_base.vx", - # "mobile_base.vy": "mobile_base.vy", - # "mobile_base.vtheta": "mobile_base.vtheta", +} + +REACHY2_VEL = { + "mobile_base.vx": "vx", + "mobile_base.vy": "vy", + "mobile_base.vtheta": "vtheta", } @@ -81,15 +83,6 @@ class Reachy2Robot(Robot): @property def observation_features(self) -> dict: return {**self.motors_features, **self.camera_features} - # return dict.fromkeys( - # REACHY2_MOTORS.keys(), - # float, - # ) - # return { - # "dtype": "float32", - # "shape": len(REACHY2_MOTORS), - # "names": {"motors": list(REACHY2_MOTORS)}, - # } @property def action_features(self) -> dict: @@ -100,21 +93,16 @@ class Reachy2Robot(Robot): return { cam: (self.cameras[cam].height, self.cameras[cam].width, 3) for cam in self.cameras } - # cam_ft = {} - # for cam_key, cam in self.cameras.items(): - # cam_ft[cam_key] = { - # "shape": (cam.height, cam.width, cam.channels), - # "names": ["height", "width", "channels"], - # "info": None, - # } - # return cam_ft @property def motors_features(self) -> dict: - return dict.fromkeys( - REACHY2_MOTORS.keys(), + return {**dict.fromkeys( + REACHY2_JOINTS.keys(), float, - ) + ), **dict.fromkeys( + REACHY2_VEL.keys(), + float, + )} @property def is_connected(self) -> bool: @@ -129,10 +117,6 @@ class Reachy2Robot(Robot): for cam in self.cameras.values(): cam.connect() - # if not self.is_connected: - # print("Could not connect to the cameras, check that all cameras are plugged-in.") - # raise ConnectionError() - self.configure() def configure(self) -> None: @@ -147,7 +131,9 @@ class Reachy2Robot(Robot): pass def _get_state(self) -> dict: - return {k: self.reachy.joints[v].present_position for k, v in REACHY2_MOTORS.items()} + pos_dict = {k: self.reachy.joints[v].present_position for k, v in REACHY2_JOINTS.items()} + vel_dict = {k: self.reachy.mobile_base.odometry[v] for k, v in REACHY2_VEL.items()} + return {**pos_dict, **vel_dict} def get_observation(self) -> dict[str, np.ndarray]: obs_dict = {} @@ -157,21 +143,10 @@ class Reachy2Robot(Robot): obs_dict = self._get_state() self.logs["read_pos_dt_s"] = time.perf_counter() - before_read_t - # state = np.asarray(list(state.values())) - # obs_dict[OBS_STATE] = state - + # Capture images from cameras for cam_key, cam in self.cameras.items(): obs_dict[cam_key] = cam.async_read() - # Capture images from cameras - # for cam_key, cam in self.cameras.items(): - # before_camread_t = time.perf_counter() - # frame = cam.async_read(timeout_ms=200) - # print(f"Async frame shape:", frame.shape) - - # obs_dict[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read(timeout_ms=200) - # self.logs[f"async_read_camera_{cam_key}_dt_s"] = time.perf_counter() - before_camread_t - return obs_dict def send_action(self, action: dict[str, Any]) -> dict[str, Any]: @@ -179,13 +154,22 @@ class Reachy2Robot(Robot): raise ConnectionError() before_write_t = time.perf_counter() + + vel = {} for key, val in action.items(): - if key not in REACHY2_MOTORS: - raise KeyError(f"Key '{key}' is not a valid motor key in Reachy 2.") + if key not in REACHY2_JOINTS: + if key not in REACHY2_VEL: + raise KeyError(f"Key '{key}' is not a valid motor key in Reachy 2.") + else: + vel[REACHY2_VEL[key]] = val else: - self.reachy.joints[REACHY2_MOTORS[key]].goal_position = val + self.reachy.joints[REACHY2_JOINTS[key]].goal_position = val + self.reachy.mobile_base.set_goal_speed(vel["vx"], vel["vy"], vel["vtheta"]) + # # We don't want the teleoperator reachy2_specific to send the goal positions # self.reachy.send_goal_positions() + # self.reachy.send_speed_command() + self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t return action 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 60cb599b7..1e48eaf5b 100644 --- a/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_fake_teleoperator/reachy2_fake_teleoperator.py @@ -32,7 +32,7 @@ from .config_reachy2_fake_teleoperator import Reachy2FakeTeleoperatorConfig logger = logging.getLogger(__name__) # {lerobot_keys: reachy2_sdk_keys} -REACHY2_MOTORS = { +REACHY2_JOINTS = { "neck_yaw.pos": "head.neck.yaw", "neck_pitch.pos": "head.neck.pitch", "neck_roll.pos": "head.neck.roll", @@ -54,9 +54,12 @@ REACHY2_MOTORS = { "l_gripper.pos": "l_arm.gripper", "l_antenna.pos": "head.l_antenna", "r_antenna.pos": "head.r_antenna", - # "mobile_base.vx": "mobile_base.vx", - # "mobile_base.vy": "mobile_base.vy", - # "mobile_base.vtheta": "mobile_base.vtheta", +} + +REACHY2_VEL = { + "mobile_base.vx": "x", + "mobile_base.vy": "y", + "mobile_base.vtheta": "theta", } @@ -75,10 +78,13 @@ class Reachy2FakeTeleoperator(Teleoperator): @property def action_features(self) -> dict[str, type]: - return dict.fromkeys( - REACHY2_MOTORS.keys(), + return {**dict.fromkeys( + REACHY2_JOINTS.keys(), float, - ) + ), **dict.fromkeys( + REACHY2_VEL.keys(), + float, + )} @property def feedback_features(self) -> dict[str, type]: @@ -107,10 +113,11 @@ class Reachy2FakeTeleoperator(Teleoperator): def get_action(self) -> dict[str, float]: start = time.perf_counter() - action = {k: self.reachy.joints[v].goal_position for k, v in REACHY2_MOTORS.items()} + joint_action = {k: self.reachy.joints[v].goal_position for k, v in REACHY2_JOINTS.items()} + 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 action + return {**joint_action, **vel_action} def send_feedback(self, feedback: dict[str, float]) -> None: # TODO(rcadene, aliberts): Implement force feedback