switch from scipy to pyquaternion

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(

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

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