From 183ffa0f2bd87935624396210992f0e48e2959ac Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Fri, 18 Oct 2024 16:42:44 +0200
Subject: [PATCH 1/9] add function

---
 src/reachy2_sdk/parts/head.py | 77 +++++++++++++++++++++++++++++++++++
 1 file changed, 77 insertions(+)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index e0c4a3fa..dafe3726 100644
--- a/src/reachy2_sdk/parts/head.py
+++ b/src/reachy2_sdk/parts/head.py
@@ -29,6 +29,7 @@
 )
 from reachy2_sdk_api.head_pb2_grpc import HeadServiceStub
 from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Point, Quaternion, Rotation3d
+from scipy.spatial.transform import Rotation as R
 
 from ..orbita.orbita3d import Orbita3d
 from ..utils.utils import get_grpc_interpolation_mode
@@ -317,6 +318,82 @@ def look_at(
             self._wait_goto(response)
         return response
 
+    def rotate_by(
+        self,
+        roll: float = 0,
+        pitch: float = 0,
+        yaw: float = 0,
+        duration: float = 2,
+        wait: bool = False,
+        degrees: bool = True,
+        frame: str = "robot",
+        interpolation_mode: str = "minimum_jerk",
+    ) -> GoToId:
+        """Rotate the neck by the specified angles.
+
+        Args:
+            roll: The angle in degrees to rotate around the x-axis (roll). Defaults to 0.
+            pitch: The angle in degrees to rotate around the y-axis (pitch). Defaults to 0.
+            yaw: The angle in degrees to rotate around the z-axis (yaw). Defaults to 0.
+            duration: The time in seconds for the neck to reach the target posture. Defaults to 2.
+            wait: Whether to wait for the movement to complete before returning. Defaults to False.
+            degrees: Whether the angles are provided in degrees. If True, the angles will be converted to radians.
+                Defaults to True.
+            frame: The frame of reference for the rotation. Can be either "robot" or "head". Defaults to "robot".
+            interpolation_mode: The interpolation mode for the movement, either "minimum_jerk" or "linear".
+                Defaults to "minimum_jerk".
+        Raises:
+            ValueError: If the frame is not "robot" or "head".
+        """
+        if frame not in ["robot", "head"]:
+            raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'head'")
+
+        if not degrees:
+            roll, pitch, yaw = np.rad2deg([roll, pitch, yaw])
+
+        actual_rpy = self.get_current_positions()
+        target_rpy = [actual_rpy[0] + roll, actual_rpy[1] + pitch, actual_rpy[2] + yaw]
+
+        if frame == "head":
+            target_rpy = [actual_rpy[0] + roll, actual_rpy[1] + pitch, actual_rpy[2] + yaw]
+        elif frame == "robot":
+            current_rotation = R.from_euler("XYZ", actual_rpy, degrees=True).as_euler("xyz", degrees=True)
+            new_rotation = current_rotation
+            new_rotation[0] += roll
+            new_rotation[1] += pitch
+            new_rotation[2] += yaw
+            target_rpy = R.from_euler("xyz", new_rotation, degrees=True).as_euler("XYZ", degrees=True)
+
+        target = np.deg2rad(target_rpy).tolist()
+        joints_goal = NeckOrientation(
+            rotation=Rotation3d(
+                rpy=ExtEulerAngles(
+                    roll=FloatValue(value=target[0]),
+                    pitch=FloatValue(value=target[1]),
+                    yaw=FloatValue(value=target[2]),
+                )
+            )
+        )
+
+        request = GoToRequest(
+            joints_goal=JointsGoal(
+                neck_joint_goal=NeckJointGoal(
+                    id=self._part_id,
+                    joints_goal=joints_goal,
+                    duration=FloatValue(value=duration),
+                )
+            ),
+            interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
+        )
+
+        response = self._goto_stub.GoToJoints(request)
+
+        if response.id == -1:
+            self._logger.error(f"Orientation {target} was not reachable. No command sent.")
+        elif wait:
+            self._wait_goto(response)
+        return response
+
     def goto_posture(
         self,
         duration: float = 2,

From b1645b62c146b4597f5b89bb1341444757a90a1d Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Mon, 21 Oct 2024 16:49:58 +0200
Subject: [PATCH 2/9] with tests and scipy

---
 src/reachy2_sdk/parts/head.py                 | 26 +++++------
 .../online/test_advanced_goto_functions.py    | 43 +++++++++++++++++++
 2 files changed, 57 insertions(+), 12 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index dafe3726..f5fe618f 100644
--- a/src/reachy2_sdk/parts/head.py
+++ b/src/reachy2_sdk/parts/head.py
@@ -135,8 +135,7 @@ def goto(
         wait: bool = False,
         interpolation_mode: str = "minimum_jerk",
         degrees: bool = True,
-    ) -> GoToId:
-        ...
+    ) -> GoToId: ...
 
     @overload
     def goto(
@@ -146,8 +145,7 @@ def goto(
         wait: bool = False,
         interpolation_mode: str = "minimum_jerk",
         degrees: bool = True,
-    ) -> GoToId:
-        ...
+    ) -> GoToId: ...
 
     def goto(
         self,
@@ -344,7 +342,13 @@ def rotate_by(
                 Defaults to "minimum_jerk".
         Raises:
             ValueError: If the frame is not "robot" or "head".
+            ValueError: If the duration is set to 0.
+            ValueError: If the interpolation mode is not "minimum_jerk" or "linear".
         """
+        if duration == 0:
+            raise ValueError("duration cannot be set to 0.")
+        if interpolation_mode not in ["minimum_jerk", "linear"]:
+            raise ValueError(f"Unknown interpolation mode {interpolation_mode}! Should be 'minimum_jerk' or 'linear'")
         if frame not in ["robot", "head"]:
             raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'head'")
 
@@ -352,18 +356,16 @@ def rotate_by(
             roll, pitch, yaw = np.rad2deg([roll, pitch, yaw])
 
         actual_rpy = self.get_current_positions()
-        target_rpy = [actual_rpy[0] + roll, actual_rpy[1] + pitch, actual_rpy[2] + yaw]
+        current_rotation = R.from_euler("XYZ", actual_rpy, degrees=True)
 
         if frame == "head":
-            target_rpy = [actual_rpy[0] + roll, actual_rpy[1] + pitch, actual_rpy[2] + yaw]
+            additional_rotation = R.from_euler("xyz", [roll, pitch, yaw], degrees=True)
+            new_rotation = current_rotation * additional_rotation
         elif frame == "robot":
-            current_rotation = R.from_euler("XYZ", actual_rpy, degrees=True).as_euler("xyz", degrees=True)
-            new_rotation = current_rotation
-            new_rotation[0] += roll
-            new_rotation[1] += pitch
-            new_rotation[2] += yaw
-            target_rpy = R.from_euler("xyz", new_rotation, degrees=True).as_euler("XYZ", degrees=True)
+            additional_rotation = R.from_euler("XYZ", [roll, pitch, yaw], degrees=True)
+            new_rotation = additional_rotation * current_rotation
 
+        target_rpy = new_rotation.as_euler("XYZ", degrees=True)
         target = np.deg2rad(target_rpy).tolist()
         joints_goal = NeckOrientation(
             rotation=Rotation3d(
diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py
index 15371e6c..0a4057ce 100644
--- a/tests/units/online/test_advanced_goto_functions.py
+++ b/tests/units/online/test_advanced_goto_functions.py
@@ -848,3 +848,46 @@ def test_rotate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None:
 
     with pytest.raises(ValueError):
         reachy_sdk_zeroed.r_arm.forward_kinematics(reachy_sdk_zeroed.get_goto_joints_request(req8).goal_positions)
+
+
+@pytest.mark.online
+def test_head_rotation(reachy_sdk_zeroed: ReachySDK) -> None:
+    reachy_sdk_zeroed.head.goto_posture("default")
+    reachy_sdk_zeroed.head.rotate_by(roll=-10, yaw=90, frame="robot")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="head")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(roll=30, frame="robot")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [30, 0, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="robot")
+    reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="head")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [-30, 0, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(pitch=-30, frame="head")
+    reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="robot")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03)
+
+    reachy_sdk_zeroed.head.rotate_by(yaw=30, frame="head")
+    current_orientation = reachy_sdk_zeroed.head.get_current_positions()
+    assert np.allclose(current_orientation, [0, 30, 120], atol=1e-03)
+
+    with pytest.raises(ValueError):
+        reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head", duration=0)
+
+    with pytest.raises(ValueError):
+        reachy_sdk_zeroed.head.rotate_by(roll=30, frame="coucou")
+
+    with pytest.raises(ValueError):
+        reachy_sdk_zeroed.head.rotate_by(roll=30, interpolation_mode="coucou")

From 9c0840db6a475572ccdbd2533bba2f09609909bb Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Mon, 21 Oct 2024 18:11:30 +0200
Subject: [PATCH 3/9] switch from scipy to pyquaternion

---
 src/reachy2_sdk/parts/head.py                 | 34 ++++++-------------
 src/reachy2_sdk/utils/utils.py                | 28 +++++++++++++++
 .../online/test_advanced_goto_functions.py    | 33 +++++++++---------
 3 files changed, 55 insertions(+), 40 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index f5fe618f..993fab62 100644
--- a/src/reachy2_sdk/parts/head.py
+++ b/src/reachy2_sdk/parts/head.py
@@ -29,10 +29,9 @@
 )
 from reachy2_sdk_api.head_pb2_grpc import HeadServiceStub
 from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Point, Quaternion, Rotation3d
-from scipy.spatial.transform import Rotation as R
 
 from ..orbita.orbita3d import Orbita3d
-from ..utils.utils import get_grpc_interpolation_mode
+from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler
 from .goto_based_part import IGoToBasedPart
 from .joints_based_part import JointsBasedPart
 
@@ -135,7 +134,8 @@ def goto(
         wait: bool = False,
         interpolation_mode: str = "minimum_jerk",
         degrees: bool = True,
-    ) -> GoToId: ...
+    ) -> GoToId:
+        ...
 
     @overload
     def goto(
@@ -145,7 +145,8 @@ def goto(
         wait: bool = False,
         interpolation_mode: str = "minimum_jerk",
         degrees: bool = True,
-    ) -> GoToId: ...
+    ) -> GoToId:
+        ...
 
     def goto(
         self,
@@ -355,27 +356,14 @@ def rotate_by(
         if not degrees:
             roll, pitch, yaw = np.rad2deg([roll, pitch, yaw])
 
-        actual_rpy = self.get_current_positions()
-        current_rotation = R.from_euler("XYZ", actual_rpy, degrees=True)
-
+        current_quaternion = self.get_current_orientation()
+        additional_quaternion = quaternion_from_euler(roll, pitch, yaw, degrees=True)
         if frame == "head":
-            additional_rotation = R.from_euler("xyz", [roll, pitch, yaw], degrees=True)
-            new_rotation = current_rotation * additional_rotation
+            target = current_quaternion * additional_quaternion
         elif frame == "robot":
-            additional_rotation = R.from_euler("XYZ", [roll, pitch, yaw], degrees=True)
-            new_rotation = additional_rotation * current_rotation
-
-        target_rpy = new_rotation.as_euler("XYZ", degrees=True)
-        target = np.deg2rad(target_rpy).tolist()
-        joints_goal = NeckOrientation(
-            rotation=Rotation3d(
-                rpy=ExtEulerAngles(
-                    roll=FloatValue(value=target[0]),
-                    pitch=FloatValue(value=target[1]),
-                    yaw=FloatValue(value=target[2]),
-                )
-            )
-        )
+            target = additional_quaternion * current_quaternion
+
+        joints_goal = NeckOrientation(rotation=Rotation3d(q=Quaternion(w=target.w, x=target.x, y=target.y, z=target.z)))
 
         request = GoToRequest(
             joints_goal=JointsGoal(
diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py
index 3336b331..6ac4dc8f 100644
--- a/src/reachy2_sdk/utils/utils.py
+++ b/src/reachy2_sdk/utils/utils.py
@@ -293,6 +293,34 @@ def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool
     return pose
 
 
+def quaternion_from_euler(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion:
+    """Convert Euler angles to a quaternion using the pyquaternion library."""
+    if degrees:
+        roll = np.deg2rad(roll)
+        pitch = np.deg2rad(pitch)
+        yaw = np.deg2rad(yaw)
+
+    qx = Quaternion(axis=[1, 0, 0], angle=roll)
+    qy = Quaternion(axis=[0, 1, 0], angle=pitch)
+    qz = Quaternion(axis=[0, 0, 1], angle=yaw)
+
+    quaternion = qz * qy * qx
+
+    return quaternion
+
+
+def euler_from_quaternion(quaternion: Quaternion, degrees: bool = True) -> Tuple[float, float, float]:
+    """Convert a quaternion back to Euler angles."""
+    yaw, pitch, roll = quaternion.yaw_pitch_roll
+
+    if degrees:
+        roll = np.rad2deg(roll)
+        pitch = np.rad2deg(pitch)
+        yaw = np.rad2deg(yaw)
+
+    return roll, pitch, yaw
+
+
 def rotate_in_self(frame: npt.NDArray[np.float64], rotation: List[float], degrees: bool = True) -> npt.NDArray[np.float64]:
     """Return a new homogeneous 4x4 pose matrix that is the input matrix rotated in itself.
 
diff --git a/tests/units/online/test_advanced_goto_functions.py b/tests/units/online/test_advanced_goto_functions.py
index 0a4057ce..82579030 100644
--- a/tests/units/online/test_advanced_goto_functions.py
+++ b/tests/units/online/test_advanced_goto_functions.py
@@ -852,36 +852,35 @@ def test_rotate_by_gripper_frame(reachy_sdk_zeroed: ReachySDK) -> None:
 
 @pytest.mark.online
 def test_head_rotation(reachy_sdk_zeroed: ReachySDK) -> None:
-    reachy_sdk_zeroed.head.goto_posture("default")
-    reachy_sdk_zeroed.head.rotate_by(roll=-10, yaw=90, frame="robot")
+    reachy_sdk_zeroed.head.goto([0, 0, 90], duration=1, wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [0, 0, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head")
+    reachy_sdk_zeroed.head.rotate_by(roll=30, duration=1, frame="head", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [0, 30, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="head")
+    reachy_sdk_zeroed.head.rotate_by(roll=-30, duration=1, frame="head", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [0, 0, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [0, 0, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(roll=30, frame="robot")
+    reachy_sdk_zeroed.head.rotate_by(roll=30, duration=1, frame="robot", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [30, 0, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [30, 0, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(roll=-30, frame="robot")
-    reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="head")
+    reachy_sdk_zeroed.head.rotate_by(roll=-30, duration=1, frame="robot", wait=True)
+    reachy_sdk_zeroed.head.rotate_by(pitch=30, duration=1, frame="head", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [-30, 0, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [-30, 0, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(pitch=-30, frame="head")
-    reachy_sdk_zeroed.head.rotate_by(pitch=30, frame="robot")
+    reachy_sdk_zeroed.head.rotate_by(pitch=-30, duration=1, frame="head", wait=True)
+    reachy_sdk_zeroed.head.rotate_by(pitch=30, duration=1, frame="robot", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [0, 30, 90], atol=1e-03)
+    assert np.allclose(current_orientation, [0, 30, 90], atol=5)
 
-    reachy_sdk_zeroed.head.rotate_by(yaw=30, frame="head")
+    reachy_sdk_zeroed.head.rotate_by(yaw=30, duration=1, frame="head", wait=True)
     current_orientation = reachy_sdk_zeroed.head.get_current_positions()
-    assert np.allclose(current_orientation, [0, 30, 120], atol=1e-03)
+    assert np.allclose(current_orientation, [0, 30, 120], atol=5)
 
     with pytest.raises(ValueError):
         reachy_sdk_zeroed.head.rotate_by(roll=30, frame="head", duration=0)

From e66970418e56a3d311f88781889ee684d3c69ad3 Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Mon, 21 Oct 2024 18:15:50 +0200
Subject: [PATCH 4/9] correct docstring error

---
 src/reachy2_sdk/parts/head.py | 7 +++----
 1 file changed, 3 insertions(+), 4 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index 993fab62..b59754a7 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,
@@ -341,6 +339,7 @@ def rotate_by(
             frame: The frame of reference for the rotation. Can be either "robot" or "head". Defaults to "robot".
             interpolation_mode: The interpolation mode for the movement, either "minimum_jerk" or "linear".
                 Defaults to "minimum_jerk".
+
         Raises:
             ValueError: If the frame is not "robot" or "head".
             ValueError: If the duration is set to 0.

From d8652176b92321ed6ce1b07b46c12659cbd36611 Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Mon, 21 Oct 2024 18:18:18 +0200
Subject: [PATCH 5/9] correct linter error

---
 src/reachy2_sdk/parts/head.py | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index b59754a7..b374b7d0 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,
@@ -340,6 +342,7 @@ def rotate_by(
             interpolation_mode: The interpolation mode for the movement, either "minimum_jerk" or "linear".
                 Defaults to "minimum_jerk".
 
+
         Raises:
             ValueError: If the frame is not "robot" or "head".
             ValueError: If the duration is set to 0.

From d0c85189ddf38695af1b3d9e9836c56fb50cd478 Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Tue, 22 Oct 2024 16:35:44 +0200
Subject: [PATCH 6/9] modification post-review

---
 src/reachy2_sdk/parts/head.py  | 51 +++++++++++++++-------------------
 src/reachy2_sdk/utils/utils.py | 27 +++++++++---------
 2 files changed, 36 insertions(+), 42 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index b374b7d0..f48b3611 100644
--- a/src/reachy2_sdk/parts/head.py
+++ b/src/reachy2_sdk/parts/head.py
@@ -348,43 +348,38 @@ def rotate_by(
             ValueError: If the duration is set to 0.
             ValueError: If the interpolation mode is not "minimum_jerk" or "linear".
         """
-        if duration == 0:
-            raise ValueError("duration cannot be set to 0.")
-        if interpolation_mode not in ["minimum_jerk", "linear"]:
-            raise ValueError(f"Unknown interpolation mode {interpolation_mode}! Should be 'minimum_jerk' or 'linear'")
         if frame not in ["robot", "head"]:
             raise ValueError(f"Unknown frame {frame}! Should be 'robot' or 'head'")
-
         if not degrees:
             roll, pitch, yaw = np.rad2deg([roll, pitch, yaw])
 
-        current_quaternion = self.get_current_orientation()
-        additional_quaternion = quaternion_from_euler(roll, pitch, yaw, degrees=True)
-        if frame == "head":
-            target = current_quaternion * additional_quaternion
-        elif frame == "robot":
-            target = additional_quaternion * current_quaternion
+        try:
+            goto = self.get_goto_queue()[-1]
+        except IndexError:
+            goto = self.get_goto_playing()
 
-        joints_goal = NeckOrientation(rotation=Rotation3d(q=Quaternion(w=target.w, x=target.x, y=target.y, z=target.z)))
+        if goto.id != -1:
+            joints_request = self._get_goto_joints_request(goto)
+        else:
+            joints_request = None
 
-        request = GoToRequest(
-            joints_goal=JointsGoal(
-                neck_joint_goal=NeckJointGoal(
-                    id=self._part_id,
-                    joints_goal=joints_goal,
-                    duration=FloatValue(value=duration),
-                )
-            ),
-            interpolation_mode=get_grpc_interpolation_mode(interpolation_mode),
-        )
+        if joints_request is not None:
+            initial_orientation = joints_request.goal_positions
+            initial_orientation[1] += 10
+            initial_quaternion = quaternion_from_euler(
+                initial_orientation[0], initial_orientation[1], initial_orientation[2], degrees=True
+            )
+        else:
+            initial_quaternion = self.get_current_orientation()
 
-        response = self._goto_stub.GoToJoints(request)
+        additional_quaternion = quaternion_from_euler(roll, pitch, yaw, degrees=True)
 
-        if response.id == -1:
-            self._logger.error(f"Orientation {target} was not reachable. No command sent.")
-        elif wait:
-            self._wait_goto(response)
-        return response
+        if frame == "head":
+            target_quaternion = initial_quaternion * additional_quaternion
+        elif frame == "robot":
+            target_quaternion = additional_quaternion * initial_quaternion
+
+        return self.goto(target_quaternion, duration, wait, interpolation_mode)
 
     def goto_posture(
         self,
diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py
index 6ac4dc8f..c06053e7 100644
--- a/src/reachy2_sdk/utils/utils.py
+++ b/src/reachy2_sdk/utils/utils.py
@@ -294,7 +294,18 @@ def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool
 
 
 def quaternion_from_euler(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion:
-    """Convert Euler angles to a quaternion using the pyquaternion library."""
+    """Convert Euler angles (intrinsic XYZ order) to a quaternion using the pyquaternion library.
+
+    Args:
+        roll (float): Rotation angle around the X-axis (roll), in degrees by default.
+        pitch (float): Rotation angle around the Y-axis (pitch), in degrees by default.
+        yaw (float): Rotation angle around the Z-axis (yaw), in degrees by default.
+        degrees (bool): If True, the input angles are interpreted as degrees. If False, they are
+            interpreted as radians. Defaults to True.
+
+    Returns:
+        Quaternion: The quaternion representing the combined rotation in 3D space.
+    """
     if degrees:
         roll = np.deg2rad(roll)
         pitch = np.deg2rad(pitch)
@@ -304,23 +315,11 @@ def quaternion_from_euler(roll: float, pitch: float, yaw: float, degrees: bool =
     qy = Quaternion(axis=[0, 1, 0], angle=pitch)
     qz = Quaternion(axis=[0, 0, 1], angle=yaw)
 
-    quaternion = qz * qy * qx
+    quaternion = qx * qy * qz
 
     return quaternion
 
 
-def euler_from_quaternion(quaternion: Quaternion, degrees: bool = True) -> Tuple[float, float, float]:
-    """Convert a quaternion back to Euler angles."""
-    yaw, pitch, roll = quaternion.yaw_pitch_roll
-
-    if degrees:
-        roll = np.rad2deg(roll)
-        pitch = np.rad2deg(pitch)
-        yaw = np.rad2deg(yaw)
-
-    return roll, pitch, yaw
-
-
 def rotate_in_self(frame: npt.NDArray[np.float64], rotation: List[float], degrees: bool = True) -> npt.NDArray[np.float64]:
     """Return a new homogeneous 4x4 pose matrix that is the input matrix rotated in itself.
 

From e4a8e0dea107f4225b149ef62f9092a3735ee1fc Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Tue, 22 Oct 2024 17:45:41 +0200
Subject: [PATCH 7/9] add unit test

---
 src/reachy2_sdk/parts/head.py     |  6 +++---
 src/reachy2_sdk/utils/utils.py    |  2 +-
 tests/units/offline/test_utils.py | 25 +++++++++++++++++++++++++
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index f48b3611..ab5642e0 100644
--- a/src/reachy2_sdk/parts/head.py
+++ b/src/reachy2_sdk/parts/head.py
@@ -31,7 +31,7 @@
 from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles, Point, Quaternion, Rotation3d
 
 from ..orbita.orbita3d import Orbita3d
-from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler
+from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler_angles
 from .goto_based_part import IGoToBasedPart
 from .joints_based_part import JointsBasedPart
 
@@ -366,13 +366,13 @@ def rotate_by(
         if joints_request is not None:
             initial_orientation = joints_request.goal_positions
             initial_orientation[1] += 10
-            initial_quaternion = quaternion_from_euler(
+            initial_quaternion = quaternion_from_euler_angles(
                 initial_orientation[0], initial_orientation[1], initial_orientation[2], degrees=True
             )
         else:
             initial_quaternion = self.get_current_orientation()
 
-        additional_quaternion = quaternion_from_euler(roll, pitch, yaw, degrees=True)
+        additional_quaternion = quaternion_from_euler_angles(roll, pitch, yaw, degrees=True)
 
         if frame == "head":
             target_quaternion = initial_quaternion * additional_quaternion
diff --git a/src/reachy2_sdk/utils/utils.py b/src/reachy2_sdk/utils/utils.py
index c06053e7..7517aa10 100644
--- a/src/reachy2_sdk/utils/utils.py
+++ b/src/reachy2_sdk/utils/utils.py
@@ -293,7 +293,7 @@ def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool
     return pose
 
 
-def quaternion_from_euler(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion:
+def quaternion_from_euler_angles(roll: float, pitch: float, yaw: float, degrees: bool = True) -> Quaternion:
     """Convert Euler angles (intrinsic XYZ order) to a quaternion using the pyquaternion library.
 
     Args:
diff --git a/tests/units/offline/test_utils.py b/tests/units/offline/test_utils.py
index 4d5bff34..134b3856 100644
--- a/tests/units/offline/test_utils.py
+++ b/tests/units/offline/test_utils.py
@@ -1,6 +1,7 @@
 import numpy as np
 import pytest
 from google.protobuf.wrappers_pb2 import FloatValue
+from pyquaternion import Quaternion
 from reachy2_sdk_api.kinematics_pb2 import ExtEulerAngles
 
 from reachy2_sdk.utils.utils import (
@@ -15,6 +16,7 @@
     invert_affine_transformation_matrix,
     list_to_arm_position,
     matrix_from_euler_angles,
+    quaternion_from_euler_angles,
     rotate_in_self,
     translate_in_self,
 )
@@ -140,6 +142,29 @@ def test_get_pose_matrix() -> None:
         get_pose_matrix([0.1, 0.2, 0.1], [-20, -90, -50, 10])
 
 
+@pytest.mark.offline
+def test_quaternion_from_euler_angles() -> None:
+    q1 = quaternion_from_euler_angles(0, 0, 0)
+    assert q1 == Quaternion(1, 0, 0, 0)
+    q2 = quaternion_from_euler_angles(90, 0, 0)
+    assert q2 == Quaternion(axis=[1, 0, 0], angle=np.pi / 2)
+    q3 = quaternion_from_euler_angles(0, 90, 0)
+    assert q3 == Quaternion(axis=[0, 1, 0], angle=np.pi / 2)
+    q4 = quaternion_from_euler_angles(0, 0, 90)
+    assert q4 == Quaternion(axis=[0, 0, 1], angle=np.pi / 2)
+    q5 = quaternion_from_euler_angles(45, 45, 45).normalised
+    expected_quat = Quaternion(axis=[0.57735027, 0.57735027, 0.57735027], angle=np.pi / 4).normalised
+    dot_product = np.dot(q5.elements, expected_quat.elements)
+    assert np.isclose(dot_product, 1.0, atol=1e-1) or np.isclose(dot_product, -1.0, atol=1e-1)
+
+    q6 = quaternion_from_euler_angles(np.pi / 2, 0, 0, degrees=False)
+    assert q6 == Quaternion(axis=[1, 0, 0], angle=np.pi / 2)
+    q7 = quaternion_from_euler_angles(0, np.pi / 2, 0, degrees=False)
+    assert q7 == Quaternion(axis=[0, 1, 0], angle=np.pi / 2)
+    q8 = quaternion_from_euler_angles(0, 0, np.pi / 2, degrees=False)
+    assert q8 == Quaternion(axis=[0, 0, 1], angle=np.pi / 2)
+
+
 @pytest.mark.offline
 def test_rotate_in_self() -> None:
     A = get_pose_matrix([0.2, -0.2, -0.1], [0, -90, 0])

From 8a75a7f6af7cfc3ba78d8bda1ac7675c4d731608 Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Wed, 23 Oct 2024 14:41:56 +0200
Subject: [PATCH 8/9] =?UTF-8?q?modify=20test=20and=20add=20explanation=20o?=
 =?UTF-8?q?n=2010=C2=B0=20offset?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 src/reachy2_sdk/parts/head.py     | 9 +++++----
 tests/units/offline/test_utils.py | 7 ++++---
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/src/reachy2_sdk/parts/head.py b/src/reachy2_sdk/parts/head.py
index ab5642e0..b364c423 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,
@@ -365,6 +363,9 @@ def rotate_by(
 
         if joints_request is not None:
             initial_orientation = joints_request.goal_positions
+
+            # as there is a 10° offset between the joint space
+            # and the zero position in cartesian space in Reachy's frame for the yaw joint :
             initial_orientation[1] += 10
             initial_quaternion = quaternion_from_euler_angles(
                 initial_orientation[0], initial_orientation[1], initial_orientation[2], degrees=True
diff --git a/tests/units/offline/test_utils.py b/tests/units/offline/test_utils.py
index 134b3856..8c368467 100644
--- a/tests/units/offline/test_utils.py
+++ b/tests/units/offline/test_utils.py
@@ -153,9 +153,10 @@ def test_quaternion_from_euler_angles() -> None:
     q4 = quaternion_from_euler_angles(0, 0, 90)
     assert q4 == Quaternion(axis=[0, 0, 1], angle=np.pi / 2)
     q5 = quaternion_from_euler_angles(45, 45, 45).normalised
-    expected_quat = Quaternion(axis=[0.57735027, 0.57735027, 0.57735027], angle=np.pi / 4).normalised
-    dot_product = np.dot(q5.elements, expected_quat.elements)
-    assert np.isclose(dot_product, 1.0, atol=1e-1) or np.isclose(dot_product, -1.0, atol=1e-1)
+    q5_ref = Quaternion(axis=[0.679, 0.281, 0.679], angle=convert_to_radians(85.8))
+    unit_quat = Quaternion()
+    expected_unit = q5 * q5_ref.inverse
+    assert np.allclose(unit_quat.elements, expected_unit.elements, atol=1e-3)
 
     q6 = quaternion_from_euler_angles(np.pi / 2, 0, 0, degrees=False)
     assert q6 == Quaternion(axis=[1, 0, 0], angle=np.pi / 2)

From 4448755a45bef27bf2ee1677dd1cb7a0edee806a Mon Sep 17 00:00:00 2001
From: ClaireHzl <126695207+ClaireHzl@users.noreply.github.com>
Date: Wed, 23 Oct 2024 14:55:02 +0200
Subject: [PATCH 9/9] correct 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 b364c423..e012002f 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,