diff --git a/src/reachy2_sdk/parts/arm.py b/src/reachy2_sdk/parts/arm.py index b4685a78..c0fca22b 100644 --- a/src/reachy2_sdk/parts/arm.py +++ b/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(): diff --git a/src/reachy2_sdk/reachy_sdk.py b/src/reachy2_sdk/reachy_sdk.py index 138833c5..cdb8053a 100644 --- a/src/reachy2_sdk/reachy_sdk.py +++ b/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, diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py index cfaf879c..72758427 100644 --- a/tests/units/online/test_advanced_goto_functions.py +++ b/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 diff --git a/tests/units/online/test_basic_movements.py b/tests/units/online/test_basic_movements.py index 70187fb8..808561ad 100644 --- a/tests/units/online/test_basic_movements.py +++ b/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: