Add and fix send_goal_positions tests

Author: glannuzel

Diff for: src/reachy2_sdk/grippers/parallel_gripper.py

@@ -173,22 +173,10 @@ def _get_goal_positions_message(self) -> Optional[HandPositionRequest]:
                     )
                 ),
             )
+            self._joints["finger"]._is_moving = True
             return command
         return None
 
-    def send_goal_positions(self, check_positions: bool = True) -> None:
-        """Send the goal position to the actuator's joints.
-
-        If any goal position has been specified to the gripper, sends them to the robot.
-        If the hand is off, the command is not sent.
-
-        Args :
-            check_positions: A boolean indicating whether to check the positions after sending the command.
-                Defaults to True.
-        """
-        super().send_goal_positions(check_positions)
-        self._joints["finger"]._is_moving = True
-
     def goto_posture(
         self,
         common_posture: str = "default",

Diff for: tests/units/online/conftest.py

@@ -31,6 +31,7 @@ def reachy_sdk() -> ReachySDK:
 @pytest.fixture
 def reachy_sdk_zeroed(reachy_sdk: ReachySDK) -> ReachySDK:
     reachy_sdk.cancel_all_goto()
+    reachy_sdk.turn_on()
     for joint in reachy_sdk.joints.values():
         joint.goal_position = 0
     reachy_sdk.send_goal_positions()

Diff for: tests/units/online/test_send_goal_positions.py

@@ -73,9 +73,12 @@ def test_reachy_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
     assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 27, 1e-03)
     assert np.isclose(reachy_sdk_zeroed.r_arm.gripper.present_position, r_gripper_present_positions, 1e-03)
     assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.present_position, 68, 1e-03)
+    assert reachy_sdk_zeroed.l_arm.gripper.is_moving()
 
     reachy_sdk_zeroed.r_arm.turn_off()
 
+    assert not reachy_sdk_zeroed.l_arm.gripper.is_moving()
+
     reachy_sdk_zeroed.r_arm.shoulder.roll.goal_position = -7
     reachy_sdk_zeroed.r_arm.elbow.pitch.goal_position = -30
     reachy_sdk_zeroed.l_arm.shoulder.pitch.goal_position = 8
@@ -173,12 +176,183 @@ def test_arm_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
     assert np.isclose(reachy_sdk_zeroed.l_arm.shoulder.roll.present_position, l_arm_present_positions[1], 1e-03)
     assert np.isclose(reachy_sdk_zeroed.l_arm.wrist.pitch.present_position, 8, 1e-03)
     assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.present_position, 9, 1e-03)
+    assert reachy_sdk_zeroed.l_arm.gripper.is_moving()
 
     reachy_sdk_zeroed.l_arm.gripper.turn_off()
     time.sleep(0.1)
 
+    assert not reachy_sdk_zeroed.l_arm.gripper.is_moving()
     reachy_sdk_zeroed.l_arm.gripper.goal_position = 110
     reachy_sdk_zeroed.l_arm.send_goal_positions()
     time.sleep(0.1)
 
     assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.present_position, 9, 1e-03)
