Merge pull request #469 from pollen-robotics/468-add-gripper-paramete…

…r-in-goto_posture

468 add gripper parameter in goto posture

src/reachy2_sdk/parts/arm.py

@@ -600,6 +600,7 @@ def goto_posture(
         wait: bool = False,
         wait_for_goto_end: bool = True,
         interpolation_mode: str = "minimum_jerk",
+        open_gripper: bool = False,
     ) -> GoToId:
         """Send all joints to standard positions with optional parameters for duration, waiting, and interpolation mode.
 
@@ -616,14 +617,15 @@ def goto_posture(
                 will cancel all executing moves and queues. Defaults to `True`.
             interpolation_mode: The type of interpolation used when moving the arm's joints.
                 Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
+            open_gripper: If `True`, the gripper will open, if `False`, it stays in its current position.
+                Defaults to `False`.
 
         Returns:
             A unique GoToId identifier for this specific movement.
         """
         joints = self.get_default_posture_joints(common_posture=common_posture)
-        if common_posture == "default":
-            if self._gripper is not None and self._gripper.is_on():
-                self._gripper.open()
+        if self._gripper is not None and self._gripper.is_on() and open_gripper:
+            self._gripper.open()
         if not wait_for_goto_end:
             self.cancel_all_goto()
         if self.is_on():

src/reachy2_sdk/reachy_sdk.py

@@ -610,6 +610,7 @@ def goto_posture(
         wait: bool = False,
         wait_for_goto_end: bool = True,
         interpolation_mode: str = "minimum_jerk",
+        open_gripper: bool = False,
     ) -> GoToHomeId:
         """Move the robot to a predefined posture.
 
@@ -625,6 +626,8 @@ def goto_posture(
                 will cancel all executing moves and queues. Defaults to `True`.
             interpolation_mode: The type of interpolation used when moving the arm's joints.
                 Can be 'minimum_jerk' or 'linear'. Defaults to 'minimum_jerk'.
+            open_gripper: If `True`, the gripper will open, if `False`, it stays in its current position.
+                Defaults to `False`.
 
         Returns:
             A GoToHomeId containing movement GoToIds for each part.
@@ -651,6 +654,7 @@ def goto_posture(
                 wait=wait_r_arm,
                 wait_for_goto_end=wait_for_goto_end,
                 interpolation_mode=interpolation_mode,
+                open_gripper=open_gripper,
             )
         if self.l_arm is not None:
             l_arm_id = self.l_arm.goto_posture(
@@ -659,6 +663,7 @@ def goto_posture(
                 wait=wait,
                 wait_for_goto_end=wait_for_goto_end,
                 interpolation_mode=interpolation_mode,
+                open_gripper=open_gripper,
             )
         ids = GoToHomeId(
             head=head_id,

tests/units/online/test_advanced_goto_functions.py

@@ -274,7 +274,7 @@ def test_reachy_goto_posture(reachy_sdk_zeroed: ReachySDK) -> None:
     # Test the default pose
     reachy_sdk_zeroed.l_arm.gripper.close()
     reachy_sdk_zeroed.r_arm.gripper.close()
-    reachy_sdk_zeroed.goto_posture("default")
+    reachy_sdk_zeroed.goto_posture("default", open_gripper=True)
     time.sleep(2)
     assert reachy_sdk_zeroed.l_arm.gripper.opening == 100.0
     assert reachy_sdk_zeroed.r_arm.gripper.opening == 100.0
@@ -283,7 +283,7 @@ def test_reachy_goto_posture(reachy_sdk_zeroed: ReachySDK) -> None:
     reachy_sdk_zeroed.goto_posture("elbow_90")
     time.sleep(2)
     reachy_sdk_zeroed.l_arm.turn_off()
-    reachy_sdk_zeroed.goto_posture("default")
+    reachy_sdk_zeroed.goto_posture("default", open_gripper=True)
     time.sleep(2)
     assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.opening, 30, atol=5)
     assert reachy_sdk_zeroed.r_arm.gripper.opening == 100.0

tests/units/online/test_basic_movements.py

@@ -207,6 +207,12 @@ def test_get_default_posture_matrix(reachy_sdk_zeroed: ReachySDK) -> None:
     with pytest.raises(ValueError):
         reachy_sdk_zeroed.r_arm.get_default_posture_matrix("coucou")
 
+    reachy_sdk_zeroed.l_arm.gripper.set_opening(50)
+    reachy_sdk_zeroed.goto_posture(common_posture="default", wait=True)
+    assert np.allclose(reachy_sdk_zeroed.l_arm.gripper.opening, 50, atol=5)
+    reachy_sdk_zeroed.goto_posture(common_posture="default", wait=True, open_gripper=True)
+    assert np.allclose(reachy_sdk_zeroed.l_arm.gripper.opening, 100, atol=5)
+
 
 @pytest.mark.online
 def test_get_default_posture_joints(reachy_sdk_zeroed: ReachySDK) -> None: