From f31acfd67fd4d37184cbaf227e34d7bed0a575f4 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 28 Aug 2025 15:15:05 +0200 Subject: [PATCH] Add use_present_position to teleoperator --- .../reachy2_teleoperator/config_reachy2_teleoperator.py | 8 +++++++- .../reachy2_teleoperator/reachy2_teleoperator.py | 7 ++++++- tests/teleoperators/test_reachy2_teleoperator.py | 9 +++++++-- 3 files changed, 20 insertions(+), 4 deletions(-) diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/config_reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/config_reachy2_teleoperator.py index 5b066a98f..4e1496a3c 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/config_reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/config_reachy2_teleoperator.py @@ -22,8 +22,14 @@ from ..config import TeleoperatorConfig @TeleoperatorConfig.register_subclass("reachy2_teleoperator") @dataclass class Reachy2TeleoperatorConfig(TeleoperatorConfig): - # Port to connect to the arm + # IP address of the Reachy 2 robot used as teleoperator ip_address: str | None = "localhost" + + # Whether to use the present position of the joints as actions + # if False, the goal position of the joints will be used + use_present_position: bool = False + + # Which parts of the robot to use with_mobile_base: bool = True with_l_arm: bool = True with_r_arm: bool = True diff --git a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py index caafa932f..d2f44b30b 100644 --- a/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py +++ b/src/lerobot/teleoperators/reachy2_teleoperator/reachy2_teleoperator.py @@ -136,7 +136,12 @@ class Reachy2Teleoperator(Teleoperator): start = time.perf_counter() if self.reachy and self.is_connected: - joint_action = {k: self.reachy.joints[v].goal_position for k, v in self.joints_dict.items()} + if self.config.use_present_position: + joint_action = { + k: self.reachy.joints[v].present_position for k, v in self.joints_dict.items() + } + 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") diff --git a/tests/teleoperators/test_reachy2_teleoperator.py b/tests/teleoperators/test_reachy2_teleoperator.py index 6f64efc2d..6fe5bda0f 100644 --- a/tests/teleoperators/test_reachy2_teleoperator.py +++ b/tests/teleoperators/test_reachy2_teleoperator.py @@ -60,6 +60,7 @@ PARAMS = [ {"with_mobile_base": False, "with_l_arm": False, "with_antennas": False}, {"with_r_arm": False, "with_neck": False, "with_antennas": False}, {"with_mobile_base": False, "with_neck": False}, + {"use_present_position": True}, ] @@ -76,7 +77,8 @@ def _make_reachy2_sdk_mock(): # Mock joints with some dummy positions joints = { k: MagicMock( - goal_position=float(i), + present_position=float(i), + goal_position=float(i) + 0.5, ) for i, k in enumerate(REACHY2_JOINTS.values()) } @@ -133,7 +135,10 @@ def test_get_action(reachy2): assert set(action.keys()) == expected_keys for motor in reachy2.joints_dict.keys(): - assert action[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].goal_position + if reachy2.config.use_present_position: + assert action[motor] == reachy2.reachy.joints[REACHY2_JOINTS[motor]].present_position + 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]]