From 6acb6bd3e552c11e38b62143f4cca0fb91f460cb Mon Sep 17 00:00:00 2001 From: glannuzel Date: Tue, 29 Jul 2025 10:18:47 +0200 Subject: [PATCH] Fix observation state --- src/lerobot/robots/reachy2/robot_reachy2.py | 28 ++++++++++++--------- 1 file changed, 16 insertions(+), 12 deletions(-) diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index 75df78595..5f75bb841 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -60,7 +60,9 @@ REACHY2_MOTORS = { class Reachy2Robot(Robot): - """[Reachy 2](https://www.pollen-robotics.com/reachy/), by Pollen Robotics.""" + """ + [Reachy 2](https://www.pollen-robotics.com/reachy/), by Pollen Robotics. + """ config_class = Reachy2RobotConfig name = "reachy2" @@ -115,11 +117,10 @@ class Reachy2Robot(Robot): # for cam in self.cameras.values(): # cam.connect() - # self.is_connected = self.is_connected and cam.is_connected - if not self.is_connected: - print("Could not connect to the cameras, check that all cameras are plugged-in.") - raise ConnectionError() + # if not self.is_connected: + # print("Could not connect to the cameras, check that all cameras are plugged-in.") + # raise ConnectionError() self.configure() @@ -142,17 +143,19 @@ class Reachy2Robot(Robot): # Read Reachy 2 state before_read_t = time.perf_counter() - state = self._get_state() + 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 + # state = np.asarray(list(state.values())) + # obs_dict[OBS_STATE] = state # Capture images from cameras # for cam_key, cam in self.cameras.items(): - # before_camread_t = time.perf_counter() - # obs_dict[f"{OBS_IMAGES}.{cam_key}"] = cam.async_read() - # self.logs[f"read_camera_{cam_key}_dt_s"] = cam.logs["delta_timestamp_s"] + # 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 @@ -167,7 +170,8 @@ class Reachy2Robot(Robot): raise KeyError(f"Key '{key}' is not a valid motor key in Reachy 2.") else: self.reachy.joints[REACHY2_MOTORS[key]].goal_position = val - self.reachy.send_goal_positions() + # # We don't want the teleoperator reachy2_specific to send the goal positions + # self.reachy.send_goal_positions() self.logs["write_pos_dt_s"] = time.perf_counter() - before_write_t return action