Use ensure_safe_goal_position

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"

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:

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)]