Merge pull request #197 from pollen-robotics/186-create-reachyhome

186 create reachyhome

src/reachy2_sdk/arm.py

@@ -315,7 +315,7 @@ def goto_position_orientation(
         self._goto_stub.GoToCartesian(request)
 
     def goto_joints(
-        self, positions: List[float], duration: float = 2, degrees: bool = True, interpolation_mode: str = "minimum_jerk"
+        self, positions: List[float], duration: float = 2, interpolation_mode: str = "minimum_jerk", degrees: bool = True
     ) -> GoToId:
         """Move the arm's joints to reach the given position.
 

src/reachy2_sdk/reachy_sdk.py

@@ -49,6 +49,7 @@
 )
 
 SimplifiedRequest = namedtuple("SimplifiedRequest", ["part", "goal_positions", "duration", "mode"])
+GoToHomeId = namedtuple("GoToHomeId", ["head", "r_arm", "l_arm"])
 
 _T = t.TypeVar("_T")
 
@@ -157,6 +158,7 @@ def disconnect(self) -> None:
                 "grpc_status",
                 "connect",
                 "disconnect",
+                "home",
                 "turn_on",
                 "turn_off",
                 "enabled_parts",
@@ -595,6 +597,30 @@ def turn_off(self) -> bool:
 
         return True
 
+    def home(self, wait_for_goto_end: bool = True, duration: float = 2, interpolation_mode: str = "minimum_jerk") -> GoToHomeId:
+        """Send all joints to 0 in specified duration.
+
+        Setting wait_for_goto_end to False will cancel all gotos on all parts and immediately send the 0 commands.
+        Otherwise, the 0 commands will be sent to a part when all gotos of its queue has been played.
+        """
+        head_id = None
+        r_arm_id = None
+        l_arm_id = None
+        if not wait_for_goto_end:
+            self.cancel_all_goto()
+        if self.head is not None:
+            head_id = self.head.rotate_to(0, 0, 0, duration, interpolation_mode)
+        if self.r_arm is not None:
+            r_arm_id = self.r_arm.goto_joints([0, 0, 0, 0, 0, 0, 0], duration, interpolation_mode)
+        if self.l_arm is not None:
+            l_arm_id = self.l_arm.goto_joints([0, 0, 0, 0, 0, 0, 0], duration, interpolation_mode)
+        ids = GoToHomeId(
+            head=head_id,
+            r_arm=r_arm_id,
+            l_arm=l_arm_id,
+        )
+        return ids
+
     def is_goto_finished(self, id: GoToId) -> bool:
         """Return True if goto has been played and has been cancelled, False otherwise."""
         state = self.get_goto_state(id)

tests/test_advanced_goto_functions.py

@@ -2,6 +2,7 @@
 
 import numpy as np
 import pytest
+from pyquaternion import Quaternion
 from reachy2_sdk_api.goto_pb2 import GoalStatus, GoToId
 
 from src.reachy2_sdk.reachy_sdk import ReachySDK
@@ -287,6 +288,109 @@ def test_get_goto_joints_request(reachy_sdk_zeroed: ReachySDK) -> None:
     assert cancel.ack
 
 
+@pytest.mark.online
+def test_reachy_home(reachy_sdk_zeroed: ReachySDK) -> None:
+    zero_arm = [0, 0, 0, 0, 0, 0, 0]
+    zero_head = Quaternion(axis=[1, 0, 0], angle=0.0)
+
+    # Test waiting for part's gotos to end
+
+    req1 = reachy_sdk_zeroed.head.rotate_to(30, 0, 0, duration=4)
+    req2 = reachy_sdk_zeroed.r_arm.goto_joints([0, 10, 20, -40, 10, 10, -15], duration=5)
+    req3 = reachy_sdk_zeroed.l_arm.goto_joints([10, 10, 15, -20, 15, -15, -10], duration=6)
+
+    time.sleep(2)
+
+    req_h, req_r, req_l = reachy_sdk_zeroed.home()
+
+    assert reachy_sdk_zeroed.get_goto_state(req1).goal_status == GoalStatus.STATUS_EXECUTING
+    assert reachy_sdk_zeroed.get_goto_state(req2).goal_status == GoalStatus.STATUS_EXECUTING
+    assert reachy_sdk_zeroed.get_goto_state(req3).goal_status == GoalStatus.STATUS_EXECUTING
+
+    assert (reachy_sdk_zeroed.get_goto_state(req_h).goal_status == GoalStatus.STATUS_ACCEPTED) | (
+        reachy_sdk_zeroed.get_goto_state(req_h).goal_status == GoalStatus.STATUS_UNKNOWN
+    )  # should be ACCEPTED ?
+    assert (reachy_sdk_zeroed.get_goto_state(req_r).goal_status == GoalStatus.STATUS_ACCEPTED) | (
+        reachy_sdk_zeroed.get_goto_state(req_r).goal_status == GoalStatus.STATUS_UNKNOWN
+    )  # should be ACCEPTED ?
+    assert (reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_ACCEPTED) | (
+        reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_UNKNOWN
+    )  # should be ACCEPTED ?
+
+    while not is_goto_finished(reachy_sdk_zeroed, req2):
+        time.sleep(0.1)
+
+    assert reachy_sdk_zeroed.get_goto_state(req1).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req2).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req3).goal_status == GoalStatus.STATUS_EXECUTING
+
+    assert reachy_sdk_zeroed.get_goto_state(req_h).goal_status == GoalStatus.STATUS_EXECUTING
+    assert reachy_sdk_zeroed.get_goto_state(req_r).goal_status == GoalStatus.STATUS_EXECUTING
+    assert (reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_ACCEPTED) | (
+        reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_UNKNOWN
+    )  # should be ACCEPTED ?
+
+    while not is_goto_finished(reachy_sdk_zeroed, req_l):
+        time.sleep(0.1)
+
+    ans_r = reachy_sdk_zeroed.get_goto_joints_request(req_r)
+    assert ans_r.part == "r_arm"
+    assert np.allclose(ans_r.goal_positions, zero_arm, atol=1e-01)
+    assert ans_r.duration == 2
+    assert ans_r.mode == "minimum_jerk"
+
+    assert reachy_sdk_zeroed.get_goto_state(req_h).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req_r).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert np.isclose(Quaternion.distance(reachy_sdk_zeroed.head.get_orientation(), zero_head), 0, atol=1e-04)
+    assert np.allclose(reachy_sdk_zeroed.r_arm.get_joints_positions(), zero_arm, atol=1e-01)
+    assert np.allclose(reachy_sdk_zeroed.l_arm.get_joints_positions(), zero_arm, atol=1e-01)
+
+    cancel = reachy_sdk_zeroed.cancel_all_goto()
+    assert cancel.ack
+
+    # Test without waiting for part's gotos to end
+
+    req4 = reachy_sdk_zeroed.head.rotate_to(30, 0, 0, duration=4)
+    req5 = reachy_sdk_zeroed.r_arm.goto_joints([0, 10, 20, -40, 10, 10, -15], duration=5)
+    req6 = reachy_sdk_zeroed.l_arm.goto_joints([10, 10, 15, -20, 15, -15, -10], duration=6)
+
+    time.sleep(2)
+
+    req_h2, req_r2, req_l2 = reachy_sdk_zeroed.home(wait_for_goto_end=False, duration=1, interpolation_mode="linear")
+
+    assert (reachy_sdk_zeroed.get_goto_state(req4).goal_status == GoalStatus.STATUS_CANCELING) | (
+        reachy_sdk_zeroed.get_goto_state(req4).goal_status == GoalStatus.STATUS_CANCELED
+    )
+    assert (reachy_sdk_zeroed.get_goto_state(req5).goal_status == GoalStatus.STATUS_CANCELING) | (
+        reachy_sdk_zeroed.get_goto_state(req5).goal_status == GoalStatus.STATUS_CANCELED
+    )
+    assert (reachy_sdk_zeroed.get_goto_state(req6).goal_status == GoalStatus.STATUS_CANCELING) | (
+        reachy_sdk_zeroed.get_goto_state(req6).goal_status == GoalStatus.STATUS_CANCELED
+    )
+    assert reachy_sdk_zeroed.get_goto_state(req_h2).goal_status == GoalStatus.STATUS_EXECUTING
+    assert reachy_sdk_zeroed.get_goto_state(req_r2).goal_status == GoalStatus.STATUS_EXECUTING
+    assert reachy_sdk_zeroed.get_goto_state(req_l2).goal_status == GoalStatus.STATUS_EXECUTING
+
+    while not is_goto_finished(reachy_sdk_zeroed, req_l2):
+        time.sleep(0.1)
+
+    ans_l2 = reachy_sdk_zeroed.get_goto_joints_request(req_l2)
+    assert ans_l2.part == "l_arm"
+    assert np.allclose(ans_l2.goal_positions, zero_arm, atol=1e-01)
+    assert ans_l2.duration == 1
+    assert ans_l2.mode == "linear"
+
+    assert reachy_sdk_zeroed.get_goto_state(req_h).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req_r).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert reachy_sdk_zeroed.get_goto_state(req_l).goal_status == GoalStatus.STATUS_SUCCEEDED
+    assert np.isclose(
+        Quaternion.distance(reachy_sdk_zeroed.head.get_orientation(), zero_head), 0, atol=1e-03
+    )  # why not 1e-04 here?
+    assert np.allclose(reachy_sdk_zeroed.r_arm.get_joints_positions(), zero_arm, atol=1e-01)
+    assert np.allclose(reachy_sdk_zeroed.l_arm.get_joints_positions(), zero_arm, atol=1e-01)
+
+
 @pytest.mark.online
 def test_is_goto_finished(reachy_sdk_zeroed: ReachySDK) -> None:
     req1 = reachy_sdk_zeroed.head.rotate_to(30, 0, 0, duration=2)