From bc88445eae6c4ea50586255a7e42ca4cc8db14ac Mon Sep 17 00:00:00 2001 From: glannuzel Date: Mon, 1 Sep 2025 11:31:48 +0200 Subject: [PATCH] Use ensure_safe_goal_position --- .../robots/reachy2/configuration_reachy2.py | 5 ++--- src/lerobot/robots/reachy2/robot_reachy2.py | 14 ++++++++++++++ tests/robots/test_reachy2.py | 16 +++++++++++++--- 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/src/lerobot/robots/reachy2/configuration_reachy2.py b/src/lerobot/robots/reachy2/configuration_reachy2.py index 7c531bc75..aa25351c6 100644 --- a/src/lerobot/robots/reachy2/configuration_reachy2.py +++ b/src/lerobot/robots/reachy2/configuration_reachy2.py @@ -25,9 +25,8 @@ from ..config import RobotConfig @dataclass class Reachy2RobotConfig(RobotConfig): # `max_relative_target` limits the magnitude of the relative positional target vector for safety purposes. - # Set this to a positive scalar to have the same value for all motors, or a list that is the same length as - # the number of motors in your follower arms. - max_relative_target: int | None = None + # Set this to a positive scalar to have the same value for all motors. + max_relative_target: float | None = None # IP address of the Reachy 2 robot ip_address: str | None = "localhost" diff --git a/src/lerobot/robots/reachy2/robot_reachy2.py b/src/lerobot/robots/reachy2/robot_reachy2.py index cd1256d5d..ecc488a79 100644 --- a/src/lerobot/robots/reachy2/robot_reachy2.py +++ b/src/lerobot/robots/reachy2/robot_reachy2.py @@ -23,6 +23,7 @@ from reachy2_sdk import ReachySDK from lerobot.cameras.utils import make_cameras_from_configs from ..robot import Robot +from ..utils import ensure_safe_goal_position from .configuration_reachy2 import Reachy2RobotConfig # {lerobot_keys: reachy2_sdk_keys} @@ -186,6 +187,7 @@ class Reachy2Robot(Robot): before_write_t = time.perf_counter() vel = {} + goal_pos = {} for key, val in action.items(): if key not in self.joints_dict: if key not in REACHY2_VEL: @@ -193,6 +195,18 @@ class Reachy2Robot(Robot): else: vel[REACHY2_VEL[key]] = float(val) else: + if not self.use_external_commands and self.config.max_relative_target is not None: + goal_pos[key] = float(val) + goal_present_pos = { + key: ( + goal_pos[key], + self.reachy.joints[self.joints_dict[key]].present_position, + ) + } + safe_goal_pos = ensure_safe_goal_position( + goal_present_pos, float(self.config.max_relative_target) + ) + val = safe_goal_pos[key] self.reachy.joints[self.joints_dict[key]].goal_position = float(val) if self.config.with_mobile_base: diff --git a/tests/robots/test_reachy2.py b/tests/robots/test_reachy2.py index 5a9b388bb..cd0513a19 100644 --- a/tests/robots/test_reachy2.py +++ b/tests/robots/test_reachy2.py @@ -63,6 +63,7 @@ PARAMS = [ {"use_external_commands": True, "disable_torque_on_disconnect": True}, {"use_external_commands": True, "with_mobile_base": False, "with_neck": False}, {"disable_torque_on_disconnect": False}, + {"max_relative_target": 5}, {"with_right_teleop_camera": False}, {"with_left_teleop_camera": False, "with_right_teleop_camera": False}, {"with_left_teleop_camera": False, "with_torso_camera": True}, @@ -294,19 +295,28 @@ def test_get_observation(reachy2): def test_send_action(reachy2): reachy2.connect() - action = {k: i * 10.0 for i, k in enumerate(reachy2.joints_dict.keys())} + action = {k: i * 10.0 for i, k in enumerate(reachy2.joints_dict.keys(), start=1)} if reachy2.config.with_mobile_base: action.update({k: i * 0.1 for i, k in enumerate(REACHY2_VEL.keys(), start=1)}) + previous_present_position = { + k: reachy2.reachy.joints[REACHY2_JOINTS[k]].present_position for k in reachy2.joints_dict.keys() + } returned = reachy2.send_action(action) - assert returned == action + if reachy2.config.max_relative_target is None: + assert returned == action assert reachy2.reachy._goal_position_set_total == len(reachy2.joints_dict) for motor in reachy2.joints_dict.keys(): expected_pos = action[motor] real_pos = reachy2.reachy.joints[REACHY2_JOINTS[motor]].goal_position - assert real_pos == expected_pos + if reachy2.config.max_relative_target is None: + assert real_pos == expected_pos + else: + assert real_pos == previous_present_position[motor] + np.sign(expected_pos) * min( + abs(expected_pos - real_pos), reachy2.config.max_relative_target + ) if reachy2.config.with_mobile_base: goal_speed = [i * 0.1 for i, _ in enumerate(REACHY2_VEL.keys(), start=1)]