8000 Quaternion unit tests pass · RPellowski/spatialmath-python@ff6061d · GitHub
[go: up one dir, main page]
Skip to content

Commit ff6061d

Browse files
petercorkepetercorke
committed
Quaternion unit tests pass
1 parent 91441b6 commit ff6061d

File tree

3 files changed

+975
-44
lines changed

3 files changed

+975
-44
lines changed

‎spatialmath/base/quaternions.py

Lines changed: 24 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -181,7 +181,7 @@ def qqmul(q1, q2):
181181
This is the quaternion or Hamilton product. If both operands are unit-quaternions then
182182
the product will be a unit-quaternion.
183183
184-
:seealso: qvmul
184+
:seealso: qvmul, inner
185185
186186
"""
187187
q1 = argcheck.getvector(q1,4)
@@ -191,6 +191,29 @@ def qqmul(q1, q2):
191191

192192
return np.r_[s1 * s2 - np.dot(v1, v2), s1 * v2 + s2 * v1 + np.cross(v1, v2)]
193193

194+
195+
def inner(q1, q2):
196+
"""
197+
Quaternion innert product
198+
199+
:arg q0: quaternion as a 4-vector
200+
:type q0: : array_like
201+
:arg q1: uaternion as a 4-vector
202+
:type q1: array_like
203+
:return: inner product
204+
:rtype: numpy.ndarray, shape=(4,)
205+
206+
This is the inner or dot product of two quaternions, it is the sum of the element-wise
207+
product.
208+
209+
:seealso: qvmul
210+
211+
"""
212+
q1 = argcheck.getvector(q1,4)
213+
q2 = argcheck.getvector(q2,4)
214+
215+
return np.dot(q1, q2)
216+
194217
def qvmul(q, v):
195218
"""
196219
Vector rotation

‎spatialmath/quaternion.py

Lines changed: 62 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -20,15 +20,15 @@ class Quaternion(UserList):
2020

2121
def __init__(self, s=None, v=None, check=True, norm=True):
2222
"""
23-
A unit quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
23+
A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
2424
A quaternion can be considered as a rotation about a vector in space where
2525
q = cos (theta/2) sin(theta/2) <vx vy vz>
2626
where <vx vy vz> is a unit vector.
2727
:param s: scalar
2828
:param v: vector
2929
"""
3030
if s is None and v is None:
31-
self.data = [ quat.qone() ]
31+
self.data = [ np.array([0, 0, 0, 0]) ]
3232

3333
elif argcheck.isscalar(s) and argcheck.isvector(v,3):
3434
self.data = [ np.r_[s, argcheck.getvector(v)] ]
@@ -37,13 +37,23 @@ def __init__(self, s=None, v=None, check=True, norm=True):
3737
self.data = [ argcheck.getvector(s) ]
3838

3939
elif type(s) is list:
40-
if check:
41-
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
42-
self.data = s
40+
if isinstance(s[0], np.ndarray):
41+
if check:
42+
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
43+
self.data = s
44+
elif isinstance(s[0], self.__class__):
45+
# possibly a list of objects of same type
46+
assert all( map( lambda x: isinstance(x, self.__class__), s) ), 'all elements of list must have same type'
47+
self.data = [x._A for x in s]
48+
else:
49+
raise ValueError('incorrect list')
4350

4451
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
4552
self.data = [x for x in s]
4653

54+
elif isinstance(s, Quaternion):
55+
self.data = s.data
56+
4757
else:
4858
raise ValueError('bad argument to Quaternion constructor')
4959

@@ -64,7 +74,7 @@ def _A(self):
6474
return self.data
6575

6676
def __getitem__(self, i):
67-
print('getitem', i)
77+
#print('getitem', i)
6878
#return self.__class__(self.data[i])
6979
return self.__class__(self.data[i])
7080

@@ -122,15 +132,13 @@ def vec(self):
122132
def pure(cls, v):
123133
return cls(s=0, v=argcheck.getvector(v,3), norm=True)
124134

135+
@property
125136
def conj(self):
126-
if instance(v, np.ndarray) and len(shape) > 1 and v.shape[1] == 3:
127-
return self.__class__( [quat.conj(q._A) for q in self] )
128-
else:
129-
return self.__class__(quat.conj(self._A))
137+
return Quaternion( [quat.conj(q._A) for q in self], norm=False)
138+
130139

131-
def conj(self):
132-
return self.__class__( [quat.conj(q._A) for q in self] )
133140

141+
@property
134142
def norm(self):
135143
"""Return the norm of this quaternion.
136144
Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py
@@ -139,10 +147,11 @@ def norm(self):
139147
@return: the norm
140148
"""
141149
if len(self) == 1:
142-
return np.linalg.norm(self.double())
150+
return quat.qnorm(self._A)
143151
else:
144-
return np.array([quat.norm(q._A) for q in self])
152+
return np.array([quat.qnorm(q._A) for q in self])
145153

