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

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. """
"""Return inverse of quaternion. """
"""Return real part of quaternion. """
"""Return imaginary part of quaternion. """
""" The axis and angle between two quaternions """ else: |