Merge pull request #434 from pollen-robotics/433-add_rotate_for_head

433 add rotate for head

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
+from ..utils.utils import get_grpc_interpolation_mode, quaternion_from_euler_angles
 from .goto_based_part import IGoToBasedPart
 from .joints_based_part import JointsBasedPart
 
@@ -317,6 +317,73 @@ 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".
+            ValueError: If the duration is set to 0.
+            ValueError: If the interpolation mode is not "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])
+
+        try:
+            goto = self.get_goto_queue()[-1]
+        except IndexError:
+            goto = self.get_goto_playing()
+
+        if goto.id != -1:
+            joints_request = self._get_goto_joints_request(goto)
+        else:
+            joints_request = None
+
+        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
+            )
+        else:
+            initial_quaternion = self.get_current_orientation()
+
+        additional_quaternion = quaternion_from_euler_angles(roll, pitch, yaw, degrees=True)
+
+        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,
         duration: float = 2,

src/reachy2_sdk/utils/utils.py

@@ -293,6 +293,33 @@ def get_pose_matrix(position: List[float], rotation: List[float], degrees: bool
     return pose
 
 
+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:
+        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)
+        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 = qx * qy * qz
+
+    return quaternion
+
+
 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.
 

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,30 @@ 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
+    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)
+    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])

tests/units/online/test_advanced_goto_functions.py

@@ -848,3 +848,45 @@ 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([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=5)
+
+    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=5)
+
+    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=5)
+
+    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=5)
+
+    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=5)
+
+    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=5)
+
+    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=5)
+
+    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")