154+
@property
146155
def unit(self):
147156
"""Return an equivalent unit quaternion
148157
Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py
@@ -153,11 +162,25 @@ def unit(self):
153162
return UnitQuaternion( [quat.unit(q._A) for q in self], norm=False)
154163

155164

165+
@property
156166
def matrix(self):
157-
return qmatrix(self._A)
167+
return quat.matrix(self._A)
158168

159169
#-------------------------------------------- arithmetic
160170

171+
def inner(self, other):
172+
assert isinstance(other, Quaternion), 'operands to inner must be Quaternion subclass'
173+
return self._op2(other, lambda x, y: quat.inner(x, y), list1=False )
174+
175+
def __eq__(self, other):
176+
assert type(self) == type(other), 'operands to == are of different types'
177+
return self._op2(other, lambda x, y: quat.isequal(x, y), list1=False )
178+
179+
def __ne__(self, other):
180+
assert type(self) == type(other), 'operands to == are of different types'
181+
return self._op2(other, lambda x, y: not quat.isequal(x, y), list1=False )
182+
183+
161184
def __mul__(left, right):
162185
"""
163186
multiply quaternion
@@ -204,7 +227,7 @@ def __mul__(left, right):
204227
if type(left) == type(right):
205228
# quaternion * quaternion case (same class)
206229
return left.__class__( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
207-
elif isinstance(other, Quaternion):
230+
elif isinstance(right, Quaternion):
208231
# quaternion * quaternion case (different class)
209232
return Quaternion( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
210233
elif argcheck.isscalar(right):
@@ -258,7 +281,7 @@ def __rmul__(right, left):
258281
A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
259282
"""
260283
# scalar * quaternion case
261-
return Quaternion([other*q._A for q in self])
284+
return Quaternion([left*q._A for q in right])
262285

263286
def __imul__(left, right):
264287
"""
@@ -409,13 +432,6 @@ def __sub__(left, right):
409432
assert type(left) == type(right), 'operands to - are of different types'
410433
return Quaternion( left._op2(right, lambda x, y: x - y ) )
411434

412-
413-
def __eq__(self, other):
414-
assert type(self) == type(other), 'operands to == are of different types'
415-
return self._op2(other, lambda x, y: quat.isequal(x, y), list1=False )
416-
417-
def __ne__(self, other):
418-
return not self.__eq__(other)
419435

420436
def _op2(self, other, op, list1=True):
421437

@@ -430,10 +446,10 @@ def _op2(self, other, op, list1=True):
430446
return [op(self._A, x._A) for x in other]
431447
else:
432448
if len(other) == 1:
433-
print('== Nx1')
449+
#print('== Nx1')
434450
return [op(x._A, other._A) for x in self]
435451
elif len(self) == len(other):
436-
print('== NxN')
452+
#print('== NxN')
437453
return [op(x._A, y._A) for (x,y) in zip(self, other)]
438454
else:
439455
raise ValueError('length of lists to == must be same length')
@@ -472,7 +488,7 @@ def _op2(self, other, op, list1=True):
472488
def __repr__(self):
473489
s = ''
474490
for q in self:
475-
s += quat.print(q._A, file=None) + '\n'
491+
s += quat.qprint(q._A, file=None) + '\n'
476492
s.rstrip('\n')
477493
return s
478494

@@ -536,19 +552,24 @@ def __init__(self, s=None, v=None, norm=True, check=True):
536552
self.data = [q]
537553

538554
elif argcheck.isvector(s,4):
539-
print('uq constructor 4vec')
555+
#print('uq constructor 4vec')
540556
q = argcheck.getvector(s)
541557
# if norm:
542558
# q = quat.unit(q)
543559
print(q)
544560
self.data = [q]
545561

546562
elif type(s) is list:
547-
if check:
548-
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
549-
if norm:
550-
s = [quat.unit(q) for q in s]
551-
self.data = s
563+
if isinstance(s[0], np.ndarray):
564+
if check:
565+
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
566+
self.data = s
567+
elif type(s[0]) == type(self):
568+
# possibly a list of objects of same type
569+
assert all( map( lambda x: type(x) == type(self), s) ), 'all elements of list must have same type'
570+
self.data = [x._A for x in s]
571+
else:
572+
raise ValueError('incorrect list')
552573

553574
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
554575
if norm:
@@ -769,14 +790,16 @@ def to_se3(self):
769790
from .pose import SO3
770791
return SE3(so3=SO3.np(self.r()))
771792

793+
772794

773795
def __repr__(self):
774796
s = ''
775797
for q in self:
776-
s += quat.print(q._A, delim=('<<', '>>'), file=None) + '\n'
798+
s += quat.qprint(q._A, delim=('<<', '>>'), file=None) + '\n'
777799
s.rstrip('\n')
778800
return s
779801

802+
780803
def __str__(self):
781804
return self.__repr__()
782805

@@ -808,6 +831,10 @@ def to_se3(self):
808831

809832

810833
if __name__ == '__main__': # pragma: no cover
834+
835+
import pathlib
836+
import os.path
837+
811838
q = Quaternion([1,2,3,4])
812839
print(q)
813840
q.append(q)
@@ -826,4 +853,4 @@ def to_se3(self):
826853
# import pathlib
827854
# import os.path
828855

829-
# runfile(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_quaternion.py") )
856+
exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_quaternion.py")).read() )

0 commit comments

Comments
 (0)
0