Skip to content

Commit 7540fcc

Browse files
petercorkepetercorke
committed
Added dual quaternions
1 parent 2c81729 commit 7540fcc

7 files changed

Lines changed: 341 additions & 3 deletions

File tree

‎README.md‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -40,7 +40,7 @@ space:
4040

4141
| Represents | in 3D | in 2D |
4242
| ------------ | ---------------- | -------- |
43-
| pose | ``SE3`` ``Twist3`` | ``SE2`` ``Twist2`` |
43+
| pose | ``SE3`` ``Twist3`` ``UnitDualQuaternion`` | ``SE2`` ``Twist2`` |
4444
| orientation | ``SO3`` ``UnitQuaternion`` | ``SO2`` |
4545
4646
@@ -50,6 +50,7 @@ More specifically:
5050
* `SO3` matrices belonging to the group SO(3) for orientation in 3-dimensions
5151
* `UnitQuaternion` belonging to the group S3 for orientation in 3-dimensions
5252
* `Twist3` vectors belonging to the group se(3) for pose in 3-dimensions
53+
* `UnitDualQuaternion` maps to the group SE(3) for position and orientation (pose) in 3-dimensions
5354
* `SE2` matrices belonging to the group SE(2) for position and orientation (pose) in 2-dimensions
5455
* `SO2` matrices belonging to the group SO(2) for orientation in 2-dimensions
5556
* `Twist2` vectors belonging to the group se(2) for pose in 2-dimensions

‎docs/source/3d_dualquaternion.rst‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
Dual Quaternion
2+
^^^^^^^^^^^^^^^
3+
4+
.. autoclass:: spatialmath.DualQuaternion.DualQuaternion
5+
:members:
6+
:undoc-members:
7+
:show-inheritance:
8+
:inherited-members:
9+
:special-members: __mul__, __truediv__, __add__, __sub__, __init__
10+
:exclude-members: count, copy, index, sort, remove

‎docs/source/3d_pose_dualquaternion.rst‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,10 @@
1+
Unit dual quaternion
2+
^^^^^^^^^^^^^^^^^^^^^
3+
4+
.. autoclass:: spatialmath.DualQuaternion.UnitDualQuaternion
5+
:members:
6+
:undoc-members:
7+
:show-inheritance:
8+
:inherited-members:
9+
:special-members: __mul__, __truediv__, __add__, __sub__, __init__
10+
:exclude-members: count, copy, index, sort, remove, arghandler, binop, unop

‎docs/source/index.rst‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,6 @@ Spatial Maths for Python
1111
:maxdepth: 2
1212

1313
intro
14-
3D-space
1514
spatialmath
1615
functions
1716
indices

‎docs/source/spatialmath.rst‎

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ Pose in 3D
2424

2525
3d_pose_SE3
2626
3d_pose_twist
27+
3d_pose_dualquaternion
2728

2829

2930
Orientation in 3D
@@ -57,7 +58,8 @@ Geometry
5758
.. toctree::
5859
:maxdepth: 2
5960

60-
3d_quaternion
61+
3d_quaternion
62+
3d_dualquaternion
6163
3d_plucker
6264
3d_plane
6365

‎spatialmath/DualQuaternion.py‎

