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