quaternions

Functions to operate on, or return, quaternions.

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)

Terms used in function names:

  • mat : array shape (3, 3) (3D non-homogenous coordinates)
  • aff : affine array shape (4, 4) (3D homogenous coordinates)
  • quat : quaternion shape (4,)
  • axangle : rotations encoded by axis vector and angle scalar
axangle2quat(vector, theta[, is_normalized]) Quaternion for rotation of angle theta around vector
fillpositive(xyz[, w2_thresh]) Compute unit quaternion from last 3 values
mat2quat(M) Calculate quaternion corresponding to given rotation matrix
nearly_equivalent(q1, q2[, rtol, atol]) Returns True if q1 and q2 give near equivalent transforms
qconjugate(q) Conjugate of quaternion
qeye() Return identity quaternion
qinverse(q) Return multiplicative inverse of quaternion q
qisunit(q) Return True is this is very nearly a unit quaternion
qmult(q1, q2) Multiply two quaternions
qnorm(q) Return norm of quaternion
quat2axangle(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

axangle2quat

transforms3d.quaternions.axangle2quat(vector, theta, is_normalized=False)

Quaternion for rotation of angle theta around vector

Parameters:

vector : 3 element sequence

vector specifying axis for rotation.

theta : scalar

angle of rotation in radians.

is_normalized : bool, optional

True if vector is already normalized (has norm of 1). Default False.

Returns:

quat : 4 element sequence of symbols

quaternion giving specified rotation

Notes

Formula from http://mathworld.wolfram.com/EulerParameters.html

Examples

>>> q = axangle2quat([1, 0, 0], np.pi)
>>> np.allclose(q, [0, 1, 0,  0])
True

fillpositive

transforms3d.quaternions.fillpositive(xyz, w2_thresh=None)

Compute unit quaternion from last 3 values

Parameters:

xyz : iterable

iterable containing 3 values, corresponding to quaternion x, y, z

w2_thresh : None or float, optional

threshold to determine if w squared is really negative. If None (default) then w2_thresh set equal to -np.finfo(xyz.dtype).eps, if possible, otherwise -np.finfo(np.float).eps

Returns:

wxyz : array shape (4,)

Full 4 values of quaternion

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

mat2quat

transforms3d.quaternions.mat2quat(M)

Calculate quaternion corresponding to given rotation matrix

Parameters:

M : array-like

3x3 rotation matrix

Returns:

q : (4,) array

closest quaternion to input matrix, having positive q[0]

Notes

http://en.wikipedia.org/wiki/Rotation_matrix#Quaternion

Bar-Itzhack, Itzhack Y. (2000), “New method for extracting the quaternion from a rotation matrix”, AIAA Journal of Guidance, Control and Dynamics 23(6):1085-1087 (Engineering Note), ISSN 0731-5090

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

nearly_equivalent

transforms3d.quaternions.nearly_equivalent(q1, q2, rtol=1e-05, atol=1e-08)

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

w, x, y, z of first quaternion

q2 : 4 element sequence

w, x, y, z of second quaternion

Returns:

equiv : bool

True if q1 and q2 are nearly equivalent, False otherwise

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

qconjugate

transforms3d.quaternions.qconjugate(q)

Conjugate of quaternion

Parameters:

q : 4 element sequence

w, i, j, k of quaternion

Returns:

conjq : array shape (4,)

w, i, j, k of conjugate of q

qeye

transforms3d.quaternions.qeye()

Return identity quaternion

qinverse

transforms3d.quaternions.qinverse(q)

Return multiplicative inverse of quaternion q

Parameters:

q : 4 element sequence

w, i, j, k of quaternion

Returns:

invq : array shape (4,)

w, i, j, k of quaternion inverse

qisunit

transforms3d.quaternions.qisunit(q)

Return True is this is very nearly a unit quaternion

qmult

transforms3d.quaternions.qmult(q1, q2)

Multiply two quaternions

Parameters:

q1 : 4 element sequence

q2 : 4 element sequence

Returns:

q12 : shape (4,) array

Notes

See : http://en.wikipedia.org/wiki/Quaternions#Hamilton_product

qnorm

transforms3d.quaternions.qnorm(q)

Return norm of quaternion

Parameters:

q : 4 element sequence

w, i, j, k of quaternion

Returns:

n : scalar

quaternion norm

quat2axangle

transforms3d.quaternions.quat2axangle(quat, identity_thresh=None)

Convert quaternion to rotation of angle around axis

Parameters:

quat : 4 element sequence

w, x, y, z forming quaternion.

identity_thresh : None or scalar, optional

Threshold below which the norm of the vector part of the quaternion (x, y, z) is deemed to be 0, leading to the identity rotation. None (the default) leads to a threshold estimated based on the precision of the input.

Returns:

theta : scalar

angle of rotation.

vector : array shape (3,)

axis around which rotation occurs.

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].

The algorithm allows for quaternions that have not been normalized.

Examples

>>> vec, theta = quat2axangle([0, 1, 0, 0])
>>> vec
array([ 1.,  0.,  0.])
>>> np.allclose(theta, np.pi)
True

If this is an identity rotation, we return a zero angle and an arbitrary vector:

>>> quat2axangle([1, 0, 0, 0])
(array([ 1.,  0.,  0.]), 0.0)

If any of the quaternion values are not finite, we return a NaN in the angle, and an arbitrary vector:

>>> quat2axangle([1, np.inf, 0, 0])
(array([ 1.,  0.,  0.]), nan)

quat2mat

transforms3d.quaternions.quat2mat(q)

Calculate rotation matrix corresponding to quaternion

Parameters:

q : 4 element array-like

Returns:

M : (3,3) array

Rotation matrix corresponding to input quaternion q

Notes

Rotation matrix applies to column vectors, and is applied to the left of coordinate vectors. The algorithm here allows quaternions that have not been normalized.

References

Algorithm from http://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

rotate_vector

transforms3d.quaternions.rotate_vector(v, q)

Apply transformation in quaternion q to vector v

Parameters:

v : 3 element sequence

3 dimensional vector

q : 4 element sequence

w, i, j, k of quaternion

Returns:

vdash : array shape (3,)

v rotated by quaternion q

Notes

See: http://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Describing_rotations_with_quaternions