Lines changed: 315 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,315 @@
1+
import numpy as np
2+
from spatialmath import Quaternion, UnitQuaternion, SE3
3+
from spatialmath import base
4+
5+
# TODO scalar multiplication
6+
7+
class DualQuaternion:
8+
r"""
9+
A dual number is an ordered pair :math:`\hat{a} = (a, b)` or written as
10+
:math:`a + \epsilon b` where :math:`\epsilon^2 = 0`.
11+
12+
A dual quaternion can be considered as either:
13+
14+
- a quaternion with dual numbers as coefficients
15+
- a dual of quaternions, written as an ordered pair of quaternions
16+
17+
The latter form is used here.
18+
19+
:References:
20+
21+
- http://web.cs.iastate.edu/~cs577/handouts/dual-quaternion.pdf
22+
- https://en.wikipedia.org/wiki/Dual_quaternion
23+
"""
24+
25+
def __init__(self, real=None, dual=None):
26+
"""
27+
Construct a new dual quaternion
28+
29+
:param real: real quaternion
30+
:type real: Quaternion or UnitQuaternion
31+
:param dual: dual quaternion
32+
:type dual: Quaternion or UnitQuaternion
33+
:raises ValueError: incorrect parameters
34+
35+
Example:
36+
37+
.. runblock:: pycon
38+
39+
>>> from spatialmath import DualQuaternion, Quaternion
40+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
41+
>>> print(d)
42+
>>> d = DualQuaternion([1, 2, 3, 4, 5, 6, 7, 8])
43+
>>> print(d)
44+
45+
"""
46+
47+
if real is None and dual is None:
48+
self.real = None
49+
self.dual = None
50+
return
51+
elif dual is None and base.isvector(real, 8):
52+
self.real = Quaternion(real[0:4])
53+
self.dual = Quaternion(real[4:8])
54+
elif real is not None and dual is not None:
55+
if not isinstance(real, Quaternion):
56+
raise ValueError('real part must be a Quaternion subclass')
57+
if not isinstance(dual, Quaternion):
58+
raise ValueError('real part must be a Quaternion subclass')
59+
self.real = real # quaternion, real part
60+
self.dual = dual # quaternion, dual part
61+
else:
62+
raise ValueError('expecting zero or two parameters')
63+
64+
def __repr__(self):
65+
return str(self)
66+
67+
def __str__(self):
68+
"""
69+
String representation of dual quaternion
70+
71+
:return: compact string representation
72+
:rtype: str
73+
74+
Example:
75+
76+
.. runblock:: pycon
77+
78+
>>> from spatialmath import DualQuaternion, Quaternion
79+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
80+
>>> str(d)
81+
"""
82+
return str(self.real) + " + ε " + str(self.dual)
83+
84+
def norm(self):
85+
"""
86+
Norm of a dual quaternion
87+
88+
:return: Norm as a dual number
89+
:rtype: 2-tuple
90+
91+
Example:
92+
93+
.. runblock:: pycon
94+
95+
>>> from spatialmath import DualQuaternion, Quaternion
96+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
97+
>>> d.norm() # norm is a dual number
98+
"""
99+
a = self.real * self.real.conj()
100+
b= self.real * self.dual.conj() + self.dual * self.real.conj()
101+
return (a.s, b.s)
102+
103+
def conj(self):
104+
r"""
105+
Conjugate of dual quaternion
106+
107+
:return: Conjugate
108+
:rtype: DualQuaternion
109+
110+
There are several conjugates defined for a dual quaternion. This one
111+
mirrors conjugation for a regular quaternion. For the dual quaternion
112+
:math:`(p, q)` it returns :math:`(p^*, q^*)`.
113+
114+
Example:
115+
116+
.. runblock:: pycon
117+
118+
>>> from spatialmath import DualQuaternion, Quaternion
119+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
120+
>>> d.conj()
121+
"""
122+
return DualQuaternion(self.real.conj(), self.dual.conj())
123+
124+
def __add__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
125+
"""
126+
Sum of two dual quaternions
127+
128+
:return: Product
129+
:rtype: DualQuaternion
130+
131+
Example:
132+
133+
.. runblock:: pycon
134+
135+
>>> from spatialmath import DualQuaternion, Quaternion
136+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
137+
>>> d + d
138+
"""
139+
return DualQuaternion(left.real + right.real, left.dual + right.dual)
140+
141+
def __sub__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
142+
"""
143+
Difference of two dual quaternions
144+
145+
:return: Product
146+
:rtype: DualQuaternion
147+
148+
Example:
149+
150+
.. runblock:: pycon
151+
152+
>>> from spatialmath import DualQuaternion, Quaternion
153+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
154+
>>> d - d
155+
"""
156+
return DualQuaternion(left.real - right.real, left.dual - right.dual)
157+
158+
def __mul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-argument
159+
"""
160+
Product of two dual quaternions
161+
162+
:return: Product
163+
:rtype: DualQuaternion
164+
165+
Example:
166+
167+
.. runblock:: pycon
168+
169+
>>> from spatialmath import DualQuaternion, Quaternion
170+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
171+
>>> d * d
172+
"""
173+
real = left.real * right.real
174+
dual = left.real * right.dual + left.dual * right.real
175+
176+
if isinstance(left, UnitDualQuaternion) and isinstance(left, UnitDualQuaternion):
177+
return UnitDualQuaternion(real, dual)
178+
else:
179+
return DualQuaternion(real, dual)
180+
181+
def matrix(self):
182+
"""
183+
Dual quaternion as a matrix
184+
185+
:return: Matrix represensation
186+
:rtype: ndarray(8,8)
187+
188+
Dual quaternion multiplication can also be written as a matrix-vector
189+
product.
190+
191+
Example:
192+
193+
.. runblock:: pycon
194+
195+
>>> from spatialmath import DualQuaternion, Quaternion
196+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
197+
>>> d.matrix()
198+
>>> d.matrix() @ d.vec
199+
>>> d * d
200+
"""
201+
return np.block([
202+
[self.real.matrix, np.zeros((4,4))],
203+
[self.dual.matrix, self.real.matrix]
204+
])
205+
206+
@property
207+
def vec(self):
208+
"""
209+
Dual quaternion as a vector
210+
211+
:return: Vector represensation
212+
:rtype: ndarray(8)
213+
214+
Example:
215+
216+
.. runblock:: pycon
217+
218+
>>> from spatialmath import DualQuaternion, Quaternion
219+
>>> d = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
220+
>>> d.vec
221+
"""
222+
return np.r_[self.real.vec, self.dual.vec]
223+
224+
225+
# def log(self):
226+
# pass
227+
228+
class UnitDualQuaternion(DualQuaternion):
229+
230+
def __init__(self, real=None, dual=None):
231+
"""
232+
Create new unit dual quaternion
233+
234+
:param real: real quaternion or SE(3) matrix
235+
:type real: Quaternion, UnitQuaternion or SE3
236+
:param dual: dual quaternion
237+
:type dual: Quaternion or UnitQuaternion
238+
239+
- ``UnitDualQuaternion(real, dual)`` is a new unit dual quaternion with
240+
real and dual parts as specified.
241+
- ``UnitDualQuaternion(T)`` is a new unit dual quaternion equivalent to
242+
the rigid-body motion described by the SE3 value ``T``.
243+
244+
Example:
245+
246+
.. runblock:: pycon
247+
248+
>>> from spatialmath import DualQuaternion
249+
>>> T = SE3.Rand()
250+
>>> print(T)
251+
>>> d = UnitDualQuaternion(T))
252+
>>> print(d)
253+
>>> type(d)
254+
>>> print(d.norm()) # norm is (1, 0)
255+
"""
256+
if real is None and dual is None:
257+
self.real = None
258+
self.dual = None
259+
return
260+
elif real is not None and dual is not None:
261+
self.real = real # quaternion, real part
262+
self.dual = dual # quaternion, dual part
263+
elif dual is None and isinstance(real, SE3):
264+
T = real
265+
S = UnitQuaternion(T.R)
266+
D = Quaternion.Pure(T.t)
267+
268+
self.real = S
269+
self.dual = 0.5 * D * S
270+
271+
def T(self):
272+
"""
273+
Convert unit dual quaternion to SE(3) matrix
274+
275+
:return: SE(3) matrix
276+
:rtype: SE3
277+
278+
Example:
279+
280+
.. runblock:: pycon
281+
282+
>>> from spatialmath import DualQuaternion
283+
>>> T = SE3.Rand()
284+
>>> print(T)
285+
>>> d = UnitDualQuaternion(T))
286+
>>> print(d)
287+
>>> print(d.T)
288+
"""
289+
R = base.q2r(self.real.A)
290+
t = 2 * self.dual * self.real.conj()
291+
292+
return SE3(base.rt2tr(R, t.v))
293+
294+
# def exp(self):
295+
# w = self.real.v
296+
# v = self.dual.v
297+
# theta = base.norm(w)
298+
299+
if __name__ == "__main__":
300+
301+
a = DualQuaternion(Quaternion([1,2,3,4]), Quaternion([5,6,7,8]))
302+
print(a)
303+
print(a.vec)
304+
print(a.conj())
305+
print(a.norm())
306+
print(a+a)
307+
print(a*a)
308+
print(a.matrix())
309+
310+
T = SE3.Rand()
311+
print(T)
312+
313+
print(aa := UnitDualQuaternion(T))
314+
print(aa.norm())
315+
print(aa.T())

‎spatialmath/__init__.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -5,4 +5,5 @@
55
from spatialmath.spatialvector import SpatialVelocity, SpatialAcceleration, \
66
SpatialForce, SpatialMomentum, SpatialInertia
77
from spatialmath.quaternion import Quaternion, UnitQuaternion
8+
from spatialmath.DualQuaternion import DualQuaternion, UnitDualQuaternion
89
#from spatialmath.Plucker import *

0 commit comments

Comments
 (0)