Add function to convert quaternion to euler

src/alitra/convert/__init__.py

@@ -0,0 +1 @@
+from alitra.convert.quaternion import quaternion_to_euler

src/alitra/convert/quaternion.py

@@ -0,0 +1,22 @@
+from scipy.spatial.transform.rotation import Rotation
+
+from alitra import Euler, Quaternion
+
+
+def quaternion_to_euler(
+    quaternion: Quaternion, sequence: str = "ZYX", degrees: bool = False
+) -> Euler:
+    """
+    Transform a quaternion into Euler angles.
+    :param quaternion: A Quaternion object.
+    :param sequence: Rotation sequence for the Euler angles.
+    :param degrees: Set to true if the resulting Euler angles should be in degrees. Default is radians.
+    :return: Euler object.
+    """
+    rotation_object: Rotation = Rotation(quaternion.as_np_array())
+    euler: Euler = Euler.from_array(
+        rotation_object.as_euler(sequence, degrees=degrees),
+        from_="robot",
+        to_="asset",
+    )
+    return euler

tests/convert/test_quaternion.py

@@ -0,0 +1,31 @@
+import numpy as np
+import pytest
+
+from alitra import Euler, Quaternion
+from alitra.convert import quaternion_to_euler
+
+
+@pytest.mark.parametrize(
+    "quaternion, expected",
+    [
+        (
+            Quaternion(x=0, y=0, z=0, w=1, frame="robot"),
+            Euler(psi=0, phi=0, theta=0, from_="robot", to_="asset"),
+        ),
+        (
+            Quaternion(x=0.1830127, y=0.5, z=0.5, w=0.6830127, frame="robot"),
+            Euler(
+                psi=1.5707963,  # Z
+                theta=0.5235988,  # Y
+                phi=1.0471976,  # X
+                from_="robot",
+                to_="asset",
+            ),
+        ),
+    ],
+)
+def test_quaternion_to_euler(quaternion, expected):
+    euler_angles: Euler = quaternion_to_euler(
+        quaternion=quaternion, sequence="ZYX", degrees=False
+    )
+    assert np.allclose(euler_angles.as_np_array(), expected.as_np_array())