From 8ef17e609300a5f52f36eeb39c5281de10d00920 Mon Sep 17 00:00:00 2001 From: giulero Date: Wed, 5 Oct 2022 18:55:42 +0200 Subject: [PATCH 1/8] Add few operations --- src/liecasadi/dualquaternion.py | 39 ++++++++++++++++++++++++++------- 1 file changed, 31 insertions(+), 8 deletions(-) diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index f7d40c0..3f716be 100644 --- a/src/liecasadi/dualquaternion.py +++ b/src/liecasadi/dualquaternion.py @@ -28,6 +28,14 @@ def __repr__(self) -> str: 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,6 +48,19 @@ def __rmul__(self, other: Scalar) -> "DualQuaternion": """ return DualQuaternion(qr=other * self.qr, qd=other * self.qd) + def __sum__(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 + self.Qr + qd = self.Qd + self.Qd + return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) + @staticmethod def from_quaternion_and_translation( quat: Vector, transl: Vector @@ -52,13 +73,13 @@ def from_quaternion_and_translation( @staticmethod def from_matrix(m: Matrix) -> "DualQuaternion": 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 translation(self) -> Vector: - return 2.0 * (self.Qd * self.Qr.conjugate()).coeff() + return 2.0 * (self.Qd * self.Qr.conjugate()).coeffs() def rotation(self) -> SO3: return SO3(xyzw=self.Qr.coeffs()) @@ -66,10 +87,12 @@ def rotation(self) -> SO3: def inverse(self) -> "DualQuaternion": qr_inv = self.Qr.conjugate() qd = -qr_inv * self.Qd * qr_inv - return DualQuaternion(qr=qr_inv, qd=qd) + return DualQuaternion(qr=qr_inv.coeffs(), qd=qd.coeffs()) - def translation(self): - return 2 * (self.Qd * self.Qr.conjugate()).coeffs() + def conjugate(self): + qr = self.Qr.conjugate() + qd = self.Qd.conjugate() + return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) if __name__ == "__main__": @@ -90,5 +113,5 @@ def translation(self): d3 = DualQuaternion.from_matrix(np.eye(4)) - print((3 * dq2).inverse()) - print(dq.translation()) + # print((3 * dq2).inverse()) + # print(dq.translation()) From 9bd849c380882e17484754b2e4b61bd8476c1560 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 12:19:44 +0200 Subject: [PATCH 2/8] Add new methods --- src/liecasadi/dualquaternion.py | 43 +++++++++++++++++---------------- 1 file changed, 22 insertions(+), 21 deletions(-) diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index 3f716be..f2beb10 100644 --- a/src/liecasadi/dualquaternion.py +++ b/src/liecasadi/dualquaternion.py @@ -48,7 +48,7 @@ def __rmul__(self, other: Scalar) -> "DualQuaternion": """ return DualQuaternion(qr=other * self.qr, qd=other * self.qd) - def __sum__(self, other: "DualQuaternion") -> "DualQuaternion": + def __add__(self, other: "DualQuaternion") -> "DualQuaternion": """Sum of 2 Dual quaternions Args: @@ -61,6 +61,19 @@ def __sum__(self, other: "DualQuaternion") -> "DualQuaternion": qd = self.Qd + self.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 - self.Qr + qd = self.Qd - self.Qd + return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) + @staticmethod def from_quaternion_and_translation( quat: Vector, transl: Vector @@ -94,24 +107,12 @@ def conjugate(self): qd = self.Qd.conjugate() return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) + def as_matrix(self): + r = self.rotation().as_matrix() + t = self.translation() -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) - - quat2 = np.random.randn(4) * 4 - quat2 = quat2 / np.linalg.norm(quat2) - - trans2 = np.random.randn(4) * 4 - - dq2 = DualQuaternion(quat2, trans2) - - d3 = DualQuaternion.from_matrix(np.eye(4)) - - # print((3 * dq2).inverse()) - # print(dq.translation()) + @staticmethod + def Identity(): + return DualQuaternion( + qr=SO3.Identity().as_quat().coeffs, qd=Quaternion([0, 0, 0, 0]) + ) From d85f146b99d680dc90d3041f8906d498edb341ea Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 12:20:11 +0200 Subject: [PATCH 3/8] Add new copy method --- src/liecasadi/se3.py | 3 +++ 1 file changed, 3 insertions(+) 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( From a5204624bad9614488c49fe286f7c12463278098 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 12:25:23 +0200 Subject: [PATCH 4/8] Fix dummy bug --- src/liecasadi/dualquaternion.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index f2beb10..a03b418 100644 --- a/src/liecasadi/dualquaternion.py +++ b/src/liecasadi/dualquaternion.py @@ -57,8 +57,8 @@ def __add__(self, other: "DualQuaternion") -> "DualQuaternion": Returns: DualQuaternion: the sum of Dual Quaternions """ - qr = self.Qr + self.Qr - qd = self.Qd + self.Qd + qr = self.Qr + other.Qr + qd = self.Qd + other.Qd return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) def __sub__(self, other: "DualQuaternion") -> "DualQuaternion": @@ -70,8 +70,8 @@ def __sub__(self, other: "DualQuaternion") -> "DualQuaternion": Returns: DualQuaternion: the difference of Dual Quaternions """ - qr = self.Qr - self.Qr - qd = self.Qd - self.Qd + qr = self.Qr - other.Qr + qd = self.Qd - other.Qd return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) @staticmethod From e9d29c115fe5f08d8428e612b0bde812ccf99dee Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 12:35:03 +0200 Subject: [PATCH 5/8] Add some documentation --- src/liecasadi/dualquaternion.py | 62 ++++++++++++++++++++++++++++++--- 1 file changed, 58 insertions(+), 4 deletions(-) diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index a03b418..d12040c 100644 --- a/src/liecasadi/dualquaternion.py +++ b/src/liecasadi/dualquaternion.py @@ -14,17 +14,26 @@ @dataclasses.dataclass class DualQuaternion: + """ + Dual Quaternion class + Have a look at the document https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/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": @@ -78,6 +87,15 @@ def __sub__(self, other: "DualQuaternion") -> "DualQuaternion": 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() @@ -85,6 +103,14 @@ 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) r = se3.rotation().as_quat() t = Quaternion(cs.vertcat(se3.translation(), 0)) @@ -92,27 +118,55 @@ def from_matrix(m: Matrix) -> "DualQuaternion": return DualQuaternion(qr=r.coeffs(), qd=qd) def translation(self) -> Vector: + """ + Returns: + Vector: Translation vector xyz + """ return 2.0 * (self.Qd * self.Qr.conjugate()).coeffs() 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.coeffs(), qd=qd.coeffs()) - def conjugate(self): + 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()) - def as_matrix(self): + 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, + ) @staticmethod - def Identity(): + def Identity() -> "DualQuaternion": + """ + Returns: + DualQuaternion: the identity dual quaternion + """ return DualQuaternion( qr=SO3.Identity().as_quat().coeffs, qd=Quaternion([0, 0, 0, 0]) ) From 25e4b79785304454203766999e5a59da157f54af Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 15:34:36 +0200 Subject: [PATCH 6/8] Add transform point method --- src/liecasadi/dualquaternion.py | 35 ++++++++++++++++++++++++++++++--- 1 file changed, 32 insertions(+), 3 deletions(-) diff --git a/src/liecasadi/dualquaternion.py b/src/liecasadi/dualquaternion.py index d12040c..554189e 100644 --- a/src/liecasadi/dualquaternion.py +++ b/src/liecasadi/dualquaternion.py @@ -16,7 +16,8 @@ class DualQuaternion: """ Dual Quaternion class - Have a look at the document https://cs.gmu.edu/~jmlien/teaching/cs451/uploads/Main/dual-quaternion.pdf for some intuitions. + 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 @@ -117,12 +118,19 @@ def from_matrix(m: Matrix) -> "DualQuaternion": 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: """ Returns: Vector: Translation vector xyz """ - return 2.0 * (self.Qd * self.Qr.conjugate()).coeffs() + return 2.0 * (self.Qd * self.Qr.conjugate()).coeffs()[:3] def rotation(self) -> SO3: """ @@ -149,6 +157,15 @@ def conjugate(self) -> "DualQuaternion": qd = self.Qd.conjugate() return DualQuaternion(qr=qr.coeffs(), qd=qd.coeffs()) + 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()) + def as_matrix(self) -> Matrix: """ Returns: @@ -168,5 +185,17 @@ def Identity() -> "DualQuaternion": DualQuaternion: the identity dual quaternion """ return DualQuaternion( - qr=SO3.Identity().as_quat().coeffs, qd=Quaternion([0, 0, 0, 0]) + qr=SO3.Identity().as_quat().coeffs(), qd=Quaternion([0, 0, 0, 0]).coeffs() ) + + def transform_point(self, xyz: Vector) -> Vector: + """Rototranlates a point + Args: + xyz (Vector): the point + + 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] From ff2f5f93162e686c5e956e96fbd27b79d6562c80 Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 15:35:01 +0200 Subject: [PATCH 7/8] Add test file --- tests/test_dual_quaternions.py | 57 ++++++++++++++++++++++++++++++++++ 1 file changed, 57 insertions(+) create mode 100644 tests/test_dual_quaternions.py diff --git a/tests/test_dual_quaternions.py b/tests/test_dual_quaternions.py new file mode 100644 index 0000000..c4421d6 --- /dev/null +++ b/tests/test_dual_quaternions.py @@ -0,0 +1,57 @@ +import numpy as np +import pytest +from dual_quaternions import DualQuaternion as TestDualQuat +from icecream import ic + +from liecasadi import SE3, DualQuaternion + +# quat generation +quat = (np.random.rand(4) - 0.5) * 5 +quat = quat / np.linalg.norm(quat) + +quat2 = (np.random.rand(4) - 0.5) * 5 +quat2 = quat2 / np.linalg.norm(quat2) + +pos = (np.random.rand(3) - 0.5) * 5 +pos2 = (np.random.rand(3) - 0.5) * 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(): + conc_dual_q = dual_q1 * dual_q2 + conc_H = H1 @ H2 + assert conc_dual_q.as_matrix() - conc_H == pytest.approx(0.0, abs=1e-4) + + +def test_transform_point(): + point = np.random.rand(4, 1) + point[3] = 1.0 + assert dual_q1.transform_point(point) - (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 + ) From f42f118aaaa30efac9255f40f83a6c40032c134d Mon Sep 17 00:00:00 2001 From: giulero Date: Fri, 7 Oct 2022 15:51:12 +0200 Subject: [PATCH 8/8] Update readme with example --- README.md | 48 +++++++++++++++++++++++++++++++--- tests/test_dual_quaternions.py | 23 ++++++++-------- 2 files changed, 55 insertions(+), 16 deletions(-) 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/tests/test_dual_quaternions.py b/tests/test_dual_quaternions.py index c4421d6..60ec264 100644 --- a/tests/test_dual_quaternions.py +++ b/tests/test_dual_quaternions.py @@ -1,19 +1,18 @@ import numpy as np import pytest -from dual_quaternions import DualQuaternion as TestDualQuat -from icecream import ic from liecasadi import SE3, DualQuaternion -# quat generation -quat = (np.random.rand(4) - 0.5) * 5 +# orientation quaternion generation +quat = np.random.randn(4) * 5 quat = quat / np.linalg.norm(quat) -quat2 = (np.random.rand(4) - 0.5) * 5 +quat2 = np.random.randn(4) * 5 quat2 = quat2 / np.linalg.norm(quat2) -pos = (np.random.rand(3) - 0.5) * 5 -pos2 = (np.random.rand(3) - 0.5) * 5 +# translation vector generation +pos = np.random.randn(3) * 5 +pos2 = np.random.randn(3) * 5 H1 = SE3.from_position_quaternion(pos, quat).as_matrix() @@ -24,15 +23,15 @@ def test_concatenation(): - conc_dual_q = dual_q1 * dual_q2 - conc_H = H1 @ H2 - assert conc_dual_q.as_matrix() - conc_H == pytest.approx(0.0, abs=1e-4) + 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.rand(4, 1) + point = np.random.randn(4, 1) * 5 point[3] = 1.0 - assert dual_q1.transform_point(point) - (H1 @ point)[:3] == pytest.approx( + assert dual_q1.transform_point(point[:3]) - (H1 @ point)[:3] == pytest.approx( 0.0, abs=1e-4 )