diff --git a/README.md b/README.md index 644be84..d96ad5a 100644 --- a/README.md +++ b/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 diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index f7d40c0..554189e 100644 --- a/src/liecasadi/dualquaternion.py +++ b/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,10 +58,45 @@ 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() @@ -51,44 +104,98 @@ def from_quaternion_and_translation( @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] diff --git a/src/liecasadi/se3.py b/src/liecasadi/se3.py index faa81f4..814212f 100644 --- a/src/liecasadi/se3.py +++ b/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( diff --git a/tests/test_dual_quaternions.py b/tests/test_dual_quaternions.py new file mode 100644 index 0000000..60ec264 --- /dev/null +++ b/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 + )