quaternion.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 
4 """
5 Quaternion implementation for use in pymavlink
6 """
7 
8 from __future__ import absolute_import, division, print_function
9 
10 from builtins import object
11 import numpy as np
12 from .rotmat import Vector3, Matrix3
13 
14 __author__ = "Thomas Gubler"
15 __copyright__ = "Copyright (C) 2014 Thomas Gubler"
16 __license__ = "GNU Lesser General Public License v3"
17 __email__ = "thomasgubler@gmail.com"
18 
19 
20 class QuaternionBase(object):
21 
22  """
23  Quaternion class, this is the version which supports numpy arrays
24  If you need support for Matrix3 look at the Quaternion class
25 
26  Usage:
27  >>> from quaternion import QuaternionBase
28  >>> import numpy as np
29  >>> q = QuaternionBase([np.radians(20), np.radians(20), np.radians(20)])
30  >>> print(q)
31  [ 0.9603483 0.13871646 0.19810763 0.13871646]
32  >>> print(q.dcm)
33  [[ 0.88302222 -0.21147065 0.41898917]
34  [ 0.3213938 0.92303098 -0.21147065]
35  [-0.34202014 0.3213938 0.88302222]]
36  >>> q = QuaternionBase([1, 0, 0, 0])
37  >>> print(q.euler)
38  [ 0. -0. 0.]
39  >>> m = [[1, 0, 0], [0, 0, -1], [0, 1, 0]]
40  >>> q = QuaternionBase(m)
41  >>> vector = [0, 1, 0]
42  >>> vector2 = q.transform(vector)
43  """
44 
45  def __init__(self, attitude=[1, 0, 0, 0]):
46  """
47  Construct a quaternion from an attitude
48 
49  :param attitude: another QuaternionBase,
50  3 element list [roll, pitch, yaw],
51  4 element list [w, x, y ,z], DCM (3x3 array)
52  """
53  if isinstance(attitude, QuaternionBase):
54  self.q = attitude.q
55  elif np.array(attitude).shape == (3, 3):
56  self.dcm = attitude
57  elif len(attitude) == 4:
58  self.q = attitude
59  elif len(attitude) == 3:
60  self.euler = attitude
61  else:
62  raise TypeError("attitude is not valid")
63 
64  @property
65  def q(self):
66  """
67  Get the quaternion
68  :returns: array containing the quaternion elements
69  """
70  if self._q is None:
71  if self._euler is not None:
72  # get q from euler
73  self._q = self._euler_to_q(self.euler)
74  elif self._dcm is not None:
75  # get q from DCM
76  self._q = self._dcm_to_q(self.dcm)
77  return self._q
78 
79  def __getitem__(self, index):
80  """Returns the quaternion entry at index"""
81  return self.q[index]
82 
83  @q.setter
84  def q(self, q):
85  """
86  Set the quaternion
87  :param q: list or array of quaternion values [w, x, y, z]
88  """
89  self._q = np.array(q)
90 
91  # mark other representations as outdated, will get generated on next
92  # read
93  self._euler = None
94  self._dcm = None
95 
96  @property
97  def euler(self):
98  """
99  Get the euler angles.
100  The convention is Tait-Bryan (ZY'X'')
101 
102  :returns: array containing the euler angles [roll, pitch, yaw]
103  """
104  if self._euler is None:
105  if self._q is not None:
106  # try to get euler angles from q via DCM
107  self._dcm = self._q_to_dcm(self.q)
108  self._euler = self._dcm_to_euler(self.dcm)
109  elif self._dcm is not None:
110  # get euler angles from DCM
111  self._euler = self._dcm_to_euler(self.dcm)
112  return self._euler
113 
114  @euler.setter
115  def euler(self, euler):
116  """
117  Set the euler angles
118  :param euler: list or array of the euler angles [roll, pitch, yaw]
119 
120  """
121  assert(len(euler) == 3)
122  self._euler = np.array(euler)
123 
124  # mark other representations as outdated, will get generated on next
125  # read
126  self._q = None
127  self._dcm = None
128 
129  @property
130  def dcm(self):
131  """
132  Get the DCM
133 
134  :returns: 3x3 array
135  """
136  if self._dcm is None:
137  if self._q is not None:
138  # try to get dcm from q
139  self._dcm = self._q_to_dcm(self.q)
140  elif self._euler is not None:
141  # try to get get dcm from euler
142  self._dcm = self._euler_to_dcm(self._euler)
143  return self._dcm
144 
145  @dcm.setter
146  def dcm(self, dcm):
147  """
148  Set the DCM
149  :param dcm: 3x3 array
150 
151  """
152  assert(len(dcm) == 3)
153  for sub in dcm:
154  assert(len(sub) == 3)
155 
156  self._dcm = np.array(dcm)
157 
158  # mark other representations as outdated, will get generated on next
159  # read
160  self._q = None
161  self._euler = None
162 
163  def transform(self, v):
164  """
165  Calculates the vector transformed by this quaternion
166  :param v: array with len 3 to be transformed
167  :returns: transformed vector
168  """
169  assert(len(v) == 3)
170  assert(np.allclose(self.norm, 1))
171  # perform transformation t = q * [0, v] * q^-1 but avoid multiplication
172  # because terms cancel out
173  q0 = self.q[0]
174  qi = self.q[1:4]
175  ui = np.array(v)
176  a = q0 * ui + np.cross(qi, ui)
177  t = np.dot(qi, ui) * qi + q0 * a - np.cross(a, qi)
178  return t
179 
180  @property
181  def norm(self):
182  """
183  Returns norm of quaternion
184 
185  :returns: norm (scalar)
186  """
187  return QuaternionBase.norm_array(self.q)
188 
189  def normalize(self):
190  """Normalizes the quaternion"""
191  self.q = QuaternionBase.normalize_array(self.q)
192 
193  @property
194  def inversed(self):
195  """
196  Get inversed quaternion
197 
198  :returns: inversed quaternion
199  """
200  q_inv = self._q_inversed(self.q)
201  return QuaternionBase(q_inv)
202 
203  def __eq__(self, other):
204  """
205  Equality test (same orientation, not necessarily same rotation)
206 
207  :param other: a QuaternionBase
208  :returns: true if the quaternions are equal
209  """
210  if isinstance(other, QuaternionBase):
211  return abs(self.q.dot(other.q)) > 1 - np.finfo(float).eps
212  return NotImplemented
213 
214  def close(self, other):
215  """
216  Equality test with tolerance
217  (same orientation, not necessarily same rotation)
218 
219 
220  :param other: a QuaternionBase
221  :returns: true if the quaternions are almost equal
222  """
223  if isinstance(other, QuaternionBase):
224  return np.allclose(self.q, other.q) or np.allclose(self.q, -other.q)
225  return NotImplemented
226 
227  def __mul__(self, other):
228  """
229  :param other: QuaternionBase
230  :returns: multiplaction of this Quaternion with other
231  """
232  if isinstance(other, QuaternionBase):
233  o = other.q
234  elif len(other) == 4:
235  o = other
236  else:
237  return NotImplemented
238 
239  return QuaternionBase(self._mul_array(self.q, o))
240 
241  def __truediv__(self, other):
242  """
243  :param other: QuaternionBase
244  :returns: division of this Quaternion with other
245  """
246  if isinstance(other, QuaternionBase):
247  o = other
248  elif len(other) == 4:
249  o = QuaternionBase(other)
250  else:
251  return NotImplemented
252  return self * o.inversed
253 
254  @staticmethod
256  """
257  Normalizes the list with len 4 so that it can be used as quaternion
258  :param q: array of len 4
259  :returns: normalized array
260  """
261  assert(len(q) == 4)
262  q = np.array(q)
263  n = QuaternionBase.norm_array(q)
264  return q / n
265 
266  @staticmethod
267  def norm_array(q):
268  """
269  Calculate quaternion norm on array q
270  :param quaternion: array of len 4
271  :returns: norm (scalar)
272  """
273  assert(len(q) == 4)
274  return np.sqrt(np.dot(q, q))
275 
276  def _mul_array(self, p, q):
277  """
278  Performs multiplication of the 2 quaterniona arrays p and q
279  :param p: array of len 4
280  :param q: array of len 4
281  :returns: array of len, result of p * q (with p, q quaternions)
282  """
283  assert(len(q) == len(p) == 4)
284  p0 = p[0]
285  pi = p[1:4]
286  q0 = q[0]
287  qi = q[1:4]
288 
289  res = np.zeros(4)
290  res[0] = p0 * q0 - np.dot(pi, qi)
291  res[1:4] = p0 * qi + q0 * pi + np.cross(pi, qi)
292 
293  return res
294 
295  def _euler_to_q(self, euler):
296  """
297  Create q array from euler angles
298  :param euler: array [roll, pitch, yaw] in rad
299  :returns: array q which represents a quaternion [w, x, y, z]
300  """
301  assert(len(euler) == 3)
302  phi = euler[0]
303  theta = euler[1]
304  psi = euler[2]
305  c_phi_2 = np.cos(phi / 2)
306  s_phi_2 = np.sin(phi / 2)
307  c_theta_2 = np.cos(theta / 2)
308  s_theta_2 = np.sin(theta / 2)
309  c_psi_2 = np.cos(psi / 2)
310  s_psi_2 = np.sin(psi / 2)
311  q = np.zeros(4)
312  q[0] = (c_phi_2 * c_theta_2 * c_psi_2 +
313  s_phi_2 * s_theta_2 * s_psi_2)
314  q[1] = (s_phi_2 * c_theta_2 * c_psi_2 -
315  c_phi_2 * s_theta_2 * s_psi_2)
316  q[2] = (c_phi_2 * s_theta_2 * c_psi_2 +
317  s_phi_2 * c_theta_2 * s_psi_2)
318  q[3] = (c_phi_2 * c_theta_2 * s_psi_2 -
319  s_phi_2 * s_theta_2 * c_psi_2)
320  return q
321 
322  def _q_to_dcm(self, q):
323  """
324  Create DCM from q
325  :param q: array q which represents a quaternion [w, x, y, z]
326  :returns: 3x3 dcm array
327  """
328  assert(len(q) == 4)
329  assert(np.allclose(QuaternionBase.norm_array(q), 1))
330  dcm = np.zeros([3, 3])
331  a = q[0]
332  b = q[1]
333  c = q[2]
334  d = q[3]
335  a_sq = a * a
336  b_sq = b * b
337  c_sq = c * c
338  d_sq = d * d
339  dcm[0][0] = a_sq + b_sq - c_sq - d_sq
340  dcm[0][1] = 2 * (b * c - a * d)
341  dcm[0][2] = 2 * (a * c + b * d)
342  dcm[1][0] = 2 * (b * c + a * d)
343  dcm[1][1] = a_sq - b_sq + c_sq - d_sq
344  dcm[1][2] = 2 * (c * d - a * b)
345  dcm[2][0] = 2 * (b * d - a * c)
346  dcm[2][1] = 2 * (a * b + c * d)
347  dcm[2][2] = a_sq - b_sq - c_sq + d_sq
348  return dcm
349 
350  def _dcm_to_q(self, dcm):
351  """
352  Create q from dcm
353  Reference:
354  - Shoemake, Quaternions,
355  http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
356 
357  :param dcm: 3x3 dcm array
358  returns: quaternion array
359  """
360  assert(dcm.shape == (3, 3))
361  q = np.zeros(4)
362 
363  tr = np.trace(dcm)
364  if tr > 0:
365  s = np.sqrt(tr + 1.0)
366  q[0] = s * 0.5
367  s = 0.5 / s
368  q[1] = (dcm[2][1] - dcm[1][2]) * s
369  q[2] = (dcm[0][2] - dcm[2][0]) * s
370  q[3] = (dcm[1][0] - dcm[0][1]) * s
371  else:
372  dcm_i = np.argmax(np.diag(dcm))
373  dcm_j = (dcm_i + 1) % 3
374  dcm_k = (dcm_i + 2) % 3
375 
376  s = np.sqrt((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
377  dcm[dcm_k][dcm_k]) + 1.0)
378  q[dcm_i + 1] = s * 0.5
379  s = 0.5 / s
380  q[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s
381  q[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s
382  q[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s
383 
384  return q
385 
386  def _euler_to_dcm(self, euler):
387  """
388  Create DCM from euler angles
389  :param euler: array [roll, pitch, yaw] in rad
390  :returns: 3x3 dcm array
391  """
392  assert(len(euler) == 3)
393  phi = euler[0]
394  theta = euler[1]
395  psi = euler[2]
396  dcm = np.zeros([3, 3])
397  c_phi = np.cos(phi)
398  s_phi = np.sin(phi)
399  c_theta = np.cos(theta)
400  s_theta = np.sin(theta)
401  c_psi = np.cos(psi)
402  s_psi = np.sin(psi)
403 
404  dcm[0][0] = c_theta * c_psi
405  dcm[0][1] = -c_phi * s_psi + s_phi * s_theta * c_psi
406  dcm[0][2] = s_phi * s_psi + c_phi * s_theta * c_psi
407 
408  dcm[1][0] = c_theta * s_psi
409  dcm[1][1] = c_phi * c_psi + s_phi * s_theta * s_psi
410  dcm[1][2] = -s_phi * c_psi + c_phi * s_theta * s_psi
411 
412  dcm[2][0] = -s_theta
413  dcm[2][1] = s_phi * c_theta
414  dcm[2][2] = c_phi * c_theta
415  return dcm
416 
417  def _dcm_to_euler(self, dcm):
418  """
419  Create DCM from euler angles
420  :param dcm: 3x3 dcm array
421  :returns: array [roll, pitch, yaw] in rad
422  """
423  assert(dcm.shape == (3, 3))
424  theta = np.arcsin(min(1, max(-1, -dcm[2][0])))
425 
426  if abs(theta - np.pi/2) < 1.0e-3:
427  phi = 0.0
428  psi = (np.arctan2(dcm[1][2] - dcm[0][1],
429  dcm[0][2] + dcm[1][1]) + phi)
430  elif abs(theta + np.pi/2) < 1.0e-3:
431  phi = 0.0
432  psi = np.arctan2(dcm[1][2] - dcm[0][1],
433  dcm[0][2] + dcm[1][1] - phi)
434  else:
435  phi = np.arctan2(dcm[2][1], dcm[2][2])
436  psi = np.arctan2(dcm[1][0], dcm[0][0])
437 
438  return np.array([phi, theta, psi])
439 
440  def _q_inversed(self, q):
441  """
442  Returns inversed quaternion q
443  :param q: array q which represents a quaternion [w, x, y, z]
444  :returns: inversed array q which is a quaternion [w, x, y ,z]
445  """
446  assert(len(q) == 4)
447  return np.hstack([q[0], -q[1:4]])
448 
449  def __str__(self):
450  """String of quaternion values"""
451  return str(self.q)
452 
453 
455 
456  """
457  Quaternion class that supports pymavlink's Vector3 and Matrix3
458 
459  Usage:
460  >>> from quaternion import Quaternion
461  >>> from rotmat import Vector3, Matrix3
462  >>> m = Matrix3()
463  >>> m.from_euler(45, 0, 0)
464  >>> print(m)
465  Matrix3((1.00, 0.00, 0.00), (0.00, 0.53, -0.85), (-0.00, 0.85, 0.53))
466  >>> q = Quaternion(m)
467  >>> print(q)
468  [ 0.87330464 0.48717451 0. 0. ]
469  >>> print(q.dcm)
470  Matrix3((1.00, 0.00, 0.00), (0.00, 0.53, -0.85), (-0.00, 0.85, 0.53))
471  >>> v = Vector3(0, 1, 0)
472  >>> v2 = q.transform(v)
473  >>> print(v2)
474  Vector3(0.00, 0.53, 0.85)
475  """
476 
477  def __init__(self, attitude):
478  """
479  Construct a quaternion from an attitude
480 
481  :param attitude: another Quaternion, QuaternionBase,
482  3 element list [roll, pitch, yaw],
483  4 element list [w, x, y ,z], DCM (3x3 array or Matrix3)
484  """
485  if isinstance(attitude, Quaternion):
486  self.q = attitude.q
487  if isinstance(attitude, Matrix3):
488  self.dcm = attitude
489  elif np.array(attitude).shape == (3, 3):
490  # convert dcm array to Matrix3
491  self.dcm = self._dcm_array_to_matrix3(attitude)
492  elif isinstance(attitude, Vector3):
493  # provided euler angles
494  euler = [attitude.x, attitude.y, attitude.z]
495  super(Quaternion, self).__init__(euler)
496  else:
497  super(Quaternion, self).__init__(attitude)
498 
499  @property
500  def dcm(self):
501  """
502  Get the DCM
503 
504  :returns: Matrix3
505  """
506  if self._dcm is None:
507  if self._q is not None:
508  # try to get dcm from q
509  self._dcm = self._q_to_dcm(self.q)
510  elif self._euler is not None:
511  # try to get get dcm from euler
512  self._dcm = self._euler_to_dcm(self._euler)
513  return self._dcm
514 
515  @dcm.setter
516  def dcm(self, dcm):
517  """
518  Set the DCM
519  :param dcm: Matrix3
520 
521  """
522  assert(isinstance(dcm, Matrix3))
523  self._dcm = dcm.copy()
524 
525  # mark other representations as outdated, will get generated on next
526  # read
527  self._q = None
528  self._euler = None
529 
530  @property
531  def inversed(self):
532  """
533  Get inversed quaternion
534 
535  :returns: inversed quaternion
536  """
537  return Quaternion(super(Quaternion, self).inversed)
538 
539  def transform(self, v3):
540  """
541  Calculates the vector transformed by this quaternion
542  :param v3: Vector3 to be transformed
543  :returns: transformed vector
544  """
545  if isinstance(v3, Vector3):
546  t = super(Quaternion, self).transform([v3.x, v3.y, v3.z])
547  return Vector3(t[0], t[1], t[2])
548  elif len(v3) == 3:
549  return super(Quaternion, self).transform(v3)
550  else:
551  raise TypeError("param v3 is not a vector type")
552 
553  def _dcm_array_to_matrix3(self, dcm):
554  """
555  Converts dcm array into Matrix3
556  :param dcm: 3x3 dcm array
557  :returns: Matrix3
558  """
559  assert(dcm.shape == (3, 3))
560  a = Vector3(dcm[0][0], dcm[0][1], dcm[0][2])
561  b = Vector3(dcm[1][0], dcm[1][1], dcm[1][2])
562  c = Vector3(dcm[2][0], dcm[2][1], dcm[2][2])
563  return Matrix3(a, b, c)
564 
565  def _matrix3_to_dcm_array(self, m):
566  """
567  Converts Matrix3 in an array
568  :param m: Matrix3
569  :returns: 3x3 array
570  """
571  assert(isinstance(m, Matrix3))
572  return np.array([[m.a.x, m.a.y, m.a.z],
573  [m.b.x, m.b.y, m.b.z],
574  [m.c.x, m.c.y, m.c.z]])
575 
576  def _q_to_dcm(self, q):
577  """
578  Create DCM (Matrix3) from q
579  :param q: array q which represents a quaternion [w, x, y, z]
580  :returns: Matrix3
581  """
582  assert(len(q) == 4)
583  arr = super(Quaternion, self)._q_to_dcm(q)
584  return self._dcm_array_to_matrix3(arr)
585 
586  def _dcm_to_q(self, dcm):
587  """
588  Create q from dcm (Matrix3)
589  :param dcm: Matrix3
590  :returns: array q which represents a quaternion [w, x, y, z]
591  """
592  assert(isinstance(dcm, Matrix3))
593  arr = self._matrix3_to_dcm_array(dcm)
594  return super(Quaternion, self)._dcm_to_q(arr)
595 
596  def _euler_to_dcm(self, euler):
597  """
598  Create DCM (Matrix3) from euler angles
599  :param euler: array [roll, pitch, yaw] in rad
600  :returns: Matrix3
601  """
602  assert(len(euler) == 3)
603  m = Matrix3()
604  m.from_euler(*euler)
605  return m
606 
607  def _dcm_to_euler(self, dcm):
608  """
609  Create DCM from euler angles
610  :param dcm: Matrix3
611  :returns: array [roll, pitch, yaw] in rad
612  """
613  assert(isinstance(dcm, Matrix3))
614  return np.array(dcm.to_euler())
615 
616  def __mul__(self, other):
617  """
618  :param other: Quaternion
619  :returns: multiplaction of this Quaternion with other
620  """
621  return Quaternion(super(Quaternion, self).__mul__(other))
622 
623  def __truediv__(self, other):
624  """
625  :param other: Quaternion
626  :returns: division of this Quaternion with other
627  """
628  return Quaternion(super(Quaternion, self).__truediv__(other))
629 
630 if __name__ == "__main__":
631  import doctest
632  doctest.testmod()
633  import unittest
634  unittest.main('quaterniontest')


mavlink
Author(s): Lorenz Meier
autogenerated on Sun Jul 7 2019 03:22:07