Add use_present_position to teleoperator

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

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

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