From ec27d7433f0130e23121b983fb934b9b2a8b2a1c Mon Sep 17 00:00:00 2001
From: aestene <arnts@equinor.com>
Date: Thu, 25 Mar 2021 14:03:36 +0100
Subject: [PATCH] Add function to convert quaternion to euler

---
 src/alitra/convert/__init__.py   |  1 +
 src/alitra/convert/quaternion.py | 22 ++++++++++++++++++++++
 tests/convert/__init__.py        |  0
 tests/convert/test_quaternion.py | 31 +++++++++++++++++++++++++++++++
 4 files changed, 54 insertions(+)
 create mode 100644 src/alitra/convert/__init__.py
 create mode 100644 src/alitra/convert/quaternion.py
 create mode 100644 tests/convert/__init__.py
 create mode 100644 tests/convert/test_quaternion.py

diff --git a/src/alitra/convert/__init__.py b/src/alitra/convert/__init__.py
new file mode 100644
index 0000000..ba5fd37
--- /dev/null
+++ b/src/alitra/convert/__init__.py
@@ -0,0 +1 @@
+from alitra.convert.quaternion import quaternion_to_euler
diff --git a/src/alitra/convert/quaternion.py b/src/alitra/convert/quaternion.py
new file mode 100644
index 0000000..aaa7e9c
--- /dev/null
+++ b/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
diff --git a/tests/convert/__init__.py b/tests/convert/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/tests/convert/test_quaternion.py b/tests/convert/test_quaternion.py
new file mode 100644
index 0000000..634faa6
--- /dev/null
+++ b/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())