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
 from .goto_based_part import IGoToBasedPart
 from .joints_based_part import JointsBasedPart
 
@@ -317,6 +317,75 @@ 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 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
+
+        joints_goal = NeckOrientation(rotation=Rotation3d(q=Quaternion(w=target.w, x=target.x, y=target.y, z=target.z)))
+
+        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,

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.
 

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")