1 ''' Contains all about rotation matrices, quaternions, and various conversions.
2
3 conventions: q=( a + bi + cj + dk), with a>0
4 '''
5 from . import (assert_allclose, new_contract, contract, safe_arccos, np,
6 default_axis, normalize_length)
7 import itertools
8
9
10 new_contract('unit_quaternion', 'array[4], unit_length')
11 new_contract('axis_angle', 'tuple(direction, float)')
12
13 new_contract('axis_angle_canonical', 'tuple(direction, (float,>=0, <=pi))')
14
15
16 @contract(x='array[NxN],N>0')
17 -def check_SO(x):
18 ''' Checks that the given value is a rotation matrix of arbitrary size. '''
19 check_orthogonal(x)
20 determinant = np.linalg.det(x * 1.0)
21
22
23 assert_allclose(determinant, 1.0)
24
28 ''' Check that the argument is an orthogonal matrix. '''
29 N = x.shape[0]
30 I = np.eye(N)
31 rtol = 10E-10
32 atol = 10E-7
33 assert_allclose(I, np.dot(x, x.T), rtol=rtol, atol=atol)
34 assert_allclose(I, np.dot(x.T, x), rtol=rtol, atol=atol)
35
39 ''' Check that the argument is a skew-symmetric matrix. '''
40 n = x.shape[0]
41 ok = (np.zeros((n, n)) == x).all()
42 if not ok:
43 diag = x.diagonal()
44 if not (diag == 0).all():
45 raise ValueError('Expected skew symmetric, but diagonal is not '
46 'exactly zero: %s.' % diag)
47 for i, j in itertools.product(range(n), range(n)):
48 if i < j:
49 continue
50 if x[i, j] != -x[j, i]:
51 raise ValueError('Expected skew symmetric, but ' +
52 'a[%d][%d] = %f, a[%d][%d] = %f' % \
53 (i, j, x[i, j], j, i, x[j, i]))
54
55
56 new_contract('orthogonal', check_orthogonal)
57 new_contract('SO', check_SO)
58 new_contract('skew_symmetric', check_skew_symmetric)
59
60 new_contract('SO2', 'array[2x2],SO')
61 new_contract('SO3', 'array[3x3],SO')
62
63 new_contract('so', 'skew_symmetric')
64 new_contract('so2', 'array[2x2],skew_symmetric')
65 new_contract('so3', 'array[3x3],skew_symmetric')
66
67
68 new_contract('rotation_matrix', 'SO3')
69
70
71 @contract(theta='number', returns='SO3')
72 -def rotz(theta):
73 ''' Returns a 3x3 rotation matrix corresponding
74 to rotation around the *z* axis. '''
75 return np.array([
76 [np.cos(theta), -np.sin(theta), 0],
77 [np.sin(theta), +np.cos(theta), 0],
78 [0, 0, 1]])
79
80
81 @contract(theta='number', returns='SO2')
82 -def SO2_from_angle(theta):
83 ''' Returns a 2x2 rotation matrix. '''
84 return np.array([
85 [+np.cos(theta), -np.sin(theta)],
86 [+np.sin(theta), +np.cos(theta)]])
87
88
89 @contract(R='SO2', returns='float')
91 angle = np.arctan2(R[1, 0], R[0, 0])
92 if angle == np.pi:
93 angle = -np.pi
94 return angle
95
96
97 @contract(omega='number', returns='so2')
98 -def hat_map_2d(omega):
99 return np.array([[0, -1], [+1, 0]]) * omega
100
101
102 @contract(W='so2', returns='float')
103 -def map_hat_2d(W):
105
106 rot2d = SO2_from_angle
107 rot2d_from_angle = SO2_from_angle
108 angle_from_rot2d = angle_from_SO2
113 ''' Generate a random quaternion.
114
115 Uses the algorithm used in Kuffner, ICRA'04.
116 '''
117 s = np.random.uniform()
118 sigma1 = np.sqrt(1 - s)
119 sigma2 = np.sqrt(s)
120 theta1 = np.random.uniform() * 2 * np.pi
121 theta2 = np.random.uniform() * 2 * np.pi
122
123 q = np.array([np.cos(theta2) * sigma2,
124 np.sin(theta1) * sigma1,
125 np.cos(theta1) * sigma1,
126 np.sin(theta2) * sigma2])
127
128 q *= np.sign(q[0])
129 return q
130
131
132 @contract(returns='array[2x2]|rotation_matrix', ndim='2|3')
133 -def random_rotation(ndim=3):
134 ''' Generate a random rotation matrix.
135
136 This is a wrapper around :py:func:`random_quaternion`.
137 '''
138 if ndim == 3:
139 q = random_quaternion()
140 return rotation_from_quaternion(q)
141 elif ndim == 2:
142 return rot2d(np.random.uniform(0, 2 * np.pi))
143 else:
144 assert False
145
151
152
153 @contract(R1='rotation_matrix', R2='rotation_matrix', returns='float,>=0,<=pi')
155 '''
156 Returns the geodesic distance between two rotation matrices.
157
158 It is computed as the angle of the rotation :math:`R_1^{*} R_2^{-1}``.
159
160 '''
161 R = np.dot(R1, R2.T)
162 axis1, angle1 = axis_angle_from_rotation(R)
163 return angle1
164
165
166 @contract(v='array[3]', returns='array[3x3],skew_symmetric')
167 -def hat_map(v):
168 ''' Maps a vector to a 3x3 skew symmetric matrix. '''
169 h = np.zeros((3, 3))
170 h[0, 1] = -v[2]
171 h[0, 2] = v[1]
172 h[1, 2] = -v[0]
173 h = h - h.transpose()
174 return h
175
176
177 @contract(H='array[3x3],skew_symmetric', returns='array[3]')
178 -def map_hat(H):
179 ''' The inverse of :py:func:`hat_map`. '''
180 v = np.zeros(3)
181 v[2] = -H[0, 1]
182 v[1] = H[0, 2]
183 v[0] = -H[1, 2]
184 return v
185
189 '''
190 Converts a quaternion to a rotation matrix.
191
192 '''
193
194
195 a, b, c, d = x
196
197 r1 = [a ** 2 + b ** 2 - c ** 2 - d ** 2,
198 2 * b * c - 2 * a * d,
199 2 * b * d + 2 * a * c]
200 r2 = [2 * b * c + 2 * a * d,
201 a ** 2 - b ** 2 + c ** 2 - d ** 2,
202 2 * c * d - 2 * a * b]
203 r3 = [2 * b * d - 2 * a * c,
204 2 * c * d + 2 * a * b,
205 a ** 2 - b ** 2 - c ** 2 + d ** 2]
206
207 return np.array([r1, r2, r3])
208
209
210 @contract(R='rotation_matrix', returns='unit_quaternion')
212 '''
213 Converts a rotation matrix to a quaternion.
214
215 This is the robust method mentioned on wikipedia:
216
217 <http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation>
218
219 TODO: add the more robust method with 4x4 matrix and eigenvector
220 '''
221 largest = np.argmax(R.diagonal())
222 permutations = {0: [0, 1, 2],
223 1: [1, 2, 0],
224 2: [2, 0, 1]}
225 u, v, w = permutations[largest]
226 rr = 1 + R[u, u] - R[v, v] - R[w, w]
227 assert rr >= 0
228 r = np.sqrt(rr)
229 if r == 0:
230 return quaternion_from_axis_angle(default_axis(), 0.0)
231 else:
232 q0 = (R[w, v] - R[v, w]) / (2 * r)
233 qu = (r / 2)
234 qv = (R[u, v] + R[v, u]) / (2 * r)
235 qw = (R[w, u] + R[u, w]) / (2 * r)
236
237 Q = np.zeros(4)
238 Q[0] = q0
239 Q[u + 1] = qu
240 Q[v + 1] = qv
241 Q[w + 1] = qw
242 if Q[0] < 0:
243 Q = -Q
244 return Q
245
249 '''
250 Computes a quaternion corresponding to the rotation of *angle* radians
251 around the given *axis*.
252
253 This is the inverse of :py:func:`axis_angle_from_quaternion`.
254 '''
255 Q = np.array([
256 np.cos(angle / 2),
257 axis[0] * np.sin(angle / 2),
258 axis[1] * np.sin(angle / 2),
259 axis[2] * np.sin(angle / 2)
260 ])
261 Q *= np.sign(Q[0])
262 return Q
263
267 '''
268 This is the inverse of :py:func:`quaternion_from_axis_angle`.
269 '''
270 angle = 2 * safe_arccos(q[0])
271 if angle == 0:
272 axis = default_axis()
273 else:
274 axis = q[1:] / np.sin(angle / 2)
275 axis = axis / np.linalg.norm(axis)
276 if angle > np.pi:
277 angle -= 2 * np.pi
278 elif angle < -np.pi:
279 angle += 2 * np.pi
280
281 return axis, angle
282
283
284 @contract(axis='direction', angle='float', returns='rotation_matrix')
285 -def rotation_from_axis_angle(axis, angle):
286 '''
287 Computes the rotation matrix from the *(axis,angle)* representation
288 using Rodriguez's formula.
289 '''
290 w = axis
291 w_hat = hat_map(w)
292 w_hat2 = np.dot(w_hat, w_hat)
293 R = np.eye(3) + w_hat * np.sin(angle) + w_hat2 * (1 - np.cos(angle))
294 return R
295
296
297 @contract(R='rotation_matrix', returns='axis_angle_canonical')
299 '''
300 Returns the *(axis,angle)* representation of a given rotation.
301
302 There are a couple of symmetries:
303
304 * By convention, the angle returned is nonnegative.
305
306 * If the angle is 0, any axis will do.
307 In that case, :py:func:`default_axis` will be returned.
308
309 '''
310 angle = safe_arccos((R.trace() - 1) / 2)
311
312 if angle == 0:
313 return default_axis(), 0.0
314 else:
315 v = np.array([R[2, 1] - R[1, 2],
316 R[0, 2] - R[2, 0],
317 R[1, 0] - R[0, 1]])
318
319 computer_with_infinite_precision = False
320 if computer_with_infinite_precision:
321 axis = (1 / (2 * np.sin(angle))) * v
322 else:
323
324
325
326 axis = (v * np.sign(angle)) / np.linalg.norm(v)
327
328 return axis, angle
329
333 '''
334 Get the rotation from the *(axis,angle)* representation.
335
336 This is an alternative to :py:func:`rotation_from_axis_angle` which
337 goes through the quaternion representation instead of using Rodrigues'
338 formula.
339 '''
340 q = quaternion_from_axis_angle(axis, angle)
341 return rotation_from_quaternion(q)
342
343
344 @contract(x_axis='direction', vector_on_xy_plane='direction',
345 returns='rotation_matrix')
347 """
348 Creates a rotation matrix from the axes.
349 ``x_axis`` is the new direction of the (1,0,0) vector
350 after this rotation. ``vector_on_xy_plane`` is a vector
351 that must end up in the (x,y) plane after the rotation.
352
353 That is, it holds that: ::
354
355 R = rotation_from_axes_spec(x, v)
356 dot(R,x) == [1,0,0]
357 dot(R,v) == [?,?,0]
358
359 TODO: add exception if vectors are too close.
360 """
361 z_axis = normalize_length(np.cross(x_axis, vector_on_xy_plane))
362 y_axis = normalize_length(np.cross(z_axis, x_axis))
363 R = np.vstack((x_axis, y_axis, z_axis))
364 return R.copy('C')
365