Coverage for navipy/maths/quaternion.py : 77%

Hot-keys on this page
r m x p toggle line displays
j k next/prev highlighted chunk
0 (zero) top of page
1 (one) first highlighted chunk
"""
"""
""" axis-angle quaternion function Returns a unit quaternion
:param a: angle in degrees :param n: unit canonical vector for a specific axis :returns: a vector :rtype: (np.ndarray) ..ref: James Diebel "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors." (2006): p. 17 (6.12) """
"""Return quaternion from Euler angles and axis sequence. ai, aj, ak : Euler's roll, pitch and yaw angles axes : One of 24 axis sequences as string or encoded tuple :param ai: angle in degrees to be rotated about the first axis :param aj: angle in degrees to be rotated about the second axis :param ak: angle in degrees to be rotated about the third axis :param axes: string that encodes the order of the axes and whether rotational or stationary axes should be used :returns: a vector :rtype: (np.ndarray) ..ref: James Diebel "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors." (2006): p. 18 (6.14) """
"""Return quaternion for rotation about axis.
..ref: James Diebel "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors." (2006): p. 17 (6.4), equation 175 """
"""Return homogeneous rotation matrix from quaternion. :param quaternion : vector with at least 3 entrences (unit quaternion) :returns: a matrix :rtype: (np.ndarray) ..ref: James Diebel "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors." (2006): p. 15 (6.4) """
# According to equation 125 [q0**2 + q1**2 - q2**2 - q3**2, 2 * q1 * q2 + 2 * q0 * q3, 2 * q1 * q3 - 2 * q0 * q2], [2 * q1 * q2 - 2 * q0 * q3, q0**2 - q1**2 + q2**2 - q3**2, 2 * q2 * q3 + 2 * q0 * q1], [2 * q1 * q3 + 2 * q0 * q2, 2 * q2 * q3 - 2 * q0 * q1, q0**2 - q1**2 - q2**2 + q3**2]])
"""Return quaternion from rotation matrix.
:param matrix: a rotation matrix :returns: a vector :rtype: (np.ndarray) ..ref: James Diebel "Representing Attitude: Euler Angles, Unit Quaternions, and Rotation Vectors." (2006): p. 15 (6.5) """ # Split cases according to equation 145 # equation 141 (r[1, 2] - r[2, 1]) / np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]), (r[2, 0] - r[0, 2]) / np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2]), (r[0, 1] - r[1, 0]) / np.sqrt(1 + r[0, 0] + r[1, 1] + r[2, 2])])
# equation 142 np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]), np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]), (r[0, 1] + r[1, 0]) / np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2]), (r[2, 0] + r[0, 2]) / np.sqrt(1 + r[0, 0] - r[1, 1] - r[2, 2])]) if (r[1, 1] > r[2, 2]) and (r[0, 0] < r[1, 1]) and (r[0, 0] < -r[2, 2]): # equation 143 return (1 / 2) * np.array([(r[2, 0] - r[0, 2]) / np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]), (r[0, 1] + r[1, 0]) / np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]), np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2]), (r[1, 2] + r[2, 1]) / np.sqrt(1 - r[0, 0] + r[1, 1] - r[2, 2])]) if (r[1, 1] < r[2, 2]) and (r[0, 0] < -r[1, 1]) and (r[0, 0] < r[2, 2]): # equation 144 return (1 / 2) * np.array([(r[0, 1] - r[1, 0]) / np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]), (r[2, 0] + r[0, 2]) / np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]), (r[1, 2] + r[2, 1]) / np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2]), np.sqrt(1 - r[0, 0] - r[1, 1] + r[2, 2])])
"""Return multiplication of two quaternions. """ x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0, -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0, x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0], dtype=np.float64)
"""Return conjugate of quaternion. """ q = np.array(quaternion, dtype=np.float64, copy=True) np.negative(q[1:], q[1:]) return q
"""Return inverse of quaternion. """
"""Return real part of quaternion. """ return float(quaternion[0])
"""Return imaginary part of quaternion. """ return np.array(quaternion[1:4], dtype=np.float64, copy=True)
""" The axis and angle between two quaternions """ q = multiply(quaternion1, conjugate(quaternion0)) length = np.sum(np.sqrt(q[1:4] * q[1:4])) angle = 2 * np.arctan2(length, q[0]) if np.isclose(length, 0): axis = np.array([1, 0, 0]) else: axis = q[1:4] / length return axis, angle |