From 01fcbdffabe950de8f4e38b110c96c8d0db6c0b2 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Fri, 19 Jan 2024 12:06:25 +0100 Subject: [PATCH 1/6] Update reachy_sdk.py --- src/reachy2_sdk/reachy_sdk.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/reachy2_sdk/reachy_sdk.py b/src/reachy2_sdk/reachy_sdk.py index 4983942..39503a0 100644 --- a/src/reachy2_sdk/reachy_sdk.py +++ b/src/reachy2_sdk/reachy_sdk.py @@ -586,6 +586,10 @@ def cancel_all_goto(self) -> GoToAck: response = self._goto_stub.CancelAllGoTo(Empty()) return response + def home(self) -> None: + # TODO + return + _open_connection: List[ReachySDK] = [] From ac1c7e9483434fc513ecf322911ac00585c7c237 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 25 Jan 2024 15:15:51 +0100 Subject: [PATCH 2/6] be consistent with head goto order --- src/reachy2_sdk/arm.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/reachy2_sdk/arm.py b/src/reachy2_sdk/arm.py index 0256c77..e906f7f 100644 --- a/src/reachy2_sdk/arm.py +++ b/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. From c9c0ed34496d536ebbfa3748410e1bce9bbf6043 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 25 Jan 2024 15:16:07 +0100 Subject: [PATCH 3/6] Add reachy.home --- src/reachy2_sdk/reachy_sdk.py | 25 +++++++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/src/reachy2_sdk/reachy_sdk.py b/src/reachy2_sdk/reachy_sdk.py index 82f6ccb..a034306 100644 --- a/src/reachy2_sdk/reachy_sdk.py +++ b/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", @@ -593,14 +595,29 @@ 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: + 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 cancel_all_goto(self) -> GoToAck: response = self._goto_stub.CancelAllGoTo(Empty()) return response - def home(self) -> None: - # TODO - return - def get_goto_state(self, goto_id: GoToId) -> GoToGoalStatus: """Return the current state of a goto, given its id.""" response = self._goto_stub.GetGoToState(goto_id) From 2558565dbd8648f15a9ccaf91dd04e109816a98d Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 25 Jan 2024 15:16:58 +0100 Subject: [PATCH 4/6] Add reachy.home test --- tests/test_advanced_goto_functions.py | 104 ++++++++++++++++++++++++++ 1 file changed, 104 insertions(+) diff --git a/tests/test_advanced_goto_functions.py b/tests/test_advanced_goto_functions.py index a2de194..be59d2a 100644 --- a/tests/test_advanced_goto_functions.py +++ b/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 @@ -286,3 +287,106 @@ def test_get_goto_joints_request(reachy_sdk_zeroed: ReachySDK) -> None: cancel = reachy_sdk_zeroed.cancel_all_goto() 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) From a55a29a221dfc614af700c85ffb91435c6e7e65d Mon Sep 17 00:00:00 2001 From: glannuzel Date: Thu, 25 Jan 2024 15:22:13 +0100 Subject: [PATCH 5/6] Add description. --- src/reachy2_sdk/reachy_sdk.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/reachy2_sdk/reachy_sdk.py b/src/reachy2_sdk/reachy_sdk.py index a034306..7fc34aa 100644 --- a/src/reachy2_sdk/reachy_sdk.py +++ b/src/reachy2_sdk/reachy_sdk.py @@ -596,6 +596,11 @@ 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 From 6e5963338129e06a8d9489bdde8e91feaf59dc63 Mon Sep 17 00:00:00 2001 From: glannuzel Date: Tue, 30 Jan 2024 10:31:50 +0100 Subject: [PATCH 6/6] Update test_advanced_goto_functions.py --- tests/test_advanced_goto_functions.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/tests/test_advanced_goto_functions.py b/tests/test_advanced_goto_functions.py index 6fb5ec5..9ffccad 100644 --- a/tests/test_advanced_goto_functions.py +++ b/tests/test_advanced_goto_functions.py @@ -391,6 +391,7 @@ def test_reachy_home(reachy_sdk_zeroed: ReachySDK) -> None: 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) @@ -432,6 +433,7 @@ def test_is_goto_finished(reachy_sdk_zeroed: ReachySDK) -> None: assert reachy_sdk_zeroed.is_goto_finished(req5) assert not reachy_sdk_zeroed.is_goto_finished(req6) + @pytest.mark.online def test_is_goto_playing(reachy_sdk_zeroed: ReachySDK) -> None: req1 = reachy_sdk_zeroed.head.rotate_to(30, 0, 0, duration=2)