Skip to content

Commit c2553fa

Browse files
committed
added r2q order and tests
1 parent 95629b3 commit c2553fa

2 files changed

Lines changed: 112 additions & 57 deletions

File tree

‎spatialmath/base/quaternions.py‎

Lines changed: 40 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,7 @@ def pure(v):
5353
v = base.getvector(v, 3)
5454
return np.r_[0, v]
5555

56+
5657
def qpositive(q):
5758
"""
5859
Quaternion with positive scalar part
@@ -69,6 +70,7 @@ def qpositive(q):
6970
else:
7071
return q
7172

73+
7274
def qnorm(q):
7375
r"""
7476
Norm of a quaternion
@@ -125,7 +127,7 @@ def unit(q, tol=10):
125127
raise ValueError("cannot normalize (near) zero length quaternion")
126128
else:
127129
q /= nm
128-
130+
129131
if q[0] >= 0:
130132
return q
131133
else:
@@ -216,7 +218,7 @@ def q2v(q):
216218
>>> print(q2v(q))
217219
>>> q = [-1 / sqrt(2), 0, 1 / sqrt(2), 0]
218220
>>> print(q2v(q))
219-
221+
220222
.. warning:: There is no check that the passed value is a unit-quaternion.
221223
222224
:seealso: :func:`~v2q`
@@ -490,11 +492,12 @@ def q2r(q):
490492
y = q[2]
491493
z = q[3]
492494
return np.array([[1 - 2 * (y ** 2 + z ** 2), 2 * (x * y - s * z), 2 * (x * z + s * y)],
493-
[2 * (x * y + s * z), 1 - 2 * (x ** 2 + z ** 2), 2 * (y * z - s * x)],
495+
[2 * (x * y + s * z), 1 - 2 *
496+
(x ** 2 + z ** 2), 2 * (y * z - s * x)],
494497
[2 * (x * z - s * y), 2 * (y * z + s * x), 1 - 2 * (x ** 2 + y ** 2)]])
495498

496499

497-
def r2q(R, check=False, tol=100):
500+
def r2q(R, check=False, tol=100, order='sxyz'):
498501
"""
499502
Convert SO(3) rotation matrix to unit-quaternion
500503
@@ -504,6 +507,9 @@ def r2q(R, check=False, tol=100):
504507
:type check: bool
505508
:param tol: tolerance in units of eps
506509
:type tol: float
510+
:param order: the order of the returned quaternion. Must be 'sxyz' or
511+
'xyzs'. Defaults to 'sxyz'.
512+
:type order: str
507513
:return: unit-quaternion as Euler parameters
508514
:rtype: ndarray(4)
509515
:raises ValueError: for non SO(3) argument
@@ -534,33 +540,38 @@ def r2q(R, check=False, tol=100):
534540
if not base.isrot(R, check=check, tol=tol):
535541
raise ValueError("Argument must be a valid SO(3) matrix")
536542

537-
t12p = (R[0,1] + R[1,0]) ** 2
538-
t13p = (R[0,2] + R[2,0]) ** 2
539-
t23p = (R[1,2] + R[2,1]) ** 2
543+
t12p = (R[0, 1] + R[1, 0]) ** 2
544+
t13p = (R[0, 2] + R[2, 0]) ** 2
545+
t23p = (R[1, 2] + R[2, 1]) ** 2
540546

541-
t12m = (R[0,1] - R[1,0]) ** 2
542-
t13m = (R[0,2] - R[2,0]) ** 2
543-
t23m = (R[1,2] - R[2,1]) ** 2
547+
t12m = (R[0, 1] - R[1, 0]) ** 2
548+
t13m = (R[0, 2] - R[2, 0]) ** 2
549+
t23m = (R[1, 2] - R[2, 1]) ** 2
544550

545-
d1 = ( R[0,0] + R[1,1] + R[2,2] + 1) ** 2
546-
d2 = ( R[0,0] - R[1,1] - R[2,2] + 1) ** 2
547-
d3 = (-R[0,0] + R[1,1] - R[2,2] + 1) ** 2
548-
d4 = (-R[0,0] - R[1,1] + R[2,2] + 1) ** 2
551+
d1 = (R[0, 0] + R[1, 1] + R[2, 2] + 1) ** 2
552+
d2 = (R[0, 0] - R[1, 1] - R[2, 2] + 1) ** 2
553+
d3 = (-R[0, 0] + R[1, 1] - R[2, 2] + 1) ** 2
554+
d4 = (-R[0, 0] - R[1, 1] + R[2, 2] + 1) ** 2
549555

550-
e0 = math.sqrt( d1 + t23m + t13m + t12m) / 4.0
551-
e1 = math.sqrt(t23m + d2 + t12p + t13p) / 4.0
552-
e2 = math.sqrt(t13m + t12p + d3 + t23p) / 4.0
553-
e3 = math.sqrt(t12m + t13p + t23p + d4) / 4.0
556+
e0 = math.sqrt(d1 + t23m + t13m + t12m) / 4.0
557+
e1 = math.sqrt(t23m + d2 + t12p + t13p) / 4.0
558+
e2 = math.sqrt(t13m + t12p + d3 + t23p) / 4.0
559+
e3 = math.sqrt(t12m + t13p + t23p + d4) / 4.0
554560

555561
# transfer sign from rotation element differences
556-
if R[2,1] < R[1,2]:
562+
if R[2, 1] < R[1, 2]:
557563
e1 = -e1
558-
if R[0,2] < R[2,0]:
564+
if R[0, 2] < R[2, 0]:
559565
e2 = -e2
560-
if R[1,0] < R[0,1]:
566+
if R[1, 0] < R[0, 1]:
561567
e3 = -e3
562-
563-
return np.r_[e0, e1, e2, e3]
568+
569+
if order == 'sxyz':
570+
return np.r_[e0, e1, e2, e3]
571+
elif order == 'xyzs':
572+
return np.r_[e1, e2, e3, e0]
573+
else:
574+
raise ValueError("order is invalid, must be 'sxyz' or 'xyzs'")
564575

565576
# def r2q_old(R, check=False, tol=100):
566577
# """
@@ -688,7 +699,7 @@ def slerp(q0, q1, s, shortest=False):
688699
if shortest:
689700
if dotprod < 0:
690701
q0 = -q0 # pylint: disable=invalid-unary-operand-type
691-
dotprod = -dotprod # pylint: disable=invalid-unary-operand-type
702+
dotprod = -dotprod # pylint: disable=invalid-unary-operand-type
692703

