quaternion.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 
00004 """
00005 Quaternion implementation for use in pymavlink
00006 """
00007 
00008 from __future__ import absolute_import, division, print_function
00009 import numpy as np
00010 from .rotmat import Vector3, Matrix3
00011 
00012 __author__ = "Thomas Gubler"
00013 __copyright__ = "Copyright (C) 2014 Thomas Gubler"
00014 __license__ = "GNU Lesser General Public License v3"
00015 __email__ = "thomasgubler@gmail.com"
00016 
00017 
00018 class QuaternionBase(object):
00019 
00020     """
00021     Quaternion class, this is the version which supports numpy arrays
00022     If you need support for Matrix3 look at the Quaternion class
00023 
00024     Usage:
00025         >>> from quaternion import QuaternionBase
00026         >>> import numpy as np
00027         >>> q = QuaternionBase([np.radians(20), np.radians(20), np.radians(20)])
00028         >>> print(q)
00029         [ 0.9603483   0.13871646  0.19810763  0.13871646]
00030         >>> print(q.dcm)
00031         [[ 0.88302222 -0.21147065  0.41898917]
00032          [ 0.3213938   0.92303098 -0.21147065]
00033          [-0.34202014  0.3213938   0.88302222]]
00034         >>> q = QuaternionBase([1, 0, 0, 0])
00035         >>> print(q.euler)
00036         [ 0. -0.  0.]
00037         >>> m = [[1, 0, 0], [0, 0, -1], [0, 1, 0]]
00038         >>> q = QuaternionBase(m)
00039         >>> vector = [0, 1, 0]
00040         >>> vector2 = q.transform(vector)
00041     """
00042 
00043     def __init__(self, attitude=[1, 0, 0, 0]):
00044         """
00045         Construct a quaternion from an attitude
00046 
00047         :param attitude: another QuaternionBase,
00048             3 element list [roll, pitch, yaw],
00049             4 element list [w, x, y ,z], DCM (3x3 array)
00050         """
00051         if isinstance(attitude, QuaternionBase):
00052             self.q = attitude.q
00053         elif np.array(attitude).shape == (3, 3):
00054             self.dcm = attitude
00055         elif len(attitude) == 4:
00056             self.q = attitude
00057         elif len(attitude) == 3:
00058             self.euler = attitude
00059         else:
00060             raise TypeError("attitude is not valid")
00061 
00062     @property
00063     def q(self):
00064         """
00065         Get the quaternion
00066         :returns: array containing the quaternion elements
00067         """
00068         if self._q is None:
00069             if self._euler is not None:
00070                 # get q from euler
00071                 self._q = self._euler_to_q(self.euler)
00072             elif self._dcm is not None:
00073                 # get q from DCM
00074                 self._q = self._dcm_to_q(self.dcm)
00075         return self._q
00076 
00077     def __getitem__(self, index):
00078         """Returns the quaternion entry at index"""
00079         return self.q[index]
00080 
00081     @q.setter
00082     def q(self, q):
00083         """
00084         Set the quaternion
00085         :param q: list or array of quaternion values [w, x, y, z]
00086         """
00087         self._q = np.array(q)
00088 
00089         # mark other representations as outdated, will get generated on next
00090         # read
00091         self._euler = None
00092         self._dcm = None
00093 
00094     @property
00095     def euler(self):
00096         """
00097         Get the euler angles.
00098         The convention is Tait-Bryan (ZY'X'')
00099 
00100         :returns: array containing the euler angles [roll, pitch, yaw]
00101         """
00102         if self._euler is None:
00103             if self._q is not None:
00104                 # try to get euler angles from q via DCM
00105                 self._dcm = self._q_to_dcm(self.q)
00106                 self._euler = self._dcm_to_euler(self.dcm)
00107             elif self._dcm is not None:
00108                 # get euler angles from DCM
00109                 self._euler = self._dcm_to_euler(self.dcm)
00110         return self._euler
00111 
00112     @euler.setter
00113     def euler(self, euler):
00114         """
00115         Set the euler angles
00116         :param euler: list or array of the euler angles [roll, pitch, yaw]
00117 
00118         """
00119         assert(len(euler) == 3)
00120         self._euler = np.array(euler)
00121 
00122         # mark other representations as outdated, will get generated on next
00123         # read
00124         self._q = None
00125         self._dcm = None
00126 
00127     @property
00128     def dcm(self):
00129         """
00130         Get the DCM
00131 
00132         :returns: 3x3 array
00133         """
00134         if self._dcm is None:
00135             if self._q is not None:
00136                 # try to get dcm from q
00137                 self._dcm = self._q_to_dcm(self.q)
00138             elif self._euler is not None:
00139                 # try to get get dcm from euler
00140                 self._dcm = self._euler_to_dcm(self._euler)
00141         return self._dcm
00142 
00143     @dcm.setter
00144     def dcm(self, dcm):
00145         """
00146         Set the DCM
00147         :param dcm: 3x3 array
00148 
00149         """
00150         assert(len(dcm) == 3)
00151         for sub in dcm:
00152             assert(len(sub) == 3)
00153 
00154         self._dcm = np.array(dcm)
00155 
00156         # mark other representations as outdated, will get generated on next
00157         # read
00158         self._q = None
00159         self._euler = None
00160 
00161     def transform(self, v):
00162         """
00163         Calculates the vector transformed by this quaternion
00164         :param v: array with len 3 to be transformed
00165         :returns: transformed vector
00166         """
00167         assert(len(v) == 3)
00168         assert(np.allclose(self.norm, 1))
00169         # perform transformation t = q * [0, v] * q^-1 but avoid multiplication
00170         # because terms cancel out
00171         q0 = self.q[0]
00172         qi = self.q[1:4]
00173         ui = np.array(v)
00174         a = q0 * ui + np.cross(qi, ui)
00175         t = np.dot(qi, ui) * qi + q0 * a - np.cross(a, qi)
00176         return t
00177 
00178     @property
00179     def norm(self):
00180         """
00181         Returns norm of quaternion
00182 
00183         :returns: norm (scalar)
00184         """
00185         return QuaternionBase.norm_array(self.q)
00186 
00187     def normalize(self):
00188         """Normalizes the quaternion"""
00189         self.q = QuaternionBase.normalize_array(self.q)
00190 
00191     @property
00192     def inversed(self):
00193         """
00194         Get inversed quaternion
00195 
00196         :returns: inversed quaternion
00197         """
00198         q_inv = self._q_inversed(self.q)
00199         return QuaternionBase(q_inv)
00200 
00201     def __eq__(self, other):
00202         """
00203         Equality test (same orientation, not necessarily same rotation)
00204 
00205         :param other: a QuaternionBase
00206         :returns: true if the quaternions are equal
00207         """
00208         if isinstance(other, QuaternionBase):
00209             return abs(self.q.dot(other.q)) > 1 - np.finfo(float).eps
00210         return NotImplemented
00211 
00212     def close(self, other):
00213         """
00214         Equality test with tolerance
00215         (same orientation, not necessarily same rotation)
00216 
00217 
00218         :param other: a QuaternionBase
00219         :returns: true if the quaternions are almost equal
00220         """
00221         if isinstance(other, QuaternionBase):
00222             return np.allclose(self.q, other.q) or np.allclose(self.q, -other.q)
00223         return NotImplemented
00224 
00225     def __mul__(self, other):
00226         """
00227         :param other: QuaternionBase
00228         :returns: multiplaction of this Quaternion with other
00229         """
00230         if isinstance(other, QuaternionBase):
00231             o = other.q
00232         elif len(other) == 4:
00233             o = other
00234         else:
00235             return NotImplemented
00236 
00237         return QuaternionBase(self._mul_array(self.q, o))
00238 
00239     def __truediv__(self, other):
00240         """
00241         :param other: QuaternionBase
00242         :returns: division of this Quaternion with other
00243         """
00244         if isinstance(other, QuaternionBase):
00245             o = other
00246         elif len(other) == 4:
00247             o = QuaternionBase(other)
00248         else:
00249             return NotImplemented
00250         return self * o.inversed
00251 
00252     @staticmethod
00253     def normalize_array(q):
00254         """
00255         Normalizes the list with len 4 so that it can be used as quaternion
00256         :param q: array of len 4
00257         :returns: normalized array
00258         """
00259         assert(len(q) == 4)
00260         q = np.array(q)
00261         n = QuaternionBase.norm_array(q)
00262         return q / n
00263 
00264     @staticmethod
00265     def norm_array(q):
00266         """
00267         Calculate quaternion norm on array q
00268         :param quaternion: array of len 4
00269         :returns: norm (scalar)
00270         """
00271         assert(len(q) == 4)
00272         return np.sqrt(np.dot(q, q))
00273 
00274     def _mul_array(self, p, q):
00275         """
00276         Performs multiplication of the 2 quaterniona arrays p and q
00277         :param p: array of len 4
00278         :param q: array of len 4
00279         :returns: array of len, result of p * q (with p, q quaternions)
00280         """
00281         assert(len(q) == len(p) == 4)
00282         p0 = p[0]
00283         pi = p[1:4]
00284         q0 = q[0]
00285         qi = q[1:4]
00286 
00287         res = np.zeros(4)
00288         res[0] = p0 * q0 - np.dot(pi, qi)
00289         res[1:4] = p0 * qi + q0 * pi + np.cross(pi, qi)
00290 
00291         return res
00292 
00293     def _euler_to_q(self, euler):
00294         """
00295         Create q array from euler angles
00296         :param euler: array [roll, pitch, yaw] in rad
00297         :returns: array q which represents a quaternion [w, x, y, z]
00298         """
00299         assert(len(euler) == 3)
00300         phi = euler[0]
00301         theta = euler[1]
00302         psi = euler[2]
00303         c_phi_2 = np.cos(phi / 2)
00304         s_phi_2 = np.sin(phi / 2)
00305         c_theta_2 = np.cos(theta / 2)
00306         s_theta_2 = np.sin(theta / 2)
00307         c_psi_2 = np.cos(psi / 2)
00308         s_psi_2 = np.sin(psi / 2)
00309         q = np.zeros(4)
00310         q[0] = (c_phi_2 * c_theta_2 * c_psi_2 +
00311                 s_phi_2 * s_theta_2 * s_psi_2)
00312         q[1] = (s_phi_2 * c_theta_2 * c_psi_2 -
00313                 c_phi_2 * s_theta_2 * s_psi_2)
00314         q[2] = (c_phi_2 * s_theta_2 * c_psi_2 +
00315                 s_phi_2 * c_theta_2 * s_psi_2)
00316         q[3] = (c_phi_2 * c_theta_2 * s_psi_2 -
00317                 s_phi_2 * s_theta_2 * c_psi_2)
00318         return q
00319 
00320     def _q_to_dcm(self, q):
00321         """
00322         Create DCM from q
00323         :param q: array q which represents a quaternion [w, x, y, z]
00324         :returns: 3x3 dcm array
00325         """
00326         assert(len(q) == 4)
00327         assert(np.allclose(QuaternionBase.norm_array(q), 1))
00328         dcm = np.zeros([3, 3])
00329         a = q[0]
00330         b = q[1]
00331         c = q[2]
00332         d = q[3]
00333         a_sq = a * a
00334         b_sq = b * b
00335         c_sq = c * c
00336         d_sq = d * d
00337         dcm[0][0] = a_sq + b_sq - c_sq - d_sq
00338         dcm[0][1] = 2 * (b * c - a * d)
00339         dcm[0][2] = 2 * (a * c + b * d)
00340         dcm[1][0] = 2 * (b * c + a * d)
00341         dcm[1][1] = a_sq - b_sq + c_sq - d_sq
00342         dcm[1][2] = 2 * (c * d - a * b)
00343         dcm[2][0] = 2 * (b * d - a * c)
00344         dcm[2][1] = 2 * (a * b + c * d)
00345         dcm[2][2] = a_sq - b_sq - c_sq + d_sq
00346         return dcm
00347 
00348     def _dcm_to_q(self, dcm):
00349         """
00350         Create q from dcm
00351         Reference:
00352             - Shoemake, Quaternions,
00353             http://www.cs.ucr.edu/~vbz/resources/quatut.pdf
00354 
00355         :param dcm: 3x3 dcm array
00356         returns: quaternion array
00357         """
00358         assert(dcm.shape == (3, 3))
00359         q = np.zeros(4)
00360 
00361         tr = np.trace(dcm)
00362         if tr > 0:
00363             s = np.sqrt(tr + 1.0)
00364             q[0] = s * 0.5
00365             s = 0.5 / s
00366             q[1] = (dcm[2][1] - dcm[1][2]) * s
00367             q[2] = (dcm[0][2] - dcm[2][0]) * s
00368             q[3] = (dcm[1][0] - dcm[0][1]) * s
00369         else:
00370             dcm_i = np.argmax(np.diag(dcm))
00371             dcm_j = (dcm_i + 1) % 3
00372             dcm_k = (dcm_i + 2) % 3
00373 
00374             s = np.sqrt((dcm[dcm_i][dcm_i] - dcm[dcm_j][dcm_j] -
00375                          dcm[dcm_k][dcm_k]) + 1.0)
00376             q[dcm_i + 1] = s * 0.5
00377             s = 0.5 / s
00378             q[dcm_j + 1] = (dcm[dcm_i][dcm_j] + dcm[dcm_j][dcm_i]) * s
00379             q[dcm_k + 1] = (dcm[dcm_k][dcm_i] + dcm[dcm_i][dcm_k]) * s
00380             q[0] = (dcm[dcm_k][dcm_j] - dcm[dcm_j][dcm_k]) * s
00381 
00382         return q
00383 
00384     def _euler_to_dcm(self, euler):
00385         """
00386         Create DCM from euler angles
00387         :param euler: array [roll, pitch, yaw] in rad
00388         :returns: 3x3 dcm array
00389         """
00390         assert(len(euler) == 3)
00391         phi = euler[0]
00392         theta = euler[1]
00393         psi = euler[2]
00394         dcm = np.zeros([3, 3])
00395         c_phi = np.cos(phi)
00396         s_phi = np.sin(phi)
00397         c_theta = np.cos(theta)
00398         s_theta = np.sin(theta)
00399         c_psi = np.cos(psi)
00400         s_psi = np.sin(psi)
00401 
00402         dcm[0][0] = c_theta * c_psi
00403         dcm[0][1] = -c_phi * s_psi + s_phi * s_theta * c_psi
00404         dcm[0][2] = s_phi * s_psi + c_phi * s_theta * c_psi
00405 
00406         dcm[1][0] = c_theta * s_psi
00407         dcm[1][1] = c_phi * c_psi + s_phi * s_theta * s_psi
00408         dcm[1][2] = -s_phi * c_psi + c_phi * s_theta * s_psi
00409 
00410         dcm[2][0] = -s_theta
00411         dcm[2][1] = s_phi * c_theta
00412         dcm[2][2] = c_phi * c_theta
00413         return dcm
00414 
00415     def _dcm_to_euler(self, dcm):
00416         """
00417         Create DCM from euler angles
00418         :param dcm: 3x3 dcm array
00419         :returns: array [roll, pitch, yaw] in rad
00420         """
00421         assert(dcm.shape == (3, 3))
00422         theta = np.arcsin(min(1, max(-1, -dcm[2][0])))
00423 
00424         if abs(theta - np.pi/2) < 1.0e-3:
00425             phi = 0.0
00426             psi = (np.arctan2(dcm[1][2] - dcm[0][1],
00427                               dcm[0][2] + dcm[1][1]) + phi)
00428         elif abs(theta + np.pi/2) < 1.0e-3:
00429             phi = 0.0
00430             psi = np.arctan2(dcm[1][2] - dcm[0][1],
00431                              dcm[0][2] + dcm[1][1] - phi)
00432         else:
00433             phi = np.arctan2(dcm[2][1], dcm[2][2])
00434             psi = np.arctan2(dcm[1][0], dcm[0][0])
00435 
00436         return np.array([phi, theta, psi])
00437 
00438     def _q_inversed(self, q):
00439         """
00440         Returns inversed quaternion q
00441         :param q: array q which represents a quaternion [w, x, y, z]
00442         :returns: inversed array q which is a quaternion [w, x, y ,z]
00443         """
00444         assert(len(q) == 4)
00445         return np.hstack([q[0], -q[1:4]])
00446 
00447     def __str__(self):
00448         """String of quaternion values"""
00449         return str(self.q)
00450 
00451 
00452 class Quaternion(QuaternionBase):
00453 
00454     """
00455     Quaternion class that supports pymavlink's Vector3 and Matrix3
00456 
00457     Usage:
00458         >>> from quaternion import Quaternion
00459         >>> from rotmat import Vector3, Matrix3
00460         >>> m = Matrix3()
00461         >>> m.from_euler(45, 0, 0)
00462         >>> print(m)
00463         Matrix3((1.00, 0.00, 0.00), (0.00, 0.53, -0.85), (-0.00, 0.85, 0.53))
00464         >>> q = Quaternion(m)
00465         >>> print(q)
00466         [ 0.87330464  0.48717451  0.          0.        ]
00467         >>> print(q.dcm)
00468         Matrix3((1.00, 0.00, 0.00), (0.00, 0.53, -0.85), (-0.00, 0.85, 0.53))
00469         >>> v = Vector3(0, 1, 0)
00470         >>> v2 = q.transform(v)
00471         >>> print(v2)
00472         Vector3(0.00, 0.53, 0.85)
00473     """
00474 
00475     def __init__(self, attitude):
00476         """
00477         Construct a quaternion from an attitude
00478 
00479         :param attitude: another Quaternion, QuaternionBase,
00480             3 element list [roll, pitch, yaw],
00481             4 element list [w, x, y ,z], DCM (3x3 array or Matrix3)
00482         """
00483         if isinstance(attitude, Quaternion):
00484             self.q = attitude.q
00485         if isinstance(attitude, Matrix3):
00486             self.dcm = attitude
00487         elif np.array(attitude).shape == (3, 3):
00488             # convert dcm array to Matrix3
00489             self.dcm = self._dcm_array_to_matrix3(attitude)
00490         elif isinstance(attitude, Vector3):
00491             # provided euler angles
00492             euler = [attitude.x, attitude.y, attitude.z]
00493             super(Quaternion, self).__init__(euler)
00494         else:
00495             super(Quaternion, self).__init__(attitude)
00496 
00497     @property
00498     def dcm(self):
00499         """
00500         Get the DCM
00501 
00502         :returns: Matrix3
00503         """
00504         if self._dcm is None:
00505             if self._q is not None:
00506                 # try to get dcm from q
00507                 self._dcm = self._q_to_dcm(self.q)
00508             elif self._euler is not None:
00509                 # try to get get dcm from euler
00510                 self._dcm = self._euler_to_dcm(self._euler)
00511         return self._dcm
00512 
00513     @dcm.setter
00514     def dcm(self, dcm):
00515         """
00516         Set the DCM
00517         :param dcm: Matrix3
00518 
00519         """
00520         assert(isinstance(dcm, Matrix3))
00521         self._dcm = dcm.copy()
00522 
00523         # mark other representations as outdated, will get generated on next
00524         # read
00525         self._q = None
00526         self._euler = None
00527 
00528     @property
00529     def inversed(self):
00530         """
00531         Get inversed quaternion
00532 
00533         :returns: inversed quaternion
00534         """
00535         return Quaternion(super(Quaternion, self).inversed)
00536 
00537     def transform(self, v3):
00538         """
00539         Calculates the vector transformed by this quaternion
00540         :param v3: Vector3 to be transformed
00541         :returns: transformed vector
00542         """
00543         if isinstance(v3, Vector3):
00544             t = super(Quaternion, self).transform([v3.x, v3.y, v3.z])
00545             return Vector3(t[0], t[1], t[2])
00546         elif len(v3) == 3:
00547             return super(Quaternion, self).transform(v3)
00548         else:
00549             raise TypeError("param v3 is not a vector type")
00550 
00551     def _dcm_array_to_matrix3(self, dcm):
00552         """
00553         Converts dcm array into Matrix3
00554         :param dcm: 3x3 dcm array
00555         :returns: Matrix3
00556         """
00557         assert(dcm.shape == (3, 3))
00558         a = Vector3(dcm[0][0], dcm[0][1], dcm[0][2])
00559         b = Vector3(dcm[1][0], dcm[1][1], dcm[1][2])
00560         c = Vector3(dcm[2][0], dcm[2][1], dcm[2][2])
00561         return Matrix3(a, b, c)
00562 
00563     def _matrix3_to_dcm_array(self, m):
00564         """
00565         Converts Matrix3 in an array
00566         :param m: Matrix3
00567         :returns: 3x3 array
00568         """
00569         assert(isinstance(m, Matrix3))
00570         return np.array([[m.a.x, m.a.y, m.a.z],
00571                          [m.b.x, m.b.y, m.b.z],
00572                          [m.c.x, m.c.y, m.c.z]])
00573 
00574     def _q_to_dcm(self, q):
00575         """
00576         Create DCM (Matrix3) from q
00577         :param q: array q which represents a quaternion [w, x, y, z]
00578         :returns: Matrix3
00579         """
00580         assert(len(q) == 4)
00581         arr = super(Quaternion, self)._q_to_dcm(q)
00582         return self._dcm_array_to_matrix3(arr)
00583 
00584     def _dcm_to_q(self, dcm):
00585         """
00586         Create q from dcm (Matrix3)
00587         :param dcm: Matrix3
00588         :returns: array q which represents a quaternion [w, x, y, z]
00589         """
00590         assert(isinstance(dcm, Matrix3))
00591         arr = self._matrix3_to_dcm_array(dcm)
00592         return super(Quaternion, self)._dcm_to_q(arr)
00593 
00594     def _euler_to_dcm(self, euler):
00595         """
00596         Create DCM (Matrix3) from euler angles
00597         :param euler: array [roll, pitch, yaw] in rad
00598         :returns: Matrix3
00599         """
00600         assert(len(euler) == 3)
00601         m = Matrix3()
00602         m.from_euler(*euler)
00603         return m
00604 
00605     def _dcm_to_euler(self, dcm):
00606         """
00607         Create DCM from euler angles
00608         :param dcm: Matrix3
00609         :returns: array [roll, pitch, yaw] in rad
00610         """
00611         assert(isinstance(dcm, Matrix3))
00612         return np.array(dcm.to_euler())
00613     
00614     def __mul__(self, other):
00615         """
00616         :param other: Quaternion
00617         :returns: multiplaction of this Quaternion with other
00618         """
00619         return Quaternion(super(Quaternion, self).__mul__(other))
00620 
00621     def __truediv__(self, other):
00622         """
00623         :param other: Quaternion
00624         :returns: division of this Quaternion with other
00625         """
00626         return Quaternion(super(Quaternion, self).__truediv__(other))
00627 
00628 if __name__ == "__main__":
00629     import doctest
00630     doctest.testmod()
00631     import unittest
00632     unittest.main('quaterniontest')


mavlink
Author(s): Lorenz Meier
autogenerated on Wed Sep 9 2015 18:06:17