Functions to operate on, or return, quaternions.
The module also includes functions for the closely related angle, axis pair as a specification for rotation.
Quaternions here consist of 4 values w, x, y, z, where w is the real (scalar) part, and x, y, z are the complex (vector) part.
Note - rotation matrices here apply to column vectors, that is, they are applied on the left of the vector. For example:
>>> import numpy as np
>>> q = [0, 1, 0, 0] # 180 degree rotation around axis 0
>>> M = quat2mat(q) # from this module
>>> vec = np.array([1, 2, 3]).reshape((3,1)) # column vector
>>> tvec = np.dot(M, vec)
angle_axis2mat(theta, vector[, is_normalized]) | Rotation matrix of angle theta around vector |
angle_axis2quat(theta, vector[, is_normalized]) | Quaternion for rotation of angle theta around vector |
conjugate(q) | Conjugate of quaternion |
eye() | Return identity quaternion |
fillpositive(xyz[, w2_thresh]) | Compute unit quaternion from last 3 values |
inverse(q) | Return multiplicative inverse of quaternion q |
isunit(q) | Return True is this is very nearly a unit quaternion |
mat2quat(M) | Calculate quaternion corresponding to given rotation matrix |
mult(q1, q2) | Multiply two quaternions |
nearly_equivalent(q1, q2[, rtol, atol]) | Returns True if q1 and q2 give near equivalent transforms |
norm(q) | Return norm of quaternion |
quat2angle_axis(quat[, identity_thresh]) | Convert quaternion to rotation of angle around axis |
quat2mat(q) | Calculate rotation matrix corresponding to quaternion |
rotate_vector(v, q) | Apply transformation in quaternion q to vector v |
Rotation matrix of angle theta around vector
Parameters: | theta : scalar
vector : 3 element sequence
is_normalized : bool, optional
|
---|---|
Returns: | mat : array shape (3,3)
|
Notes
From: https://en.wikipedia.org/wiki/Rotation_matrix#Axis_and_angle
Quaternion for rotation of angle theta around vector
Parameters: | theta : scalar
vector : 3 element sequence
is_normalized : bool, optional
|
---|---|
Returns: | quat : 4 element sequence of symbols
|
Notes
Formula from http://mathworld.wolfram.com/EulerParameters.html
Examples
>>> q = angle_axis2quat(np.pi, [1, 0, 0])
>>> np.allclose(q, [0, 1, 0, 0])
True
Conjugate of quaternion
Parameters: | q : 4 element sequence
|
---|---|
Returns: | conjq : array shape (4,)
|
Compute unit quaternion from last 3 values
Parameters: | xyz : iterable
w2_thresh : None or float, optional
|
---|---|
Returns: | wxyz : array shape (4,)
|
Notes
If w, x, y, z are the values in the full quaternion, assumes w is positive.
Gives error if w*w is estimated to be negative
w = 0 corresponds to a 180 degree rotation
The unit quaternion specifies that np.dot(wxyz, wxyz) == 1.
If w is positive (assumed here), w is given by:
w = np.sqrt(1.0-(x*x+y*y+z*z))
w2 = 1.0-(x*x+y*y+z*z) can be near zero, which will lead to numerical instability in sqrt. Here we use the system maximum float type to reduce numerical instability
Examples
>>> import numpy as np
>>> wxyz = fillpositive([0,0,0])
>>> np.all(wxyz == [1, 0, 0, 0])
True
>>> wxyz = fillpositive([1,0,0]) # Corner case; w is 0
>>> np.all(wxyz == [0, 1, 0, 0])
True
>>> np.dot(wxyz, wxyz)
1.0
Return multiplicative inverse of quaternion q
Parameters: | q : 4 element sequence
|
---|---|
Returns: | invq : array shape (4,)
|
Calculate quaternion corresponding to given rotation matrix
Parameters: | M : array-like
|
---|---|
Returns: | q : (4,) array
|
Notes
Method claimed to be robust to numerical errors in M
Constructs quaternion by calculating maximum eigenvector for matrix K (constructed from input M). Although this is not tested, a maximum eigenvalue of 1 corresponds to a valid rotation.
A quaternion q*-1 corresponds to the same rotation as q; thus the sign of the reconstructed quaternion is arbitrary, and we return quaternions with positive w (q[0]).
References
Examples
>>> import numpy as np
>>> q = mat2quat(np.eye(3)) # Identity rotation
>>> np.allclose(q, [1, 0, 0, 0])
True
>>> q = mat2quat(np.diag([1, -1, -1]))
>>> np.allclose(q, [0, 1, 0, 0]) # 180 degree rotn around axis 0
True
Multiply two quaternions
Parameters: | q1 : 4 element sequence q2 : 4 element sequence |
---|---|
Returns: | q12 : shape (4,) array |
Notes
See : https://en.wikipedia.org/wiki/Quaternions#Hamilton_product
Returns True if q1 and q2 give near equivalent transforms
q1 may be nearly numerically equal to q2, or nearly equal to q2 * -1 (because a quaternion multiplied by -1 gives the same transform).
Parameters: | q1 : 4 element sequence
q2 : 4 element sequence
|
---|---|
Returns: | equiv : bool
|
Examples
>>> q1 = [1, 0, 0, 0]
>>> nearly_equivalent(q1, [0, 1, 0, 0])
False
>>> nearly_equivalent(q1, [1, 0, 0, 0])
True
>>> nearly_equivalent(q1, [-1, 0, 0, 0])
True
Return norm of quaternion
Parameters: | q : 4 element sequence
|
---|---|
Returns: | n : scalar
|
Convert quaternion to rotation of angle around axis
Parameters: | quat : 4 element sequence
identity_thresh : None or scalar, optional
|
---|---|
Returns: | theta : scalar
vector : array shape (3,)
|
Notes
A quaternion for which x, y, z are all equal to 0, is an identity rotation. In this case we return a 0 angle and an arbitrary vector, here [1, 0, 0]
Examples
>>> theta, vec = quat2angle_axis([0, 1, 0, 0])
>>> np.allclose(theta, np.pi)
True
>>> vec
array([ 1., 0., 0.])
If this is an identity rotation, we return a zero angle and an arbitrary vector
>>> quat2angle_axis([1, 0, 0, 0])
(0.0, array([ 1., 0., 0.]))
Calculate rotation matrix corresponding to quaternion
Parameters: | q : 4 element array-like |
---|---|
Returns: | M : (3,3) array
|
Notes
Rotation matrix applies to column vectors, and is applied to the left of coordinate vectors. The algorithm here allows non-unit quaternions.
References
Algorithm from https://en.wikipedia.org/wiki/Rotation_matrix#Quaternion
Examples
>>> import numpy as np
>>> M = quat2mat([1, 0, 0, 0]) # Identity quaternion
>>> np.allclose(M, np.eye(3))
True
>>> M = quat2mat([0, 1, 0, 0]) # 180 degree rotn around axis 0
>>> np.allclose(M, np.diag([1, -1, -1]))
True
Apply transformation in quaternion q to vector v
Parameters: | v : 3 element sequence
q : 4 element sequence
|
---|---|
Returns: | vdash : array shape (3,)
|
Notes
See: https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Describing_rotations_with_quaternions