Add from_SE3 method and test

ami-iit · May 23, 2023 · 69bd269 · 69bd269
1 parent 7fd1210
commit 69bd269
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 0 deletions.
diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py
@@ -118,6 +118,21 @@ def from_matrix(m: Matrix) -> "DualQuaternion":
         qd = 0.5 * (t * r).coeffs()
         return DualQuaternion(qr=r.coeffs(), qd=qd)
 
+    @staticmethod
+    def from_SE3(se3: SE3) -> "DualQuaternion":
+        """Build dual quaternion from an SE3 object
+
+        Args:
+            se3 (SE3): an SE3 object
+
+        Returns:
+            DualQuaternion: a dual quaternion
+        """
+        r = se3.rotation().as_quat()
+        t = Quaternion(cs.vertcat(se3.translation(), 0))
+        qd = 0.5 * (t * r).coeffs()
+        return DualQuaternion(qr=r.coeffs(), qd=qd)
+
     def coeffs(self) -> Vector:
         """
         Returns:

diff --git a/tests/test_dual_quaternions.py b/tests/test_dual_quaternions.py
@@ -1,5 +1,6 @@
 import numpy as np
 import pytest
+import casadi as cs
 
 from liecasadi import SE3, DualQuaternion
 
@@ -21,6 +22,7 @@
 dual_q1 = DualQuaternion.from_matrix(H1)
 dual_q2 = DualQuaternion.from_matrix(H2)
 
+dual_q_from_se3 = DualQuaternion.from_SE3(SE3.from_position_quaternion(pos, quat))
 
 def test_concatenation():
     concat_dual_q = dual_q1 * dual_q2
@@ -54,3 +56,6 @@ def test_inverse():
     assert dual_q1.inverse().as_matrix() - np.linalg.inv(H1) == pytest.approx(
         0.0, abs=1e-4
     )
+
+def test_from_SE3():
+    assert dual_q_from_se3.as_matrix() - H1 == pytest.approx(0.0, abs=1e-4)