5 Quaternion implementation for use in pymavlink 8 from __future__
import absolute_import, division, print_function
10 from builtins
import object
12 from .rotmat
import Vector3, Matrix3
14 __author__ =
"Thomas Gubler" 15 __copyright__ =
"Copyright (C) 2014 Thomas Gubler" 16 __license__ =
"GNU Lesser General Public License v3" 17 __email__ =
"thomasgubler@gmail.com" 23 Quaternion class, this is the version which supports numpy arrays 24 If you need support for Matrix3 look at the Quaternion class 27 >>> from quaternion import QuaternionBase 28 >>> import numpy as np 29 >>> q = QuaternionBase([np.radians(20), np.radians(20), np.radians(20)]) 31 [ 0.9603483 0.13871646 0.19810763 0.13871646] 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]) 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) 47 Construct a quaternion from an attitude 49 :param attitude: another QuaternionBase, 50 3 element list [roll, pitch, yaw], 51 4 element list [w, x, y ,z], DCM (3x3 array) 53 if isinstance(attitude, QuaternionBase):
55 elif np.array(attitude).shape == (3, 3):
57 elif len(attitude) == 4:
59 elif len(attitude) == 3:
62 raise TypeError(
"attitude is not valid")
68 :returns: array containing the quaternion elements 71 if self.
_euler is not None:
74 elif self.
_dcm is not None:
80 """Returns the quaternion entry at index""" 87 :param q: list or array of quaternion values [w, x, y, z] 100 The convention is Tait-Bryan (ZY'X'') 102 :returns: array containing the euler angles [roll, pitch, yaw] 105 if self.
_q is not None:
109 elif self.
_dcm is not None:
118 :param euler: list or array of the euler angles [roll, pitch, yaw] 121 assert(len(euler) == 3)
122 self.
_euler = np.array(euler)
136 if self.
_dcm is None:
137 if self.
_q is not None:
140 elif self.
_euler is not None:
149 :param dcm: 3x3 array 152 assert(len(dcm) == 3)
154 assert(len(sub) == 3)
156 self.
_dcm = np.array(dcm)
165 Calculates the vector transformed by this quaternion 166 :param v: array with len 3 to be transformed 167 :returns: transformed vector 170 assert(np.allclose(self.
norm, 1))
176 a = q0 * ui + np.cross(qi, ui)
177 t = np.dot(qi, ui) * qi + q0 * a - np.cross(a, qi)
183 Returns norm of quaternion 185 :returns: norm (scalar) 187 return QuaternionBase.norm_array(self.
q)
190 """Normalizes the quaternion""" 191 self.
q = QuaternionBase.normalize_array(self.
q)
196 Get inversed quaternion 198 :returns: inversed quaternion 205 Equality test (same orientation, not necessarily same rotation) 207 :param other: a QuaternionBase 208 :returns: true if the quaternions are equal 210 if isinstance(other, QuaternionBase):
211 return abs(self.q.dot(other.q)) > 1 - np.finfo(float).eps
212 return NotImplemented
216 Equality test with tolerance 217 (same orientation, not necessarily same rotation) 220 :param other: a QuaternionBase 221 :returns: true if the quaternions are almost equal 223 if isinstance(other, QuaternionBase):
224 return np.allclose(self.
q, other.q)
or np.allclose(self.
q, -other.q)
225 return NotImplemented
229 :param other: QuaternionBase 230 :returns: multiplaction of this Quaternion with other 232 if isinstance(other, QuaternionBase):
234 elif len(other) == 4:
237 return NotImplemented
243 :param other: QuaternionBase 244 :returns: division of this Quaternion with other 246 if isinstance(other, QuaternionBase):
248 elif len(other) == 4:
251 return NotImplemented
252 return self * o.inversed
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 263 n = QuaternionBase.norm_array(q)
269 Calculate quaternion norm on array q 270 :param quaternion: array of len 4 271 :returns: norm (scalar) 274 return np.sqrt(np.dot(q, q))
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) 283 assert(len(q) == len(p) == 4)
290 res[0] = p0 * q0 - np.dot(pi, qi)
291 res[1:4] = p0 * qi + q0 * pi + np.cross(pi, qi)
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] 301 assert(len(euler) == 3)
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)
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)
325 :param q: array q which represents a quaternion [w, x, y, z] 326 :returns: 3x3 dcm array 329 assert(np.allclose(QuaternionBase.norm_array(q), 1))
330 dcm = np.zeros([3, 3])
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
354 - Shoemake, Quaternions, 355 http://www.cs.ucr.edu/~vbz/resources/quatut.pdf 357 :param dcm: 3x3 dcm array 358 returns: quaternion array 360 assert(dcm.shape == (3, 3))
365 s = np.sqrt(tr + 1.0)
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
372 dcm_i = np.argmax(np.diag(dcm))
373 dcm_j = (dcm_i + 1) % 3
374 dcm_k = (dcm_i + 2) % 3
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
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
388 Create DCM from euler angles 389 :param euler: array [roll, pitch, yaw] in rad 390 :returns: 3x3 dcm array 392 assert(len(euler) == 3)
396 dcm = np.zeros([3, 3])
399 c_theta = np.cos(theta)
400 s_theta = np.sin(theta)
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
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
413 dcm[2][1] = s_phi * c_theta
414 dcm[2][2] = c_phi * c_theta
419 Create DCM from euler angles 420 :param dcm: 3x3 dcm array 421 :returns: array [roll, pitch, yaw] in rad 423 assert(dcm.shape == (3, 3))
424 theta = np.arcsin(min(1, max(-1, -dcm[2][0])))
426 if abs(theta - np.pi/2) < 1.0e-3:
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:
432 psi = np.arctan2(dcm[1][2] - dcm[0][1],
433 dcm[0][2] + dcm[1][1] - phi)
435 phi = np.arctan2(dcm[2][1], dcm[2][2])
436 psi = np.arctan2(dcm[1][0], dcm[0][0])
438 return np.array([phi, theta, psi])
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] 447 return np.hstack([q[0], -q[1:4]])
450 """String of quaternion values""" 457 Quaternion class that supports pymavlink's Vector3 and Matrix3 460 >>> from quaternion import Quaternion 461 >>> from rotmat import Vector3, Matrix3 463 >>> m.from_euler(45, 0, 0) 465 Matrix3((1.00, 0.00, 0.00), (0.00, 0.53, -0.85), (-0.00, 0.85, 0.53)) 466 >>> q = Quaternion(m) 468 [ 0.87330464 0.48717451 0. 0. ] 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) 474 Vector3(0.00, 0.53, 0.85) 479 Construct a quaternion from an attitude 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) 485 if isinstance(attitude, Quaternion):
487 if isinstance(attitude, Matrix3):
489 elif np.array(attitude).shape == (3, 3):
492 elif isinstance(attitude, Vector3):
494 euler = [attitude.x, attitude.y, attitude.z]
495 super(Quaternion, self).
__init__(euler)
497 super(Quaternion, self).
__init__(attitude)
506 if self.
_dcm is None:
507 if self.
_q is not None:
510 elif self.
_euler is not None:
522 assert(isinstance(dcm, Matrix3))
523 self.
_dcm = dcm.copy()
533 Get inversed quaternion 535 :returns: inversed quaternion 537 return Quaternion(super(Quaternion, self).inversed)
541 Calculates the vector transformed by this quaternion 542 :param v3: Vector3 to be transformed 543 :returns: transformed vector 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])
549 return super(Quaternion, self).
transform(v3)
551 raise TypeError(
"param v3 is not a vector type")
555 Converts dcm array into Matrix3 556 :param dcm: 3x3 dcm array 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)
567 Converts Matrix3 in an array 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]])
578 Create DCM (Matrix3) from q 579 :param q: array q which represents a quaternion [w, x, y, z] 583 arr = super(Quaternion, self).
_q_to_dcm(q)
588 Create q from dcm (Matrix3) 590 :returns: array q which represents a quaternion [w, x, y, z] 592 assert(isinstance(dcm, Matrix3))
594 return super(Quaternion, self).
_dcm_to_q(arr)
598 Create DCM (Matrix3) from euler angles 599 :param euler: array [roll, pitch, yaw] in rad 602 assert(len(euler) == 3)
609 Create DCM from euler angles 611 :returns: array [roll, pitch, yaw] in rad 613 assert(isinstance(dcm, Matrix3))
614 return np.array(dcm.to_euler())
618 :param other: Quaternion 619 :returns: multiplaction of this Quaternion with other 625 :param other: Quaternion 626 :returns: division of this Quaternion with other 630 if __name__ ==
"__main__":
634 unittest.main(
'quaterniontest')
def _matrix3_to_dcm_array(self, m)
def __getitem__(self, index)
def _euler_to_q(self, euler)
def __truediv__(self, other)
def _euler_to_dcm(self, euler)
def _euler_to_dcm(self, euler)
def __init__(self, attitude)
def _dcm_array_to_matrix3(self, dcm)
def _dcm_to_euler(self, dcm)
def __truediv__(self, other)
def __init__(self, attitude=[1)
def _dcm_to_euler(self, dcm)
def _mul_array(self, p, q)