+
+
+@pytest.mark.online
+def test_head_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
+    reachy_sdk_zeroed.head.neck.pitch.goal_position = 20
+    reachy_sdk_zeroed.head.neck.roll.goal_position = 10
+    reachy_sdk_zeroed.head.neck.yaw.goal_position = -5
+    reachy_sdk_zeroed.head.l_antenna.goal_position = 27
+    reachy_sdk_zeroed.head.r_antenna.goal_position = 33
+
+    reachy_sdk_zeroed.head.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.neck.pitch.present_position, 20, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.roll.present_position, 10, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.yaw.present_position, -5, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, 27, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 33, 1e-03)
+
+    reachy_sdk_zeroed.head.neck.turn_off()
+    time.sleep(0.1)
+    reachy_sdk_zeroed.head.neck.pitch.goal_position = 30
+    reachy_sdk_zeroed.head.neck.roll.goal_position = 20
+    reachy_sdk_zeroed.head.neck.yaw.goal_position = 10
+    reachy_sdk_zeroed.head.l_antenna.goal_position = 37
+    reachy_sdk_zeroed.head.r_antenna.goal_position = 43
+
+    reachy_sdk_zeroed.head.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.neck.pitch.present_position, 20, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.roll.present_position, 10, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.yaw.present_position, -5, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, 37, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 43, 1e-03)
+
+    reachy_sdk_zeroed.head.neck.turn_on()
+    time.sleep(0.1)
+    reachy_sdk_zeroed.head.neck.pitch.goal_position = 40
+
+    reachy_sdk_zeroed.head.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.neck.pitch.present_position, 40, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.roll.present_position, 10, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.yaw.present_position, -5, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, 37, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 43, 1e-03)
+
+
+@pytest.mark.online
+def test_orbita_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
+    r_arm_present_positions = reachy_sdk_zeroed.r_arm.get_current_positions()
+
+    reachy_sdk_zeroed.r_arm.shoulder.pitch.goal_position = 20
+    reachy_sdk_zeroed.r_arm.shoulder.roll.goal_position = -10
+    reachy_sdk_zeroed.r_arm.elbow.pitch.goal_position = -50
+
+    reachy_sdk_zeroed.r_arm.shoulder.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.pitch.present_position, 20, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.roll.present_position, -10, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.elbow.pitch.present_position, r_arm_present_positions[3], 1e-03)
+
+    reachy_sdk_zeroed.r_arm.elbow.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.pitch.present_position, 20, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.roll.present_position, -10, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.elbow.pitch.present_position, -50, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.elbow.yaw.present_position, r_arm_present_positions[2], 1e-03)
+
+    reachy_sdk_zeroed.head.neck.pitch.goal_position = 3
+    reachy_sdk_zeroed.head.neck.roll.goal_position = 5
+    reachy_sdk_zeroed.head.neck.yaw.goal_position = 7
+
+    l_arm_present_positions = reachy_sdk_zeroed.l_arm.get_current_positions()
+    reachy_sdk_zeroed.l_arm.wrist.roll.goal_position = 6
+
+    reachy_sdk_zeroed.head.neck.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.neck.pitch.present_position, 3, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.roll.present_position, 5, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.neck.yaw.present_position, 7, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.l_arm.wrist.roll.present_position, l_arm_present_positions[4], 1e-03)
+
+    reachy_sdk_zeroed.l_arm.wrist.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.l_arm.wrist.roll.present_position, 6, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.l_arm.wrist.pitch.present_position, l_arm_present_positions[5], 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.l_arm.wrist.yaw.present_position, l_arm_present_positions[6], 1e-03)
+
+    reachy_sdk_zeroed.r_arm.turn_off()
+    time.sleep(0.1)
+    r_arm_present_positions = reachy_sdk_zeroed.r_arm.get_current_positions()
+    reachy_sdk_zeroed.r_arm.shoulder.pitch.goal_position = 2
+
+    reachy_sdk_zeroed.r_arm.shoulder.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.pitch.present_position, r_arm_present_positions[0], 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.r_arm.shoulder.roll.present_position, r_arm_present_positions[1], 1e-03)
+
+
+@pytest.mark.online
+def test_gripper_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
+    l_gripper_present_position = reachy_sdk_zeroed.l_arm.gripper.present_position
+
+    reachy_sdk_zeroed.r_arm.gripper.goal_position = 68
+    reachy_sdk_zeroed.l_arm.gripper.goal_position = 110
+
+    reachy_sdk_zeroed.r_arm.gripper.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.gripper.present_position, 68, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.present_position, l_gripper_present_position, 1e-03)
+    assert reachy_sdk_zeroed.r_arm.gripper.is_moving()
+
+    reachy_sdk_zeroed.l_arm.gripper.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.gripper.present_position, 68, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.l_arm.gripper.present_position, 110, 1e-03)
+    assert not reachy_sdk_zeroed.r_arm.gripper.is_moving()
+    assert reachy_sdk_zeroed.l_arm.gripper.is_moving()
+
+    reachy_sdk_zeroed.r_arm.gripper.turn_off()
+    time.sleep(0.1)
+    r_gripper_present_position = reachy_sdk_zeroed.r_arm.gripper.present_position
+    assert not reachy_sdk_zeroed.l_arm.gripper.is_moving()
+
+    reachy_sdk_zeroed.r_arm.gripper.goal_position = 80
+    reachy_sdk_zeroed.r_arm.gripper.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.r_arm.gripper.present_position, r_gripper_present_position, 1e-03)
+
+
+@pytest.mark.online
+def test_antenna_send_goal_positions(reachy_sdk_zeroed: ReachySDK) -> None:
+    r_antenna_present_position = reachy_sdk_zeroed.head.r_antenna.present_position
+    reachy_sdk_zeroed.head.l_antenna.goal_position = 27
+    reachy_sdk_zeroed.head.r_antenna.goal_position = 33
+
+    reachy_sdk_zeroed.head.l_antenna.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.l_antenna.present_position, 27, 1e-03)
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, r_antenna_present_position, 1e-03)
+
+    reachy_sdk_zeroed.head.r_antenna.turn_off()
+    time.sleep(0.1)
+
+    reachy_sdk_zeroed.head.r_antenna.goal_position = 37
+    reachy_sdk_zeroed.head.r_antenna.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, r_antenna_present_position, 1e-03)
+
+    reachy_sdk_zeroed.head.r_antenna.turn_on()
+    time.sleep(0.1)
+
+    reachy_sdk_zeroed.head.r_antenna.send_goal_positions()
+    time.sleep(0.1)
+
+    assert np.isclose(reachy_sdk_zeroed.head.r_antenna.present_position, 37, 1e-03)