3535from spatialmath .base .vectors import *
3636import spatialmath .base as tr
3737from spatialmath .base .quaternions import *
38+ import spatialmath as sm
3839
3940
4041class 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
127169if __name__ == '__main__' :
128170 unittest .main ()
0 commit comments