From d023e33cd8eeff736fa4da4e78d87cb2ce6e7007 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 28 Aug 2025 18:40:05 +0200 Subject: [PATCH] Use odometry for vel with present_position --- .../reachy2_teleoperator.py | 7 +++++- .../test_reachy2_teleoperator.py | 22 ++++++++++++++----- 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py index d2f44b30b..245f2f6cc 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py @@ -142,11 +142,16 @@ class Reachy2Teleoperator(Teleoperator): } else: 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") return joint_action - vel_action = {k: self.reachy.mobile_base.last_cmd_vel[v] for k, v in REACHY2_VEL.items()} + + if self.config.use_present_position: + 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} diff --git a/tests/teleoperators/test_reachy2_teleoperator.py b/tests/teleoperators/test_reachy2_teleoperator.py index 98abdf480..69fc1df41 100644 --- a/tests/teleoperators/test_reachy2_teleoperator.py +++ b/tests/teleoperators/test_reachy2_teleoperator.py @@ -87,9 +87,17 @@ def _make_reachy2_sdk_mock(): # Mock mobile base with some dummy odometry r.mobile_base = MagicMock() r.mobile_base.last_cmd_vel = { - "vx": 0.001, - "vy": 0.002, - "vtheta": 0.0, + "vx": -0.2, + "vy": 0.2, + "vtheta": 11.0, + } + r.mobile_base.odometry = { + "x": 1.0, + "y": 2.0, + "theta": 20.0, + "vx": 0.1, + "vy": -0.1, + "vtheta": 8.0, } r.connect = MagicMock(side_effect=_connect) @@ -140,8 +148,12 @@ def test_get_action(reachy2): else: assert action[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].goal_position if reachy2.config.with_mobile_base: - for vel in REACHY2_VEL.keys(): - assert action[vel] == reachy2.reachy.mobile_base.last_cmd_vel[REACHY2_VEL[vel]] + if reachy2.config.use_present_position: + for vel in REACHY2_VEL.keys(): + assert action[vel] == reachy2.reachy.mobile_base.odometry[REACHY2_VEL[vel]] + else: + for vel in REACHY2_VEL.keys(): + assert action[vel] == reachy2.reachy.mobile_base.last_cmd_vel[REACHY2_VEL[vel]] def test_no_part_declared():