Merge pull request #5 from ami-iit/feature/dual-quaternion

Feature/dual quaternion

README.md

@@ -89,12 +89,52 @@ vector6d = (np.random.rand(3) - 0.5) * 5
 tangent = SO3Tangent(vector6d)
 ```
 
+### Dual Quaternion example
+
+```python
+from liecasadi import SE3, DualQuaternion
+from numpy import np
+
+# orientation quaternion generation
+quat1 = (np.random.rand(4) - 0.5) * 5
+quat1 = quat1 / np.linalg.norm(quat1)
+quat2 = (np.random.rand(4) - 0.5) * 5
+quat2 = quat2 / np.linalg.norm(quat2)
+
+# translation vector generation
+pos1 = (np.random.rand(3) - 0.5) * 5
+pos2 = (np.random.rand(3) - 0.5) * 5
+
+dual_quaternion1 = DualQuaternion(quat1, pos1)
+dual_quaternion2 = DualQuaternion(quat2, pos2)
+
+# from a homogenous matrix
+# (using liecasadi.SE3 to generate the corresponding homogenous matrix)
+H = SE3.from_position_quaternion(pos, quat).as_matrix()
+dual_quaternion1 = DualQuaternion.from_matrix(H)
+
+# Concatenation of rigid transforms
+q1xq2 = dual_quaternion1 * dual_quaternion2
+
+# to homogeneous matrix
+print(q1xq2.as_matrix())
+
+# obtain translation
+print(q1xq2.translation())
+
+# obtain rotation
+print(q1xq2.rotation().as_matrix())
+
+# transform a point
+point = np.random.randn(3,1)
+transformed_point = dual_quaternion1.transform_point(point)
+
+# create an identity dual quaternion
+I = DualQuaternion.Identity()
+```
+
 ## 🦸‍♂️ Contributing
 
 **liecasadi** is an open-source project. Contributions are very welcome!
 
 Open an issue with your feature request or if you spot a bug. Then, you can also proceed with a Pull-requests! :rocket:
-
-## ⚠️ Work in progress
-
-- Dual Quaternion class

src/liecasadi/dualquaternion.py

@@ -14,20 +14,38 @@
 
 @dataclasses.dataclass
 class DualQuaternion:
+    """
+    Dual Quaternion class
+    Have a look at the documents https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf
+    https://faculty.sites.iastate.edu/jia/files/inline-files/dual-quaternion.pdf for some intuitions.
+    """
+
     qr: Vector
     qd: Vector
     Qr: Quaternion = field(init=False)
     Qd: Quaternion = field(init=False)
 
     def __post_init__(self):
-        warnings.warn("[DualQuaternion]: This class is under development!")
+        """Build two Quaternion objects from xyzw quaternion and xyz translation"""
         self.Qr = Quaternion(self.qr)
         self.Qd = Quaternion(self.qd)
 
     def __repr__(self) -> str:
+        """
+        Returns:
+            str: when you print the dual quaternion
+        """
         return f"Rotation quaternion: {self.Qr.xyzw} \nTranslation quaternion: {self.Qd.coeffs()}"
 
     def __mul__(self, other: "DualQuaternion") -> "DualQuaternion":
+        """Dual quaternion multiplication
+
+        Args:
+            other (DualQuaternion): a Dual Quaternion
+
+        Returns:
+            DualQuaternion: the multiplication product
+        """
         qr = self.Qr * other.Qr
         qd = self.Qr * other.Qd + self.Qd * other.Qr
         return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs())
@@ -40,55 +58,144 @@ def __rmul__(self, other: Scalar) -> "DualQuaternion":
         """
         return DualQuaternion(qr=other * self.qr, qd=other * self.qd)
 
+    def __add__(self, other: "DualQuaternion") -> "DualQuaternion":
+        """Sum of 2 Dual quaternions
+
+        Args:
+            other (DualQuaternion): a Dual Quaternion
+
+        Returns:
+            DualQuaternion: the sum of Dual Quaternions
+        """
+        qr = self.Qr + other.Qr
+        qd = self.Qd + other.Qd
+        return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs())
+
+    def __sub__(self, other: "DualQuaternion") -> "DualQuaternion":
+        """Difference of 2 Dual quaternions
+
+        Args:
+            other (DualQuaternion): a Dual Quaternion
+
+        Returns:
+            DualQuaternion: the difference of Dual Quaternions
+        """
+        qr = self.Qr - other.Qr
+        qd = self.Qd - other.Qd
+        return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs())
+
     @staticmethod
     def from_quaternion_and_translation(
         quat: Vector, transl: Vector
     ) -> "DualQuaternion":
+        """Build dual quaternion from a quaternion (xyzw) and a translation vector (xyz)
+
+        Args:
+            quat (Vector): xyzw quaternion vector
+            transl (Vector): xyz translation vector
+
+        Returns:
+            DualQuaternion: a dual quaternion
+        """
         t = Quaternion(cs.vertcat(transl, 0))
         r = Quaternion(quat)
         qd = 0.5 * (t * r).coeffs()
         return DualQuaternion(qr=r.coeffs(), qd=qd)
 
     @staticmethod
     def from_matrix(m: Matrix) -> "DualQuaternion":
+        """Build dual quaternion from an homogenous matrix
+
+        Args:
+            m (Matrix): homogenous matrix
+
+        Returns:
+            DualQuaternion: a dual quaternion
+        """
         se3 = SE3.from_matrix(m)
-        t = se3.rotation().as_quat()
-        r = Quaternion(cs.vertcat(se3.translation(), 0))
+        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:
+            Vector: the dual quaternion vector xyzwxyz
+        """
+        return cs.vertcat(self.qd, self.qr)
+
     def translation(self) -> Vector:
-        return 2.0 * (self.Qd * self.Qr.conjugate()).coeff()
+        """
+        Returns:
+            Vector: Translation vector xyz
+        """
+        return 2.0 * (self.Qd * self.Qr.conjugate()).coeffs()[:3]
 
     def rotation(self) -> SO3:
+        """
+        Returns:
+            SO3: an SO3 object
+        """
         return SO3(xyzw=self.Qr.coeffs())
 
     def inverse(self) -> "DualQuaternion":
+        """
+        Returns:
+            DualQuaternion: a dual quaternion, inverse of the original
+        """
         qr_inv = self.Qr.conjugate()
         qd = -qr_inv * self.Qd * qr_inv
-        return DualQuaternion(qr=qr_inv, qd=qd)
-
-    def translation(self):
-        return 2 * (self.Qd * self.Qr.conjugate()).coeffs()
-
+        return DualQuaternion(qr=qr_inv.coeffs(), qd=qd.coeffs())
 
-if __name__ == "__main__":
-    import numpy as np
-
-    quat = np.random.randn(4) * 4
-    quat = quat / np.linalg.norm(quat)
-
-    trans = np.random.randn(3) * 4
-    dq = DualQuaternion.from_quaternion_and_translation(quat, trans)
+    def conjugate(self) -> "DualQuaternion":
+        """
+        Returns:
+            DualQuaternion: conjugate dual quaternion
+        """
+        qr = self.Qr.conjugate()
+        qd = self.Qd.conjugate()
+        return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs())
 
-    quat2 = np.random.randn(4) * 4
-    quat2 = quat2 / np.linalg.norm(quat2)
+    def dual_conjugate(self) -> "DualQuaternion":
+        """
+        Returns:
+            DualQuaternion: dual number conjugate, used in point transformation
+        """
+        qr = self.Qr.conjugate()
+        qd = self.Qd.conjugate()
+        return DualQuaternion(qr=qr.coeffs(), qd=-qd.coeffs())
 
-    trans2 = np.random.randn(4) * 4
+    def as_matrix(self) -> Matrix:
+        """
+        Returns:
+            Matrix: the corresponding homogenous matrix
+        """
+        r = self.rotation().as_matrix()
+        t = self.translation()
+        return cs.vertcat(
+            cs.horzcat(r, t),
+            cs.horzcat([0, 0, 0, 1]).T,
+        )
 
-    dq2 = DualQuaternion(quat2, trans2)
+    @staticmethod
+    def Identity() -> "DualQuaternion":
+        """
+        Returns:
+            DualQuaternion: the identity dual quaternion
+        """
+        return DualQuaternion(
+            qr=SO3.Identity().as_quat().coeffs(), qd=Quaternion([0, 0, 0, 0]).coeffs()
+        )
 
-    d3 = DualQuaternion.from_matrix(np.eye(4))
+    def transform_point(self, xyz: Vector) -> Vector:
+        """Rototranlates a point
+        Args:
+            xyz (Vector): the point
 
-    print((3 * dq2).inverse())
-    print(dq.translation())
+        Returns:
+            Vector: the transformed point
+        """
+        p = DualQuaternion.Identity()
+        p.Qd = Quaternion([xyz[0], xyz[1], xyz[2], 0])
+        return (self * p * self.dual_conjugate()).coeffs()[:3]

src/liecasadi/se3.py

@@ -31,6 +31,9 @@ def translation(self) -> Vector:
         return self.pos
 
     def transform(self) -> Matrix:
+        return self.as_matrix()
+
+    def as_matrix(self) -> Matrix:
         a = SO3(self.xyzw)
         pos = cs.reshape(self.pos, -1, 1)
         return cs.vertcat(

tests/test_dual_quaternions.py

@@ -0,0 +1,56 @@
+import numpy as np
+import pytest
+
+from liecasadi import SE3, DualQuaternion
+
+# orientation quaternion generation
+quat = np.random.randn(4) * 5
+quat = quat / np.linalg.norm(quat)
+
+quat2 = np.random.randn(4) * 5
+quat2 = quat2 / np.linalg.norm(quat2)
+
+# translation vector generation
+pos = np.random.randn(3) * 5
+pos2 = np.random.randn(3) * 5
+
+
+H1 = SE3.from_position_quaternion(pos, quat).as_matrix()
+H2 = SE3.from_position_quaternion(pos2, quat2).as_matrix()
+
+dual_q1 = DualQuaternion.from_matrix(H1)
+dual_q2 = DualQuaternion.from_matrix(H2)
+
+
+def test_concatenation():
+    concat_dual_q = dual_q1 * dual_q2
+    concat_H = H1 @ H2
+    assert concat_dual_q.as_matrix() - concat_H == pytest.approx(0.0, abs=1e-4)
+
+
+def test_transform_point():
+    point = np.random.randn(4, 1) * 5
+    point[3] = 1.0
+    assert dual_q1.transform_point(point[:3]) - (H1 @ point)[:3] == pytest.approx(
+        0.0, abs=1e-4
+    )
+
+
+def test_to_matrix():
+    assert dual_q1.as_matrix() - H1 == pytest.approx(0.0, abs=1e-4)
+
+
+def test_translation():
+    assert dual_q1.translation() - pos == pytest.approx(0.0, abs=1e-4)
+
+
+def test_rotation():
+    assert dual_q1.rotation().as_quat().coeffs() - quat == pytest.approx(
+        0.0, abs=1e-4
+    ) or dual_q1.rotation().as_quat().coeffs() + quat == pytest.approx(0.0, abs=1e-4)
+
+
+def test_inverse():
+    assert dual_q1.inverse().as_matrix() - np.linalg.inv(H1) == pytest.approx(
+        0.0, abs=1e-4
+    )