693704
dotprod = np.clip(dotprod, -1, 1) # Clip within domain of acos()
694705
theta = math.acos(dotprod) # theta is the angle between rotation vectors
@@ -716,7 +727,8 @@ def rand():
716727
>>> from spatialmath.base import rand, qprint
717728
>>> qprint(rand())
718729
"""
719-
u = np.random.uniform(low=0, high=1, size=3) # get 3 random numbers in [0,1]
730+
u = np.random.uniform(
731+
low=0, high=1, size=3) # get 3 random numbers in [0,1]
720732
return np.r_[
721733
math.sqrt(1 - u[0]) * math.sin(2 * math.pi * u[1]),
722734
math.sqrt(1 - u[0]) * math.cos(2 * math.pi * u[1]),
@@ -848,7 +860,7 @@ def angle(q1, q2):
848860
>>> angle(q1, q2)
849861
850862
:References:
851-
863+
852864
- Metrics for 3D rotations: comparison and analysis,
853865
Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.
854866
@@ -908,4 +920,5 @@ def qprint(q, delim=('<', '>'), fmt='{: .4f}', file=sys.stdout):
908920
if __name__ == '__main__': # pragma: no cover
909921
import pathlib
910922

911-
exec(open(pathlib.Path(__file__).parent.parent.parent.absolute() / "tests" / "base" / "test_quaternions.py").read()) # pylint: disable=exec-used
923+
exec(open(pathlib.Path(__file__).parent.parent.parent.absolute() / "tests" /
924+
"base" / "test_quaternions.py").read()) # pylint: disable=exec-used

‎tests/base/test_quaternions.py‎

Lines changed: 72 additions & 30 deletions
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,7 @@
3535
from spatialmath.base.vectors import *
3636
import spatialmath.base as tr
3737
from spatialmath.base.quaternions import *
38+
import spatialmath as sm
3839

3940

4041
class TestQuaternion(unittest.TestCase):
@@ -50,27 +51,41 @@ def test_ops(self):
5051
nt.assert_equal(qnorm([1, 2, 3, 4]), math.sqrt(30))
5152
nt.assert_equal(qnorm((1, 2, 3, 4)), math.sqrt(30))
5253

53-
nt.assert_array_almost_equal(unit(np.r_[1, 2, 3, 4]), np.r_[1, 2, 3, 4] / math.sqrt(30))
54-
nt.assert_array_almost_equal(unit([1, 2, 3, 4]), np.r_[1, 2, 3, 4] / math.sqrt(30))
55-
56-
nt.assert_array_almost_equal(qqmul(np.r_[1, 2, 3, 4], np.r_[5, 6, 7, 8]), np.r_[-60, 12, 30, 24])
57-
nt.assert_array_almost_equal(qqmul([1, 2, 3, 4], [5, 6, 7, 8]), np.r_[-60, 12, 30, 24])
58-
nt.assert_array_almost_equal(qqmul(np.r_[1, 2, 3, 4], np.r_[1, 2, 3, 4]), np.r_[-28, 4, 6, 8])
59-
60-
nt.assert_array_almost_equal(matrix(np.r_[1, 2, 3, 4])@np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24])
61-
nt.assert_array_almost_equal(matrix([1, 2, 3, 4])@np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24])
62-
nt.assert_array_almost_equal(matrix(np.r_[1, 2, 3, 4])@np.r_[1, 2, 3, 4], np.r_[-28, 4, 6, 8])
63-
64-
nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 0), np.r_[1, 0, 0, 0])
65-
nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 1), np.r_[1, 2, 3, 4])
54+
nt.assert_array_almost_equal(unit(np.r_[1, 2, 3, 4]), np.r_[
55+
1, 2, 3, 4] / math.sqrt(30))
56+
nt.assert_array_almost_equal(unit([1, 2, 3, 4]), np.r_[
57+
1, 2, 3, 4] / math.sqrt(30))
58+
59+
nt.assert_array_almost_equal(
60+
qqmul(np.r_[1, 2, 3, 4], np.r_[5, 6, 7, 8]), np.r_[-60, 12, 30, 24])
61+
nt.assert_array_almost_equal(
62+
qqmul([1, 2, 3, 4], [5, 6, 7, 8]), np.r_[-60, 12, 30, 24])
63+
nt.assert_array_almost_equal(
64+
qqmul(np.r_[1, 2, 3, 4], np.r_[1, 2, 3, 4]), np.r_[-28, 4, 6, 8])
65+
66+
nt.assert_array_almost_equal(
67+
matrix(np.r_[1, 2, 3, 4])@np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24])
68+
nt.assert_array_almost_equal(
69+
matrix([1, 2, 3, 4])@np.r_[5, 6, 7, 8], np.r_[-60, 12, 30, 24])
70+
nt.assert_array_almost_equal(
71+
matrix(np.r_[1, 2, 3, 4])@np.r_[1, 2, 3, 4], np.r_[-28, 4, 6, 8])
72+
73+
nt.assert_array_almost_equal(
74+
qpow(np.r_[1, 2, 3, 4], 0), np.r_[1, 0, 0, 0])
75+
nt.assert_array_almost_equal(
76+
qpow(np.r_[1, 2, 3, 4], 1), np.r_[1, 2, 3, 4])
6677
nt.assert_array_almost_equal(qpow([1, 2, 3, 4], 1), np.r_[1, 2, 3, 4])
67-
nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], 2), np.r_[-28, 4, 6, 8])
68-
nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], -1), np.r_[1, -2, -3, -4])
69-
nt.assert_array_almost_equal(qpow(np.r_[1, 2, 3, 4], -2), np.r_[-28, -4, -6, -8])
78+
nt.assert_array_almost_equal(
79+
qpow(np.r_[1, 2, 3, 4], 2), np.r_[-28, 4, 6, 8])
80+
nt.assert_array_almost_equal(
81+
qpow(np.r_[1, 2, 3, 4], -1), np.r_[1, -2, -3, -4])
82+
nt.assert_array_almost_equal(
83+
qpow(np.r_[1, 2, 3, 4], -2), np.r_[-28, -4, -6, -8])
7084

7185
nt.assert_equal(isequal(np.r_[1, 2, 3, 4], np.r_[1, 2, 3, 4]), True)
7286
nt.assert_equal(isequal(np.r_[1, 2, 3, 4], np.r_[5, 6, 7, 8]), False)
73-
nt.assert_equal(isequal(np.r_[1, 1, 0, 0] / math.sqrt(2), np.r_[-1, -1, 0, 0] / math.sqrt(2), unitq=True), True)
87+
nt.assert_equal(isequal(np.r_[1, 1, 0, 0] / math.sqrt(2),
88+
np.r_[-1, -1, 0, 0] / math.sqrt(2), unitq=True), True)
7489

7590
s = qprint(np.r_[1, 1, 0, 0], file=None)
7691
nt.assert_equal(isinstance(s, str), True)
@@ -79,50 +94,77 @@ def test_ops(self):
7994
nt.assert_equal(isinstance(s, str), True)
8095
nt.assert_equal(len(s) > 2, True)
8196

82-
nt.assert_equal(qprint([1, 2, 3, 4], file=None), " 1.0000 < 2.0000, 3.0000, 4.0000 >")
97+
nt.assert_equal(qprint([1, 2, 3, 4], file=None),
98+
" 1.0000 < 2.0000, 3.0000, 4.0000 >")
8399

84100
nt.assert_equal(isunitvec(rand()), True)
85101

86102
def test_rotation(self):
87103
# rotation matrix to quaternion
88-
nt.assert_array_almost_equal(r2q(tr.rotx(180, 'deg')), np.r_[0, 1, 0, 0])
89-
nt.assert_array_almost_equal(r2q(tr.roty(180, 'deg')), np.r_[0, 0, 1, 0])
90-
nt.assert_array_almost_equal(r2q(tr.rotz(180, 'deg')), np.r_[0, 0, 0, 1])
104+
nt.assert_array_almost_equal(
105+
r2q(tr.rotx(180, 'deg')), np.r_[0, 1, 0, 0])
106+
nt.assert_array_almost_equal(
107+
r2q(tr.roty(180, 'deg')), np.r_[0, 0, 1, 0])
108+
nt.assert_array_almost_equal(
109+
r2q(tr.rotz(180, 'deg')), np.r_[0, 0, 0, 1])
91110

92111
# quaternion to rotation matrix
93-
nt.assert_array_almost_equal(q2r(np.r_[0, 1, 0, 0]), tr.rotx(180, 'deg'))
94-
nt.assert_array_almost_equal(q2r(np.r_[0, 0, 1, 0]), tr.roty(180, 'deg'))
95-
nt.assert_array_almost_equal(q2r(np.r_[0, 0, 0, 1]), tr.rotz(180, 'deg'))
112+
nt.assert_array_almost_equal(
113+
q2r(np.r_[0, 1, 0, 0]), tr.rotx(180, 'deg'))
114+
nt.assert_array_almost_equal(
115+
q2r(np.r_[0, 0, 1, 0]), tr.roty(180, 'deg'))
116+
nt.assert_array_almost_equal(
117+
q2r(np.r_[0, 0, 0, 1]), tr.rotz(180, 'deg'))
96118

97119
nt.assert_array_almost_equal(q2r([0, 1, 0, 0]), tr.rotx(180, 'deg'))
98120
nt.assert_array_almost_equal(q2r([0, 0, 1, 0]), tr.roty(180, 'deg'))
99121
nt.assert_array_almost_equal(q2r([0, 0, 0, 1]), tr.rotz(180, 'deg'))
100122

101123
# quaternion - vector product
102-
nt.assert_array_almost_equal(qvmul(np.r_[0, 1, 0, 0], np.r_[0, 0, 1]), np.r_[0, 0, -1])
103-
nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1])
124+
nt.assert_array_almost_equal(
125+
qvmul(np.r_[0, 1, 0, 0], np.r_[0, 0, 1]), np.r_[0, 0, -1])
126+
nt.assert_array_almost_equal(
127+
qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1])
104128

105129
def test_slerp(self):
106130
q1 = np.r_[0, 1, 0, 0]
107131
q2 = np.r_[0, 0, 1, 0]
108132

109133
nt.assert_array_almost_equal(slerp(q1, q2, 0), q1)
110134
nt.assert_array_almost_equal(slerp(q1, q2, 1), q2)
111-
nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2))
135+
nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[
136+
0, 1, 1, 0] / math.sqrt(2))
112137

113138
q1 = [0, 1, 0, 0]
114139
q2 = [0, 0, 1, 0]
115140

116141
nt.assert_array_almost_equal(slerp(q1, q2, 0), q1)
117142
nt.assert_array_almost_equal(slerp(q1, q2, 1), q2)
118-
nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[0, 1, 1, 0] / math.sqrt(2))
143+
nt.assert_array_almost_equal(slerp(q1, q2, 0.5), np.r_[
144+
0, 1, 1, 0] / math.sqrt(2))
119145

120-
nt.assert_array_almost_equal(slerp(r2q(tr.rotx(-0.3)), r2q(tr.rotx(0.3)), 0.5), np.r_[1, 0, 0, 0])
121-
nt.assert_array_almost_equal(slerp(r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)))
146+
nt.assert_array_almost_equal(
147+
slerp(r2q(tr.rotx(-0.3)), r2q(tr.rotx(0.3)), 0.5), np.r_[1, 0, 0, 0])
148+
nt.assert_array_almost_equal(
149+
slerp(r2q(tr.roty(0.3)), r2q(tr.roty(0.5)), 0.5), r2q(tr.roty(0.4)))
122150

123151
def test_rotx(self):
124152
pass
125153

154+
def test_r2q(self):
155+
r1 = sm.SE3.Rx(0.1)
156+
q1a = np.array([9.987503e-01, 4.997917e-02,
157+
0.000000e+00, 2.775558e-17])
158+
q1b = np.array([4.997917e-02, 0.000000e+00,
159+
2.775558e-17, 9.987503e-01])
160+
161+
nt.assert_array_almost_equal(q1a, r2q(r1.R))
162+
nt.assert_array_almost_equal(q1a, r2q(r1.R, order='sxyz'))
163+
nt.assert_array_almost_equal(q1b, r2q(r1.R, order='xyzs'))
164+
165+
with self.assertRaises(ValueError):
166+
nt.assert_array_almost_equal(q1a, r2q(r1.R, order='aaa'))
167+
126168

127169
if __name__ == '__main__':
128170
unittest.main()

0 commit comments

Comments
 (0)