pyquaternion.py
Go to the documentation of this file.
1 """
2 This file is part of the pyquaternion python module
3 
4 Author: Kieran Wynn
5 Website: https://github.com/KieranWynn/pyquaternion
6 Documentation: http://kieranwynn.github.io/pyquaternion/
7 
8 Version: 1.0.0
9 License: The MIT License (MIT)
10 
11 Copyright (c) 2015 Kieran Wynn
12 
13 Permission is hereby granted, free of charge, to any person obtaining a copy
14 of this software and associated documentation files (the "Software"), to deal
15 in the Software without restriction, including without limitation the rights
16 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
17 copies of the Software, and to permit persons to whom the Software is
18 furnished to do so, subject to the following conditions:
19 
20 The above copyright notice and this permission notice shall be included in all
21 copies or substantial portions of the Software.
22 
23 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
24 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
25 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
26 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
27 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
28 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
29 SOFTWARE.
30 
31 quaternion.py - This file defines the core Quaternion class
32 
33 """
34 
35 from __future__ import absolute_import, division, print_function # Add compatibility for Python 2.7+
36 
37 from math import sqrt, pi, sin, cos, asin, acos, atan2, exp, log
38 from copy import deepcopy
39 import numpy as np # Numpy is required for many vector operations
40 
41 
42 class Quaternion(object):
43  """Class to represent a 4-dimensional complex number or quaternion.
44 
45  Quaternion objects can be used generically as 4D numbers,
46  or as unit quaternions to represent rotations in 3D space.
47 
48  Attributes:
49  q: Quaternion 4-vector represented as a Numpy array
50 
51  """
52 
53  def __init__(self, *args, **kwargs):
54  """Initialise a new Quaternion object.
55 
56  See Object Initialisation docs for complete behaviour:
57 
58  http://kieranwynn.github.io/pyquaternion/initialisation/
59 
60  """
61  s = len(args)
62  if s == 0:
63  # No positional arguments supplied
64  if kwargs:
65  # Keyword arguments provided
66  if ("scalar" in kwargs) or ("vector" in kwargs):
67  scalar = kwargs.get("scalar", 0.0)
68  if scalar is None:
69  scalar = 0.0
70  else:
71  scalar = float(scalar)
72 
73  vector = kwargs.get("vector", [])
74  vector = self._validate_number_sequence(vector, 3)
75 
76  self.q = np.hstack((scalar, vector))
77  elif ("real" in kwargs) or ("imaginary" in kwargs):
78  real = kwargs.get("real", 0.0)
79  if real is None:
80  real = 0.0
81  else:
82  real = float(real)
83 
84  imaginary = kwargs.get("imaginary", [])
85  imaginary = self._validate_number_sequence(imaginary, 3)
86 
87  self.q = np.hstack((real, imaginary))
88  elif ("axis" in kwargs) or ("radians" in kwargs) or ("degrees" in kwargs) or ("angle" in kwargs):
89  try:
90  axis = self._validate_number_sequence(kwargs["axis"], 3)
91  except KeyError:
92  raise ValueError(
93  "A valid rotation 'axis' parameter must be provided to describe a meaningful rotation."
94  )
95  angle = kwargs.get('radians') or self.to_radians(kwargs.get('degrees')) or kwargs.get('angle') or 0.0
96  self.q = Quaternion._from_axis_angle(axis, angle).q
97  elif "array" in kwargs:
98  self.q = self._validate_number_sequence(kwargs["array"], 4)
99  elif "matrix" in kwargs:
100  optional_args = {key: kwargs[key] for key in kwargs if key in ['rtol', 'atol']}
101  self.q = Quaternion._from_matrix(kwargs["matrix"], **optional_args).q
102  else:
103  keys = sorted(kwargs.keys())
104  elements = [kwargs[kw] for kw in keys]
105  if len(elements) == 1:
106  r = float(elements[0])
107  self.q = np.array([r, 0.0, 0.0, 0.0])
108  else:
109  self.q = self._validate_number_sequence(elements, 4)
110 
111  else:
112  # Default initialisation
113  self.q = np.array([1.0, 0.0, 0.0, 0.0])
114  elif s == 1:
115  # Single positional argument supplied
116  if isinstance(args[0], Quaternion):
117  self.q = args[0].q
118  return
119  if args[0] is None:
120  raise TypeError("Object cannot be initialised from {}".format(type(args[0])))
121  try:
122  r = float(args[0])
123  self.q = np.array([r, 0.0, 0.0, 0.0])
124  return
125  except TypeError:
126  pass # If the single argument is not scalar, it should be a sequence
127 
128  self.q = self._validate_number_sequence(args[0], 4)
129  return
130 
131  else:
132  # More than one positional argument supplied
133  self.q = self._validate_number_sequence(args, 4)
134 
135  def __hash__(self):
136  return hash(tuple(self.q))
137 
138  def _validate_number_sequence(self, seq, n):
139  """Validate a sequence to be of a certain length and ensure it's a numpy array of floats.
140 
141  Raises:
142  ValueError: Invalid length or non-numeric value
143  """
144  if seq is None:
145  return np.zeros(n)
146  if len(seq) == n:
147  try:
148  l = [float(e) for e in seq]
149  except ValueError:
150  raise ValueError("One or more elements in sequence <{!r}> cannot be interpreted as a real number".format(seq))
151  else:
152  return np.asarray(l)
153  elif len(seq) == 0:
154  return np.zeros(n)
155  else:
156  raise ValueError("Unexpected number of elements in sequence. Got: {}, Expected: {}.".format(len(seq), n))
157 
158  # Initialise from matrix
159  @classmethod
160  def _from_matrix(cls, matrix, rtol=1e-05, atol=1e-08):
161  """Initialise from matrix representation
162 
163  Create a Quaternion by specifying the 3x3 rotation or 4x4 transformation matrix
164  (as a numpy array) from which the quaternion's rotation should be created.
165 
166  """
167  try:
168  shape = matrix.shape
169  except AttributeError:
170  raise TypeError("Invalid matrix type: Input must be a 3x3 or 4x4 numpy array or matrix")
171 
172  if shape == (3, 3):
173  R = matrix
174  elif shape == (4, 4):
175  R = matrix[:-1][:,:-1] # Upper left 3x3 sub-matrix
176  else:
177  raise ValueError("Invalid matrix shape: Input must be a 3x3 or 4x4 numpy array or matrix")
178 
179  # Check matrix properties
180  if not np.allclose(np.dot(R, R.conj().transpose()), np.eye(3), rtol=rtol, atol=atol):
181  raise ValueError("Matrix must be orthogonal, i.e. its transpose should be its inverse")
182  if not np.isclose(np.linalg.det(R), 1.0, rtol=rtol, atol=atol):
183  raise ValueError("Matrix must be special orthogonal i.e. its determinant must be +1.0")
184 
185  def decomposition_method(matrix):
186  """ Method supposedly able to deal with non-orthogonal matrices - NON-FUNCTIONAL!
187  Based on this method: http://arc.aiaa.org/doi/abs/10.2514/2.4654
188  """
189  x, y, z = 0, 1, 2 # indices
190  K = np.array([
191  [R[x, x]-R[y, y]-R[z, z], R[y, x]+R[x, y], R[z, x]+R[x, z], R[y, z]-R[z, y]],
192  [R[y, x]+R[x, y], R[y, y]-R[x, x]-R[z, z], R[z, y]+R[y, z], R[z, x]-R[x, z]],
193  [R[z, x]+R[x, z], R[z, y]+R[y, z], R[z, z]-R[x, x]-R[y, y], R[x, y]-R[y, x]],
194  [R[y, z]-R[z, y], R[z, x]-R[x, z], R[x, y]-R[y, x], R[x, x]+R[y, y]+R[z, z]]
195  ])
196  K = K / 3.0
197 
198  e_vals, e_vecs = np.linalg.eig(K)
199  print('Eigenvalues:', e_vals)
200  print('Eigenvectors:', e_vecs)
201  max_index = np.argmax(e_vals)
202  principal_component = e_vecs[max_index]
203  return principal_component
204 
205  def trace_method(matrix):
206  """
207  This code uses a modification of the algorithm described in:
208  https://d3cw3dd2w32x2b.cloudfront.net/wp-content/uploads/2015/01/matrix-to-quat.pdf
209  which is itself based on the method described here:
210  http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
211 
212  Altered to work with the column vector convention instead of row vectors
213  """
214  m = matrix.conj().transpose() # This method assumes row-vector and postmultiplication of that vector
215  if m[2, 2] < 0:
216  if m[0, 0] > m[1, 1]:
217  t = 1 + m[0, 0] - m[1, 1] - m[2, 2]
218  q = [m[1, 2]-m[2, 1], t, m[0, 1]+m[1, 0], m[2, 0]+m[0, 2]]
219  else:
220  t = 1 - m[0, 0] + m[1, 1] - m[2, 2]
221  q = [m[2, 0]-m[0, 2], m[0, 1]+m[1, 0], t, m[1, 2]+m[2, 1]]
222  else:
223  if m[0, 0] < -m[1, 1]:
224  t = 1 - m[0, 0] - m[1, 1] + m[2, 2]
225  q = [m[0, 1]-m[1, 0], m[2, 0]+m[0, 2], m[1, 2]+m[2, 1], t]
226  else:
227  t = 1 + m[0, 0] + m[1, 1] + m[2, 2]
228  q = [t, m[1, 2]-m[2, 1], m[2, 0]-m[0, 2], m[0, 1]-m[1, 0]]
229 
230  q = np.array(q).astype('float64')
231  q *= 0.5 / sqrt(t)
232  return q
233 
234  return cls(array=trace_method(R))
235 
236  # Initialise from axis-angle
237  @classmethod
238  def _from_axis_angle(cls, axis, angle):
239  """Initialise from axis and angle representation
240 
241  Create a Quaternion by specifying the 3-vector rotation axis and rotation
242  angle (in radians) from which the quaternion's rotation should be created.
243 
244  Params:
245  axis: a valid numpy 3-vector
246  angle: a real valued angle in radians
247  """
248  mag_sq = np.dot(axis, axis)
249  if mag_sq == 0.0:
250  raise ZeroDivisionError("Provided rotation axis has no length")
251  # Ensure axis is in unit vector form
252  if (abs(1.0 - mag_sq) > 1e-12):
253  axis = axis / sqrt(mag_sq)
254  theta = angle / 2.0
255  r = cos(theta)
256  i = axis * sin(theta)
257 
258  return cls(r, i[0], i[1], i[2])
259 
260  @classmethod
261  def random(cls):
262  """Generate a random unit quaternion.
263 
264  Uniformly distributed across the rotation space
265  As per: http://planning.cs.uiuc.edu/node198.html
266  """
267  r1, r2, r3 = np.random.random(3)
268 
269  q1 = sqrt(1.0 - r1) * (sin(2 * pi * r2))
270  q2 = sqrt(1.0 - r1) * (cos(2 * pi * r2))
271  q3 = sqrt(r1) * (sin(2 * pi * r3))
272  q4 = sqrt(r1) * (cos(2 * pi * r3))
273 
274  return cls(q1, q2, q3, q4)
275 
276  # Representation
277  def __str__(self):
278  """An informal, nicely printable string representation of the Quaternion object.
279  """
280  return "{:.3f} {:+.3f}i {:+.3f}j {:+.3f}k".format(self.q[0], self.q[1], self.q[2], self.q[3])
281 
282  def __repr__(self):
283  """The 'official' string representation of the Quaternion object.
284 
285  This is a string representation of a valid Python expression that could be used
286  to recreate an object with the same value (given an appropriate environment)
287  """
288  return "Quaternion({!r}, {!r}, {!r}, {!r})".format(self.q[0], self.q[1], self.q[2], self.q[3])
289 
290  def __format__(self, formatstr):
291  """Inserts a customisable, nicely printable string representation of the Quaternion object
292 
293  The syntax for `format_spec` mirrors that of the built in format specifiers for floating point types.
294  Check out the official Python [format specification mini-language](https://docs.python.org/3.4/library/string.html#formatspec) for details.
295  """
296  if formatstr.strip() == '': # Defualt behaviour mirrors self.__str__()
297  formatstr = '+.3f'
298 
299  string = \
300  "{:" + formatstr +"} " + \
301  "{:" + formatstr +"}i " + \
302  "{:" + formatstr +"}j " + \
303  "{:" + formatstr +"}k"
304  return string.format(self.q[0], self.q[1], self.q[2], self.q[3])
305 
306  # Type Conversion
307  def __int__(self):
308  """Implements type conversion to int.
309 
310  Truncates the Quaternion object by only considering the real
311  component and rounding to the next integer value towards zero.
312  Note: to round to the closest integer, use int(round(float(q)))
313  """
314  return int(self.q[0])
315 
316  def __float__(self):
317  """Implements type conversion to float.
318 
319  Truncates the Quaternion object by only considering the real
320  component.
321  """
322  return float(self.q[0])
323 
324  def __complex__(self):
325  """Implements type conversion to complex.
326 
327  Truncates the Quaternion object by only considering the real
328  component and the first imaginary component.
329  This is equivalent to a projection from the 4-dimensional hypersphere
330  to the 2-dimensional complex plane.
331  """
332  return complex(self.q[0], self.q[1])
333 
334  def __bool__(self):
335  return not (self == Quaternion(0.0))
336 
337  def __nonzero__(self):
338  return not (self == Quaternion(0.0))
339 
340  def __invert__(self):
341  return (self == Quaternion(0.0))
342 
343  # Comparison
344  def __eq__(self, other):
345  """Returns true if the following is true for each element:
346  `absolute(a - b) <= (atol + rtol * absolute(b))`
347  """
348  if isinstance(other, Quaternion):
349  r_tol = 1.0e-13
350  a_tol = 1.0e-14
351  try:
352  isEqual = np.allclose(self.q, other.q, rtol=r_tol, atol=a_tol)
353  except AttributeError:
354  raise AttributeError("Error in internal quaternion representation means it cannot be compared like a numpy array.")
355  return isEqual
356  return self.__eq__(self.__class__(other))
357 
358  # Negation
359  def __neg__(self):
360  return self.__class__(array= -self.q)
361 
362  # Absolute value
363  def __abs__(self):
364  return self.norm
365 
366  # Addition
367  def __add__(self, other):
368  if isinstance(other, Quaternion):
369  return self.__class__(array=self.q + other.q)
370  return self + self.__class__(other)
371 
372  def __iadd__(self, other):
373  return self + other
374 
375  def __radd__(self, other):
376  return self + other
377 
378  # Subtraction
379  def __sub__(self, other):
380  return self + (-other)
381 
382  def __isub__(self, other):
383  return self + (-other)
384 
385  def __rsub__(self, other):
386  return -(self - other)
387 
388  # Multiplication
389  def __mul__(self, other):
390  if isinstance(other, Quaternion):
391  return self.__class__(array=np.dot(self._q_matrix(), other.q))
392  return self * self.__class__(other)
393 
394  def __imul__(self, other):
395  return self * other
396 
397  def __rmul__(self, other):
398  return self.__class__(other) * self
399 
400  def __matmul__(self, other):
401  if isinstance(other, Quaternion):
402  return self.q.__matmul__(other.q)
403  return self.__matmul__(self.__class__(other))
404 
405  def __imatmul__(self, other):
406  return self.__matmul__(other)
407 
408  def __rmatmul__(self, other):
409  return self.__class__(other).__matmul__(self)
410 
411  # Division
412  def __div__(self, other):
413  if isinstance(other, Quaternion):
414  if other == self.__class__(0.0):
415  raise ZeroDivisionError("Quaternion divisor must be non-zero")
416  return self * other.inverse
417  return self.__div__(self.__class__(other))
418 
419  def __idiv__(self, other):
420  return self.__div__(other)
421 
422  def __rdiv__(self, other):
423  return self.__class__(other) * self.inverse
424 
425  def __truediv__(self, other):
426  return self.__div__(other)
427 
428  def __itruediv__(self, other):
429  return self.__idiv__(other)
430 
431  def __rtruediv__(self, other):
432  return self.__rdiv__(other)
433 
434  # Exponentiation
435  def __pow__(self, exponent):
436  # source: https://en.wikipedia.org/wiki/Quaternion#Exponential.2C_logarithm.2C_and_power
437  exponent = float(exponent) # Explicitly reject non-real exponents
438  norm = self.norm
439  if norm > 0.0:
440  try:
441  n, theta = self.polar_decomposition
442  except ZeroDivisionError:
443  # quaternion is a real number (no vector or imaginary part)
444  return Quaternion(scalar=self.scalar ** exponent)
445  return (self.norm ** exponent) * Quaternion(scalar=cos(exponent * theta), vector=(n * sin(exponent * theta)))
446  return Quaternion(self)
447 
448  def __ipow__(self, other):
449  return self ** other
450 
451  def __rpow__(self, other):
452  return other ** float(self)
453 
454  # Quaternion Features
455  def _vector_conjugate(self):
456  return np.hstack((self.q[0], -self.q[1:4]))
457 
458  def _sum_of_squares(self):
459  return np.dot(self.q, self.q)
460 
461  @property
462  def conjugate(self):
463  """Quaternion conjugate, encapsulated in a new instance.
464 
465  For a unit quaternion, this is the same as the inverse.
466 
467  Returns:
468  A new Quaternion object clone with its vector part negated
469  """
470  return self.__class__(scalar=self.scalar, vector=-self.vector)
471 
472  @property
473  def inverse(self):
474  """Inverse of the quaternion object, encapsulated in a new instance.
475 
476  For a unit quaternion, this is the inverse rotation, i.e. when combined with the original rotation, will result in the null rotation.
477 
478  Returns:
479  A new Quaternion object representing the inverse of this object
480  """
481  ss = self._sum_of_squares()
482  if ss > 0:
483  return self.__class__(array=(self._vector_conjugate() / ss))
484  else:
485  raise ZeroDivisionError("a zero quaternion (0 + 0i + 0j + 0k) cannot be inverted")
486 
487  @property
488  def norm(self):
489  """L2 norm of the quaternion 4-vector.
490 
491  This should be 1.0 for a unit quaternion (versor)
492  Slow but accurate. If speed is a concern, consider using _fast_normalise() instead
493 
494  Returns:
495  A scalar real number representing the square root of the sum of the squares of the elements of the quaternion.
496  """
497  mag_squared = self._sum_of_squares()
498  return sqrt(mag_squared)
499 
500  @property
501  def magnitude(self):
502  return self.norm
503 
504  def _normalise(self):
505  """Object is guaranteed to be a unit quaternion after calling this
506  operation UNLESS the object is equivalent to Quaternion(0)
507  """
508  if not self.is_unit():
509  n = self.norm
510  if n > 0:
511  self.q = self.q / n
512 
513  def _fast_normalise(self):
514  """Normalise the object to a unit quaternion using a fast approximation method if appropriate.
515 
516  Object is guaranteed to be a quaternion of approximately unit length
517  after calling this operation UNLESS the object is equivalent to Quaternion(0)
518  """
519  if not self.is_unit():
520  mag_squared = np.dot(self.q, self.q)
521  if (mag_squared == 0):
522  return
523  if (abs(1.0 - mag_squared) < 2.107342e-08):
524  mag = ((1.0 + mag_squared) / 2.0) # More efficient. Pade approximation valid if error is small
525  else:
526  mag = sqrt(mag_squared) # Error is too big, take the performance hit to calculate the square root properly
527 
528  self.q = self.q / mag
529 
530  @property
531  def normalised(self):
532  """Get a unit quaternion (versor) copy of this Quaternion object.
533 
534  A unit quaternion has a `norm` of 1.0
535 
536  Returns:
537  A new Quaternion object clone that is guaranteed to be a unit quaternion
538  """
539  q = Quaternion(self)
540  q._normalise()
541  return q
542 
543  @property
544  def polar_unit_vector(self):
545  vector_length = np.linalg.norm(self.vector)
546  if vector_length <= 0.0:
547  raise ZeroDivisionError('Quaternion is pure real and does not have a unique unit vector')
548  return self.vector / vector_length
549 
550  @property
551  def polar_angle(self):
552  return acos(self.scalar / self.norm)
553 
554  @property
556  """
557  Returns the unit vector and angle of a non-scalar quaternion according to the following decomposition
558 
559  q = q.norm() * (e ** (q.polar_unit_vector * q.polar_angle))
560 
561  source: https://en.wikipedia.org/wiki/Polar_decomposition#Quaternion_polar_decomposition
562  """
563  return self.polar_unit_vector, self.polar_angle
564 
565  @property
566  def unit(self):
567  return self.normalised
568 
569  def is_unit(self, tolerance=1e-14):
570  """Determine whether the quaternion is of unit length to within a specified tolerance value.
571 
572  Params:
573  tolerance: [optional] maximum absolute value by which the norm can differ from 1.0 for the object to be considered a unit quaternion. Defaults to `1e-14`.
574 
575  Returns:
576  `True` if the Quaternion object is of unit length to within the specified tolerance value. `False` otherwise.
577  """
578  return abs(1.0 - self._sum_of_squares()) < tolerance # if _sum_of_squares is 1, norm is 1. This saves a call to sqrt()
579 
580  def _q_matrix(self):
581  """Matrix representation of quaternion for multiplication purposes.
582  """
583  return np.array([
584  [self.q[0], -self.q[1], -self.q[2], -self.q[3]],
585  [self.q[1], self.q[0], -self.q[3], self.q[2]],
586  [self.q[2], self.q[3], self.q[0], -self.q[1]],
587  [self.q[3], -self.q[2], self.q[1], self.q[0]]])
588 
589  def _q_bar_matrix(self):
590  """Matrix representation of quaternion for multiplication purposes.
591  """
592  return np.array([
593  [self.q[0], -self.q[1], -self.q[2], -self.q[3]],
594  [self.q[1], self.q[0], self.q[3], -self.q[2]],
595  [self.q[2], -self.q[3], self.q[0], self.q[1]],
596  [self.q[3], self.q[2], -self.q[1], self.q[0]]])
597 
598  def _rotate_quaternion(self, q):
599  """Rotate a quaternion vector using the stored rotation.
600 
601  Params:
602  q: The vector to be rotated, in quaternion form (0 + xi + yj + kz)
603 
604  Returns:
605  A Quaternion object representing the rotated vector in quaternion from (0 + xi + yj + kz)
606  """
607  self._normalise()
608  return self * q * self.conjugate
609 
610  def rotate(self, vector):
611  """Rotate a 3D vector by the rotation stored in the Quaternion object.
612 
613  Params:
614  vector: A 3-vector specified as any ordered sequence of 3 real numbers corresponding to x, y, and z values.
615  Some types that are recognised are: numpy arrays, lists and tuples.
616  A 3-vector can also be represented by a Quaternion object who's scalar part is 0 and vector part is the required 3-vector.
617  Thus it is possible to call `Quaternion.rotate(q)` with another quaternion object as an input.
618 
619  Returns:
620  The rotated vector returned as the same type it was specified at input.
621 
622  Raises:
623  TypeError: if any of the vector elements cannot be converted to a real number.
624  ValueError: if `vector` cannot be interpreted as a 3-vector or a Quaternion object.
625 
626  """
627  if isinstance(vector, Quaternion):
628  return self._rotate_quaternion(vector)
629  q = Quaternion(vector=vector)
630  a = self._rotate_quaternion(q).vector
631  if isinstance(vector, list):
632  l = [x for x in a]
633  return l
634  elif isinstance(vector, tuple):
635  l = [x for x in a]
636  return tuple(l)
637  else:
638  return a
639 
640  @classmethod
641  def exp(cls, q):
642  """Quaternion Exponential.
643 
644  Find the exponential of a quaternion amount.
645 
646  Params:
647  q: the input quaternion/argument as a Quaternion object.
648 
649  Returns:
650  A quaternion amount representing the exp(q). See [Source](https://math.stackexchange.com/questions/1030737/exponential-function-of-quaternion-derivation for more information and mathematical background).
651 
652  Note:
653  The method can compute the exponential of any quaternion.
654  """
655  tolerance = 1e-17
656  v_norm = np.linalg.norm(q.vector)
657  vec = q.vector
658  if v_norm > tolerance:
659  vec = vec / v_norm
660  magnitude = exp(q.scalar)
661  return Quaternion(scalar = magnitude * cos(v_norm), vector = magnitude * sin(v_norm) * vec)
662 
663  @classmethod
664  def log(cls, q):
665  """Quaternion Logarithm.
666 
667  Find the logarithm of a quaternion amount.
668 
669  Params:
670  q: the input quaternion/argument as a Quaternion object.
671 
672  Returns:
673  A quaternion amount representing log(q) := (log(|q|), v/|v|acos(w/|q|)).
674 
675  Note:
676  The method computes the logarithm of general quaternions. See [Source](https://math.stackexchange.com/questions/2552/the-logarithm-of-quaternion/2554#2554) for more details.
677  """
678  v_norm = np.linalg.norm(q.vector)
679  q_norm = q.norm
680  tolerance = 1e-17
681  if q_norm < tolerance:
682  # 0 quaternion - undefined
683  return Quaternion(scalar=-float('inf'), vector=float('nan')*q.vector)
684  if v_norm < tolerance:
685  # real quaternions - no imaginary part
686  return Quaternion(scalar=log(q_norm), vector=[0, 0, 0])
687  vec = q.vector / v_norm
688  return Quaternion(scalar=log(q_norm), vector=acos(q.scalar/q_norm)*vec)
689 
690  @classmethod
691  def exp_map(cls, q, eta):
692  """Quaternion exponential map.
693 
694  Find the exponential map on the Riemannian manifold described
695  by the quaternion space.
696 
697  Params:
698  q: the base point of the exponential map, i.e. a Quaternion object
699  eta: the argument of the exponential map, a tangent vector, i.e. a Quaternion object
700 
701  Returns:
702  A quaternion p such that p is the endpoint of the geodesic starting at q
703  in the direction of eta, having the length equal to the magnitude of eta.
704 
705  Note:
706  The exponential map plays an important role in integrating orientation
707  variations (e.g. angular velocities). This is done by projecting
708  quaternion tangent vectors onto the quaternion manifold.
709  """
710  return q * Quaternion.exp(eta)
711 
712  @classmethod
713  def sym_exp_map(cls, q, eta):
714  """Quaternion symmetrized exponential map.
715 
716  Find the symmetrized exponential map on the quaternion Riemannian
717  manifold.
718 
719  Params:
720  q: the base point as a Quaternion object
721  eta: the tangent vector argument of the exponential map
722  as a Quaternion object
723 
724  Returns:
725  A quaternion p.
726 
727  Note:
728  The symmetrized exponential formulation is akin to the exponential
729  formulation for symmetric positive definite tensors [Source](http://www.academia.edu/7656761/On_the_Averaging_of_Symmetric_Positive-Definite_Tensors)
730  """
731  sqrt_q = q ** 0.5
732  return sqrt_q * Quaternion.exp(eta) * sqrt_q
733 
734  @classmethod
735  def log_map(cls, q, p):
736  """Quaternion logarithm map.
737 
738  Find the logarithm map on the quaternion Riemannian manifold.
739 
740  Params:
741  q: the base point at which the logarithm is computed, i.e.
742  a Quaternion object
743  p: the argument of the quaternion map, a Quaternion object
744 
745  Returns:
746  A tangent vector having the length and direction given by the
747  geodesic joining q and p.
748  """
749  return Quaternion.log(q.inverse * p)
750 
751  @classmethod
752  def sym_log_map(cls, q, p):
753  """Quaternion symmetrized logarithm map.
754 
755  Find the symmetrized logarithm map on the quaternion Riemannian manifold.
756 
757  Params:
758  q: the base point at which the logarithm is computed, i.e.
759  a Quaternion object
760  p: the argument of the quaternion map, a Quaternion object
761 
762  Returns:
763  A tangent vector corresponding to the symmetrized geodesic curve formulation.
764 
765  Note:
766  Information on the symmetrized formulations given in [Source](https://www.researchgate.net/publication/267191489_Riemannian_L_p_Averaging_on_Lie_Group_of_Nonzero_Quaternions).
767  """
768  inv_sqrt_q = (q ** (-0.5))
769  return Quaternion.log(inv_sqrt_q * p * inv_sqrt_q)
770 
771  @classmethod
772  def absolute_distance(cls, q0, q1):
773  """Quaternion absolute distance.
774 
775  Find the distance between two quaternions accounting for the sign ambiguity.
776 
777  Params:
778  q0: the first quaternion
779  q1: the second quaternion
780 
781  Returns:
782  A positive scalar corresponding to the chord of the shortest path/arc that
783  connects q0 to q1.
784 
785  Note:
786  This function does not measure the distance on the hypersphere, but
787  it takes into account the fact that q and -q encode the same rotation.
788  It is thus a good indicator for rotation similarities.
789  """
790  q0_minus_q1 = q0 - q1
791  q0_plus_q1 = q0 + q1
792  d_minus = q0_minus_q1.norm
793  d_plus = q0_plus_q1.norm
794  if d_minus < d_plus:
795  return d_minus
796  else:
797  return d_plus
798 
799  @classmethod
800  def distance(cls, q0, q1):
801  """Quaternion intrinsic distance.
802 
803  Find the intrinsic geodesic distance between q0 and q1.
804 
805  Params:
806  q0: the first quaternion
807  q1: the second quaternion
808 
809  Returns:
810  A positive amount corresponding to the length of the geodesic arc
811  connecting q0 to q1.
812 
813  Note:
814  Although the q0^(-1)*q1 != q1^(-1)*q0, the length of the path joining
815  them is given by the logarithm of those product quaternions, the norm
816  of which is the same.
817  """
818  q = Quaternion.log_map(q0, q1)
819  return q.norm
820 
821  @classmethod
822  def sym_distance(cls, q0, q1):
823  """Quaternion symmetrized distance.
824 
825  Find the intrinsic symmetrized geodesic distance between q0 and q1.
826 
827  Params:
828  q0: the first quaternion
829  q1: the second quaternion
830 
831  Returns:
832  A positive amount corresponding to the length of the symmetrized
833  geodesic curve connecting q0 to q1.
834 
835  Note:
836  This formulation is more numerically stable when performing
837  iterative gradient descent on the Riemannian quaternion manifold.
838  However, the distance between q and -q is equal to pi, rendering this
839  formulation not useful for measuring rotation similarities when the
840  samples are spread over a "solid" angle of more than pi/2 radians
841  (the spread refers to quaternions as point samples on the unit hypersphere).
842  """
843  q = Quaternion.sym_log_map(q0, q1)
844  return q.norm
845 
846  @classmethod
847  def slerp(cls, q0, q1, amount=0.5):
848  """Spherical Linear Interpolation between quaternions.
849  Implemented as described in https://en.wikipedia.org/wiki/Slerp
850 
851  Find a valid quaternion rotation at a specified distance along the
852  minor arc of a great circle passing through any two existing quaternion
853  endpoints lying on the unit radius hypersphere.
854 
855  This is a class method and is called as a method of the class itself rather than on a particular instance.
856 
857  Params:
858  q0: first endpoint rotation as a Quaternion object
859  q1: second endpoint rotation as a Quaternion object
860  amount: interpolation parameter between 0 and 1. This describes the linear placement position of
861  the result along the arc between endpoints; 0 being at `q0` and 1 being at `q1`.
862  Defaults to the midpoint (0.5).
863 
864  Returns:
865  A new Quaternion object representing the interpolated rotation. This is guaranteed to be a unit quaternion.
866 
867  Note:
868  This feature only makes sense when interpolating between unit quaternions (those lying on the unit radius hypersphere).
869  Calling this method will implicitly normalise the endpoints to unit quaternions if they are not already unit length.
870  """
871  # Ensure quaternion inputs are unit quaternions and 0 <= amount <=1
872  q0._fast_normalise()
873  q1._fast_normalise()
874  amount = np.clip(amount, 0, 1)
875 
876  dot = np.dot(q0.q, q1.q)
877 
878  # If the dot product is negative, slerp won't take the shorter path.
879  # Note that v1 and -v1 are equivalent when the negation is applied to all four components.
880  # Fix by reversing one quaternion
881  if dot < 0.0:
882  q0.q = -q0.q
883  dot = -dot
884 
885  # sin_theta_0 can not be zero
886  if dot > 0.9995:
887  qr = Quaternion(q0.q + amount * (q1.q - q0.q))
888  qr._fast_normalise()
889  return qr
890 
891  theta_0 = np.arccos(dot) # Since dot is in range [0, 0.9995], np.arccos() is safe
892  sin_theta_0 = np.sin(theta_0)
893 
894  theta = theta_0 * amount
895  sin_theta = np.sin(theta)
896 
897  s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
898  s1 = sin_theta / sin_theta_0
899  qr = Quaternion((s0 * q0.q) + (s1 * q1.q))
900  qr._fast_normalise()
901  return qr
902 
903  @classmethod
904  def intermediates(cls, q0, q1, n, include_endpoints=False):
905  """Generator method to get an iterable sequence of `n` evenly spaced quaternion
906  rotations between any two existing quaternion endpoints lying on the unit
907  radius hypersphere.
908 
909  This is a convenience function that is based on `Quaternion.slerp()` as defined above.
910 
911  This is a class method and is called as a method of the class itself rather than on a particular instance.
912 
913  Params:
914  q_start: initial endpoint rotation as a Quaternion object
915  q_end: final endpoint rotation as a Quaternion object
916  n: number of intermediate quaternion objects to include within the interval
917  include_endpoints: [optional] if set to `True`, the sequence of intermediates
918  will be 'bookended' by `q_start` and `q_end`, resulting in a sequence length of `n + 2`.
919  If set to `False`, endpoints are not included. Defaults to `False`.
920 
921  Yields:
922  A generator object iterating over a sequence of intermediate quaternion objects.
923 
924  Note:
925  This feature only makes sense when interpolating between unit quaternions (those lying on the unit radius hypersphere).
926  Calling this method will implicitly normalise the endpoints to unit quaternions if they are not already unit length.
927  """
928  step_size = 1.0 / (n + 1)
929  if include_endpoints:
930  steps = [i * step_size for i in range(0, n + 2)]
931  else:
932  steps = [i * step_size for i in range(1, n + 1)]
933  for step in steps:
934  yield cls.slerp(q0, q1, step)
935 
936  def derivative(self, rate):
937  """Get the instantaneous quaternion derivative representing a quaternion rotating at a 3D rate vector `rate`
938 
939  Params:
940  rate: numpy 3-array (or array-like) describing rotation rates about the global x, y and z axes respectively.
941 
942  Returns:
943  A unit quaternion describing the rotation rate
944  """
945  rate = self._validate_number_sequence(rate, 3)
946  return 0.5 * self * Quaternion(vector=rate)
947 
948  def integrate(self, rate, timestep):
949  """Advance a time varying quaternion to its value at a time `timestep` in the future.
950 
951  The Quaternion object will be modified to its future value.
952  It is guaranteed to remain a unit quaternion.
953 
954  Params:
955 
956  rate: numpy 3-array (or array-like) describing rotation rates about the
957  global x, y and z axes respectively.
958  timestep: interval over which to integrate into the future.
959  Assuming *now* is `T=0`, the integration occurs over the interval
960  `T=0` to `T=timestep`. Smaller intervals are more accurate when
961  `rate` changes over time.
962 
963  Note:
964  The solution is closed form given the assumption that `rate` is constant
965  over the interval of length `timestep`.
966  """
967  self._fast_normalise()
968  rate = self._validate_number_sequence(rate, 3)
969 
970  rotation_vector = rate * timestep
971  rotation_norm = np.linalg.norm(rotation_vector)
972  if rotation_norm > 0:
973  axis = rotation_vector / rotation_norm
974  angle = rotation_norm
975  q2 = Quaternion(axis=axis, angle=angle)
976  self.q = (self * q2).q
977  self._fast_normalise()
978 
979 
980  @property
981  def rotation_matrix(self):
982  """Get the 3x3 rotation matrix equivalent of the quaternion rotation.
983 
984  Returns:
985  A 3x3 orthogonal rotation matrix as a 3x3 Numpy array
986 
987  Note:
988  This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
989 
990  """
991  self._normalise()
992  product_matrix = np.dot(self._q_matrix(), self._q_bar_matrix().conj().transpose())
993  return product_matrix[1:][:, 1:]
994 
995  @property
997  """Get the 4x4 homogeneous transformation matrix equivalent of the quaternion rotation.
998 
999  Returns:
1000  A 4x4 homogeneous transformation matrix as a 4x4 Numpy array
1001 
1002  Note:
1003  This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
1004  """
1005  t = np.array([[0.0], [0.0], [0.0]])
1006  Rt = np.hstack([self.rotation_matrix, t])
1007  return np.vstack([Rt, np.array([0.0, 0.0, 0.0, 1.0])])
1008 
1009  @property
1010  def yaw_pitch_roll(self):
1011  """Get the equivalent yaw-pitch-roll angles aka. intrinsic Tait-Bryan angles following the z-y'-x'' convention
1012 
1013  Returns:
1014  yaw: rotation angle around the z-axis in radians, in the range `[-pi, pi]`
1015  pitch: rotation angle around the y'-axis in radians, in the range `[-pi/2, -pi/2]`
1016  roll: rotation angle around the x''-axis in radians, in the range `[-pi, pi]`
1017 
1018  The resulting rotation_matrix would be R = R_x(roll) R_y(pitch) R_z(yaw)
1019 
1020  Note:
1021  This feature only makes sense when referring to a unit quaternion. Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
1022  """
1023 
1024  self._normalise()
1025  yaw = np.arctan2(2 * (self.q[0] * self.q[3] - self.q[1] * self.q[2]),
1026  1 - 2 * (self.q[2] ** 2 + self.q[3] ** 2))
1027  pitch = np.arcsin(2 * (self.q[0] * self.q[2] + self.q[3] * self.q[1]))
1028  roll = np.arctan2(2 * (self.q[0] * self.q[1] - self.q[2] * self.q[3]),
1029  1 - 2 * (self.q[1] ** 2 + self.q[2] ** 2))
1030 
1031  return yaw, pitch, roll
1032 
1033  def _wrap_angle(self, theta):
1034  """Helper method: Wrap any angle to lie between -pi and pi
1035 
1036  Odd multiples of pi are wrapped to +pi (as opposed to -pi)
1037  """
1038  result = ((theta + pi) % (2 * pi)) - pi
1039  if result == -pi:
1040  result = pi
1041  return result
1042 
1043  def get_axis(self, undefined=np.zeros(3)):
1044  """Get the axis or vector about which the quaternion rotation occurs
1045 
1046  For a null rotation (a purely real quaternion), the rotation angle will
1047  always be `0`, but the rotation axis is undefined.
1048  It is by default assumed to be `[0, 0, 0]`.
1049 
1050  Params:
1051  undefined: [optional] specify the axis vector that should define a null rotation.
1052  This is geometrically meaningless, and could be any of an infinite set of vectors,
1053  but can be specified if the default (`[0, 0, 0]`) causes undesired behaviour.
1054 
1055  Returns:
1056  A Numpy unit 3-vector describing the Quaternion object's axis of rotation.
1057 
1058  Note:
1059  This feature only makes sense when referring to a unit quaternion.
1060  Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
1061  """
1062  tolerance = 1e-17
1063  self._normalise()
1064  norm = np.linalg.norm(self.vector)
1065  if norm < tolerance:
1066  # Here there are an infinite set of possible axes, use what has been specified as an undefined axis.
1067  return undefined
1068  else:
1069  return self.vector / norm
1070 
1071  @property
1072  def axis(self):
1073  return self.get_axis()
1074 
1075  @property
1076  def angle(self):
1077  """Get the angle (in radians) describing the magnitude of the quaternion rotation about its rotation axis.
1078 
1079  This is guaranteed to be within the range (-pi:pi) with the direction of
1080  rotation indicated by the sign.
1081 
1082  When a particular rotation describes a 180 degree rotation about an arbitrary
1083  axis vector `v`, the conversion to axis / angle representation may jump
1084  discontinuously between all permutations of `(-pi, pi)` and `(-v, v)`,
1085  each being geometrically equivalent (see Note in documentation).
1086 
1087  Returns:
1088  A real number in the range (-pi:pi) describing the angle of rotation
1089  in radians about a Quaternion object's axis of rotation.
1090 
1091  Note:
1092  This feature only makes sense when referring to a unit quaternion.
1093  Calling this method will implicitly normalise the Quaternion object to a unit quaternion if it is not already one.
1094  """
1095  self._normalise()
1096  norm = np.linalg.norm(self.vector)
1097  return self._wrap_angle(2.0 * atan2(norm, self.scalar))
1098 
1099  @property
1100  def degrees(self):
1101  return self.to_degrees(self.angle)
1102 
1103  @property
1104  def radians(self):
1105  return self.angle
1106 
1107  @property
1108  def scalar(self):
1109  """ Return the real or scalar component of the quaternion object.
1110 
1111  Returns:
1112  A real number i.e. float
1113  """
1114  return self.q[0]
1115 
1116  @property
1117  def vector(self):
1118  """ Return the imaginary or vector component of the quaternion object.
1119 
1120  Returns:
1121  A numpy 3-array of floats. NOT guaranteed to be a unit vector
1122  """
1123  return self.q[1:4]
1124 
1125  @vector.setter
1126  def vector(self, v):
1127  if len(v) != 3:
1128  raise AttributeError("Expected vector component to be of length 3 but received length {} instead!"
1129  .format(len(v)))
1130  self.q[1:4] = v
1131 
1132  @property
1133  def real(self):
1134  return self.scalar
1135 
1136  @property
1137  def imaginary(self):
1138  return self.vector
1139 
1140  @property
1141  def w(self):
1142  return self.q[0]
1143 
1144  @property
1145  def x(self):
1146  return self.q[1]
1147 
1148  @property
1149  def y(self):
1150  return self.q[2]
1151 
1152  @property
1153  def z(self):
1154  return self.q[3]
1155 
1156  @property
1157  def elements(self):
1158  """ Return all the elements of the quaternion object.
1159 
1160  Returns:
1161  A numpy 4-array of floats. NOT guaranteed to be a unit vector
1162  """
1163  return self.q
1164 
1165  def __getitem__(self, index):
1166  index = int(index)
1167  return self.q[index]
1168 
1169  def __setitem__(self, index, value):
1170  index = int(index)
1171  self.q[index] = float(value)
1172 
1173  def __copy__(self):
1174  result = self.__class__(self.q)
1175  return result
1176 
1177  def __deepcopy__(self, memo):
1178  result = self.__class__(deepcopy(self.q, memo))
1179  memo[id(self)] = result
1180  return result
1181 
1182  @staticmethod
1183  def to_degrees(angle_rad):
1184  if angle_rad is not None:
1185  return float(angle_rad) / pi * 180.0
1186 
1187  @staticmethod
1188  def to_radians(angle_deg):
1189  if angle_deg is not None:
1190  return float(angle_deg) / 180.0 * pi
def _validate_number_sequence(self, seq, n)
def _from_matrix(cls, matrix, rtol=1e-05, atol=1e-08)
def integrate(self, rate, timestep)
def __setitem__(self, index, value)
def intermediates(cls, q0, q1, n, include_endpoints=False)
def __init__(self, args, kwargs)
Definition: pyquaternion.py:53
def _from_axis_angle(cls, axis, angle)
def is_unit(self, tolerance=1e-14)
def get_axis(self, undefined=np.zeros(3))
def slerp(cls, q0, q1, amount=0.5)


pyquaternion
Author(s): achille
autogenerated on Sun Mar 15 2020 03:13:33