From 5d5b0feacb8c3c5a18e9b3e4c57bcacc67afbe78 Mon Sep 17 00:00:00 2001 From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com> Date: Thu, 5 Dec 2024 16:06:53 +0100 Subject: [PATCH 1/3] id -1 on goto_singlejoint if part is off --- src/reachy2_sdk/orbita/orbita_joint.py | 4 ++++ tests/units/online/test_advanced_goto_functions.py | 5 +++++ 2 files changed, 9 insertions(+) diff --git a/src/reachy2_sdk/orbita/orbita_joint.py b/src/reachy2_sdk/orbita/orbita_joint.py index 8d3ee8b8..ade4dac3 100644 --- a/src/reachy2_sdk/orbita/orbita_joint.py +++ b/src/reachy2_sdk/orbita/orbita_joint.py @@ -108,6 +108,10 @@ def goto( Returns: The GoToId associated with the movement command. """ + if self._actuator._part.is_off(): + self._actuator._logger.warning(f"{self._actuator._part._part_id.name} is off. No command sent.") + return GoToId(id=-1) + return self._actuator._part._goto_single_joint( self._position_order_in_part, goal_position=goal_position, diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py index b7b27d69..cfaf879c 100644 --- a/tests/units/online/test_advanced_goto_functions.py +++ b/tests/units/online/test_advanced_goto_functions.py @@ -574,6 +574,11 @@ def test_single_joint_goto(reachy_sdk_zeroed: ReachySDK) -> None: assert np.allclose(reachy_sdk_zeroed.head.get_current_positions(), [15, 0, 10], atol=1e-01) + reachy_sdk_zeroed.turn_off() + assert reachy_sdk_zeroed.l_arm.shoulder.roll.goto(10).id == -1 + assert reachy_sdk_zeroed.head.neck.roll.goto(10).id == -1 + reachy_sdk_zeroed.turn_on() + @pytest.mark.online def test_get_translation_by(reachy_sdk_zeroed: ReachySDK) -> None: From affbda079e63a543e307f81f751c0ebc717ba67a Mon Sep 17 00:00:00 2001 From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com> Date: Mon, 9 Dec 2024 09:53:44 +0100 Subject: [PATCH 2/3] add of is_off method in orbita --- src/reachy2_sdk/orbita/orbita.py | 8 ++++++++ src/reachy2_sdk/parts/head.py | 8 +++----- 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/reachy2_sdk/orbita/orbita.py b/src/reachy2_sdk/orbita/orbita.py index 3fc8dca9..aaf0b577 100644 --- a/src/reachy2_sdk/orbita/orbita.py +++ b/src/reachy2_sdk/orbita/orbita.py @@ -192,6 +192,14 @@ def is_on(self) -> bool: """ return not self._compliant + def is_off(self) -> bool: + """Check if the actuator is currently compliant. + + Returns: + `True` if the actuator is compliant (not stiff), `False` otherwise. + """ + return self._compliant + @property def temperatures(self) -> Dict[str, float]: """Get the current temperatures of all the motors in the actuator. diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index 47128373..5a6f353f 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -134,8 +134,7 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: - ... + ) -> GoToId: ... @overload def goto( @@ -145,8 +144,7 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: - ... + ) -> GoToId: ... def goto( self, @@ -314,7 +312,7 @@ def look_at( """ if duration == 0: raise ValueError("duration cannot be set to 0.") - if not self.neck.is_on(): + if self.neck.is_off(): self._logger.warning("head.neck is off. No command sent.") return GoToId(id=-1) From 309f5822de1937fce29d55256b8b7640d9745663 Mon Sep 17 00:00:00 2001 From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com> Date: Mon, 9 Dec 2024 09:54:54 +0100 Subject: [PATCH 3/3] fix black error --- src/reachy2_sdk/parts/head.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py index 5a6f353f..7678fee1 100644 --- a/src/reachy2_sdk/parts/head.py +++ b/src/reachy2_sdk/parts/head.py @@ -134,7 +134,8 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: ... + ) -> GoToId: + ... @overload def goto( @@ -144,7 +145,8 @@ def goto( wait: bool = False, interpolation_mode: str = "minimum_jerk", degrees: bool = True, - ) -> GoToId: ... + ) -> GoToId: + ... def goto( self,