Submodules¶

sensbiotk.transforms3d.eulerangles module¶

Module implementing Euler angle rotations and their conversions

See:

• http://en.wikipedia.org/wiki/Rotation_matrix
• http://en.wikipedia.org/wiki/Euler_angles
• http://mathworld.wolfram.com/EulerAngles.html

See also: Representing Attitude with Euler Angles and Quaternions: A Reference (2006) by James Diebel. A cached PDF link last found here:

http://citeseerx.ist.psu.edu/viewdoc/summary?doi=10.1.1.110.5134

Euler’s rotation theorem tells us that any rotation in 3D can be described by 3 angles. Let’s call the 3 angles the Euler angle vector and call the angles in the vector , and . The vector is [ , . ] and, in this description, the order of the parameters specifies the order in which the rotations occur (so the rotation corresponding to is applied first).

In order to specify the meaning of an Euler angle vector we need to specify the axes around which each of the rotations corresponding to , and will occur.

There are therefore three axes for the rotations , and ; let’s call them , .

Let us express the rotation around axis i as a 3 by 3 rotation matrix A. Similarly around j becomes 3 x 3 matrix B and around k becomes matrix G. Then the whole rotation expressed by the Euler angle vector [ , . ], R is given by:

R = np.dot(G, np.dot(B, A))

See http://mathworld.wolfram.com/EulerAngles.html

The order expresses the fact that the rotations are performed in the order of the vector ( around axis i = A first).

To convert a given Euler angle vector to a meaningful rotation, and a rotation matrix, we need to define:

• the axes i, j, k
• whether a rotation matrix should be applied on the left of a vector to be transformed (vectors are column vectors) or on the right (vectors are row vectors).
• whether the rotations move the axes as they are applied (intrinsic rotations) - compared the situation where the axes stay fixed and the vectors move within the axis frame (extrinsic)
• the handedness of the coordinate system

See: http://en.wikipedia.org/wiki/Rotation_matrix#Ambiguities

We are using the following conventions:

• axes i, j, k are the z, y, and x axes respectively. Thus an Euler angle vector [ , . ] in our convention implies a radian rotation around the z axis, followed by a rotation around the y axis, followed by a rotation around the x axis.
• the rotation matrix applies on the left, to column vectors on the right, so if R is the rotation matrix, and v is a 3 x N matrix with N column vectors, the transformed vector set vdash is given by vdash = np.dot(R, v).
• extrinsic rotations - the axes are fixed, and do not move with the rotations.
• a right-handed coordinate system

The convention of rotation around z, followed by rotation around y, followed by rotation around x, is known (confusingly) as “xyz”, pitch-roll-yaw, Cardan angles, or Tait-Bryan angles.

sensbiotk.transforms3d.eulerangles.angle_axis2euler(theta, vector, is_normalized=False)[source]¶

Convert angle, axis pair to Euler angles

Parameters:	theta : scalar angle of rotation vector : 3 element sequence vector specifying axis for rotation. is_normalized : bool, optional True if vector is already normalized (has norm of 1). Default False
Returns:	z : scalar y : scalar x : scalar Rotations in radians around z, y, x axes, respectively

Notes

It’s possible to reduce the amount of calculation a little, by combining parts of the angle_axis2mat and mat2euler functions, but the reduction in computation is small, and the code repetition is large.

Examples

>>> z, y, x = angle_axis2euler(0, [1, 0, 0])
>>> np.allclose((z, y, x), 0)
True

sensbiotk.transforms3d.eulerangles.euler2angle_axis(z=0, y=0, x=0)[source]¶

Return angle, axis corresponding to these Euler angles

Uses the z, then y, then x convention above

Parameters:	z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last)
Returns:	theta : scalar angle of rotation vector : array shape (3,) axis around which rotation occurs

Examples

>>> theta, vec = euler2angle_axis(0, 1.5, 0)
>>> print(theta)
1.5
>>> np.allclose(vec, [0, 1, 0])
True

sensbiotk.transforms3d.eulerangles.euler2mat(z=0, y=0, x=0)[source]¶

Return matrix for rotations around z, y and x axes

Uses the z, then y, then x convention above

Parameters:	z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last)
Returns:	M : array shape (3,3) Rotation matrix giving same rotation as for given angles

Notes

The direction of rotation is given by the right-hand rule (orient the thumb of the right hand along the axis around which the rotation occurs, with the end of the thumb at the positive end of the axis; curl your fingers; the direction your fingers curl is the direction of rotation). Therefore, the rotations are counterclockwise if looking along the axis of rotation from positive to negative.

Examples

>>> zrot = 1.3 # radians
>>> yrot = -0.1
>>> xrot = 0.2
>>> M = euler2mat(zrot, yrot, xrot)
>>> M.shape == (3, 3)
True

The output rotation matrix is equal to the composition of the individual rotations

>>> M1 = euler2mat(zrot)
>>> M2 = euler2mat(0, yrot)
>>> M3 = euler2mat(0, 0, xrot)
>>> composed_M = np.dot(M3, np.dot(M2, M1))
>>> np.allclose(M, composed_M)
True

You can specify rotations by named arguments

>>> np.all(M3 == euler2mat(x=xrot))
True

When applying M to a vector, the vector should column vector to the right of M. If the right hand side is a 2D array rather than a vector, then each column of the 2D array represents a vector.

>>> vec = np.array([1, 0, 0]).reshape((3,1))
>>> v2 = np.dot(M, vec)
>>> vecs = np.array([[1, 0, 0],[0, 1, 0]]).T # giving 3x2 array
>>> vecs2 = np.dot(M, vecs)

Rotations are counter-clockwise.

>>> zred = np.dot(euler2mat(z=np.pi/2), np.eye(3))
>>> np.allclose(zred, [[0, -1, 0],[1, 0, 0], [0, 0, 1]])
True
>>> yred = np.dot(euler2mat(y=np.pi/2), np.eye(3))
>>> np.allclose(yred, [[0, 0, 1],[0, 1, 0], [-1, 0, 0]])
True
>>> xred = np.dot(euler2mat(x=np.pi/2), np.eye(3))
>>> np.allclose(xred, [[1, 0, 0],[0, 0, -1], [0, 1, 0]])
True

sensbiotk.transforms3d.eulerangles.euler2quat(z=0, y=0, x=0)[source]¶

Return quaternion corresponding to these Euler angles

Uses the z, then y, then x convention above

Parameters:	z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last)
Returns:	quat : array shape (4,) Quaternion in w, x, y z (real, then vector) format

Notes

We can derive this formula in Sympy using:

1. Formula giving quaternion corresponding to rotation of theta radians about arbitrary axis: http://mathworld.wolfram.com/EulerParameters.html
2. Generated formulae from 1.) for quaternions corresponding to theta radians rotations about x, y, z axes
3. Apply quaternion multiplication formula - http://en.wikipedia.org/wiki/Quaternions#Hamilton_product - to formulae from 2.) to give formula for combined rotations.

sensbiotk.transforms3d.eulerangles.mat2euler(M, cy_thresh=None)[source]¶

Discover Euler angle vector from 3x3 matrix

Uses the conventions above.

Parameters:	M : array-like, shape (3,3) cy_thresh : None or scalar, optional threshold below which to give up on straightforward arctan for estimating x rotation. If None (default), estimate from precision of input.
Returns:	z : scalar y : scalar x : scalar Rotations in radians around z, y, x axes, respectively

Notes

If there was no numerical error, the routine could be derived using Sympy expression for z then y then x rotation matrix, which is:

[                       cos(y)*cos(z),                       -cos(y)*sin(z),         sin(y)],
[cos(x)*sin(z) + cos(z)*sin(x)*sin(y), cos(x)*cos(z) - sin(x)*sin(y)*sin(z), -cos(y)*sin(x)],
[sin(x)*sin(z) - cos(x)*cos(z)*sin(y), cos(z)*sin(x) + cos(x)*sin(y)*sin(z),  cos(x)*cos(y)]

with the obvious derivations for z, y, and x

z = atan2(-r12, r11) y = asin(r13) x = atan2(-r23, r33)

Problems arise when cos(y) is close to zero, because both of:

z = atan2(cos(y)*sin(z), cos(y)*cos(z))
x = atan2(cos(y)*sin(x), cos(x)*cos(y))

will be close to atan2(0, 0), and highly unstable.

The cy fix for numerical instability below is from: Graphics Gems IV, Paul Heckbert (editor), Academic Press, 1994, ISBN: 0123361559. Specifically it comes from EulerAngles.c by Ken Shoemake, and deals with the case where cos(y) is close to zero:

See: http://www.graphicsgems.org/

The code appears to be licensed (from the website) as “can be used without restrictions”.

sensbiotk.transforms3d.eulerangles.quat2euler(q)[source]¶

Return Euler angles corresponding to quaternion q

Parameters:	q : 4 element sequence w, x, y, z of quaternion
Returns:	z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last)

Notes

It’s possible to reduce the amount of calculation a little, by combining parts of the quat2mat and mat2euler functions, but the reduction in computation is small, and the code repetition is large.

sensbiotk.transforms3d.eulerangles.quat2euler2(q)[source]¶

Return Euler angles corresponding to quaternion q

Parameters:	q : 4 element sequence w, x, y, z of quaternion
Returns:	z : scalar Rotation angle in radians around z-axis (performed first) y : scalar Rotation angle in radians around y-axis x : scalar Rotation angle in radians around x-axis (performed last)

Notes

Simple implementation using : http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles

sensbiotk.transforms3d.quaternions module¶

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)

sensbiotk.transforms3d.quaternions.angle_axis2mat(theta, vector, is_normalized=False)[source]¶

Rotation matrix of angle theta around vector

Parameters:	theta : scalar angle of rotation vector : 3 element sequence vector specifying axis for rotation. is_normalized : bool, optional True if vector is already normalized (has norm of 1). Default False
Returns:	mat : array shape (3,3) rotation matrix specified rotation

Notes

From: http://en.wikipedia.org/wiki/Rotation_matrix#Axis_and_angle

sensbiotk.transforms3d.quaternions.angle_axis2quat(theta, vector, is_normalized=False)[source]¶

Quaternion for rotation of angle theta around vector

Parameters:	theta : scalar angle of rotation vector : 3 element sequence vector specifying axis for rotation. 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 = angle_axis2quat(np.pi, [1, 0, 0])
>>> np.allclose(q, [0, 1, 0,  0])
True

sensbiotk.transforms3d.quaternions.conjugate(q)[source]¶

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

sensbiotk.transforms3d.quaternions.eye()[source]¶

Return identity quaternion

sensbiotk.transforms3d.quaternions.fillpositive(xyz, w2_thresh=None)[source]¶

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

sensbiotk.transforms3d.quaternions.inverse(q)[source]¶

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

sensbiotk.transforms3d.quaternions.isunit(q)[source]¶

Return True is this is very nearly a unit quaternion

sensbiotk.transforms3d.quaternions.mat2quat(M)[source]¶

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

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

• 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

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

sensbiotk.transforms3d.quaternions.mult(q1, q2)[source]¶

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

sensbiotk.transforms3d.quaternions.nearly_equivalent(q1, q2, rtol=1e-05, atol=1e-08)[source]¶

Returns True if q1 and q2 give near equivalent transforms

q1 may be nearly numerically equal to q2, or nearly equal to q2 * -1 (becuase 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

sensbiotk.transforms3d.quaternions.norm(q)[source]¶

Return norm of quaternion

Parameters:	q : 4 element sequence w, i, j, k of quaternion
Returns:	n : scalar quaternion norm

sensbiotk.transforms3d.quaternions.norm2(q)[source]¶

Return norm of quaternion with sqrt

Parameters:	q : 4 element sequence w, i, j, k of quaternion
Returns:	n : scalar quaternion norm

sensbiotk.transforms3d.quaternions.normalize(q)[source]¶

Return quaternion with normalized

Parameters:	q : 4 element sequence w, i, j, k of quaternion
Returns:	n : 4 element sequence quaternion normalized

sensbiotk.transforms3d.quaternions.quat2angle_axis(quat, identity_thresh=None)[source]¶

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]

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

sensbiotk.transforms3d.quaternions.quat2mat(q)[source]¶

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 non-unit quaternions.

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

sensbiotk.transforms3d.quaternions.rotate_vector(v, q)[source]¶

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

sensbiotk.transforms3d.setup module¶

sensbiotk.transforms3d.setup.configuration(parent_package='', top_path=None)[source]¶

sensbiotk.transforms3d.six module¶

Utilities for writing code that runs on Python 2 and 3

class sensbiotk.transforms3d.six.Iterator[source]¶

Bases: object

Methods

next()[source]¶

class sensbiotk.transforms3d.six.MovedAttribute(name, old_mod, new_mod, old_attr=None, new_attr=None)[source]¶

Bases: sensbiotk.transforms3d.six._LazyDescr

class sensbiotk.transforms3d.six.MovedModule(name, old, new=None)[source]¶

Bases: sensbiotk.transforms3d.six._LazyDescr

class sensbiotk.transforms3d.six.X[source]¶

Bases: object

sensbiotk.transforms3d.six.add_move(move)[source]¶

Add an item to six.moves.

sensbiotk.transforms3d.six.b(s)[source]¶

Byte literal

sensbiotk.transforms3d.six.exec_(_code_, _globs_=None, _locs_=None)[source]¶

Execute code in a namespace.

sensbiotk.transforms3d.six.get_unbound_function(unbound)[source]¶

Get the function out of a possibly unbound function

sensbiotk.transforms3d.six.iteritems(d, **kw)[source]¶

Return an iterator over the (key, value) pairs of a dictionary.

sensbiotk.transforms3d.six.iterkeys(d, **kw)[source]¶

Return an iterator over the keys of a dictionary.

sensbiotk.transforms3d.six.iterlists(d, **kw)[source]¶

Return an iterator over the (key, [values]) pairs of a dictionary.

sensbiotk.transforms3d.six.itervalues(d, **kw)[source]¶

Return an iterator over the values of a dictionary.

sensbiotk.transforms3d.six.print_(*args, **kwargs)[source]¶

The new-style print function.

sensbiotk.transforms3d.six.remove_move(name)[source]¶

Remove item from six.moves.

sensbiotk.transforms3d.six.reraise(tp, value, tb=None)[source]¶

Reraise an exception.

sensbiotk.transforms3d.six.u(s)[source]¶

Text literal

sensbiotk.transforms3d.six.with_metaclass(meta, base=<type 'object'>)[source]¶

Create a base class with a metaclass.

sensbiotk.transforms3d.utils module¶

Utilities for transforms3d

sensbiotk.transforms3d.utils.inique(iterable)[source]¶

Generate unique elements from iterable

Parameters:	iterable : iterable
Returns:	gen : generator generator that yields unique elements from iterable

Examples

>>> tuple(inique([0, 1, 2, 0, 2, 3]))
(0, 1, 2, 3)

sensbiotk.transforms3d.utils.normalized_vector(vec)[source]¶

Return vector divided by Euclidean (L2) norm

See unit vector and Euclidean norm

Parameters:	vec : array-like shape (3,)
Returns:	nvec : array shape (3,) vector divided by L2 norm

Examples

>>> vec = [1, 2, 3]
>>> l2n = np.sqrt(np.dot(vec, vec))
>>> nvec = normalized_vector(vec)
>>> np.allclose(np.array(vec) / l2n, nvec)
True
>>> vec = np.array([[1, 2, 3]])
>>> vec.shape
(1, 3)
>>> normalized_vector(vec).shape
(3,)

sensbiotk.transforms3d.utils.permutations(iterable, r=None)[source]¶

Generate all permutations of iterable, of length r

From Python docs, expressing 2.6 itertools.permutations algorithm.

If the elements are unique, then the resulting permutations will also be unique.

Parameters:	iterable : iterable returning elements that will be permuted r : None or int length of sequence to return, if None (default) use length of iterable
Returns:	gen : generator generator that yields permutations

Examples

>>> tuple(permutations(range(3)))
((0, 1, 2), (0, 2, 1), (1, 0, 2), (1, 2, 0), (2, 0, 1), (2, 1, 0))
>>> tuple(permutations(range(3), 2))
((0, 1), (0, 2), (1, 0), (1, 2), (2, 0), (2, 1))

sensbiotk.transforms3d.utils.permuted_signs(seq)[source]¶

Generate permuted signs for sequence seq

Parameters:	seq : sequence
Returns:	gen : generator generator returning seq with signs permuted

Examples

>>> tuple(permuted_signs([1, -2, 0]))
((1, -2, 0), (1, -2, 0), (1, 2, 0), (1, 2, 0), (-1, -2, 0), (-1, -2, 0), (-1, 2, 0), (-1, 2, 0))

sensbiotk.transforms3d.utils.permuted_with_signs(seq)[source]¶

Return all permutations of seq with all sign permutations

Parameters:	seq : sequence
Returns:	gen : generator generator returning permutations and sign permutations

Examples

>>> tuple(permuted_with_signs((1,2)))
((1, 2), (1, -2), (-1, 2), (-1, -2), (2, 1), (2, -1), (-2, 1), (-2, -1))

sensbiotk.transforms3d.utils.vector_norm(vec)[source]¶

Return vector Euclidaan (L2) norm

See unit vector and Euclidean norm

Parameters:	vec : array-like shape (3,)
Returns:	norm : scalar

Examples

>>> vec = [1, 2, 3]
>>> l2n = np.sqrt(np.dot(vec, vec))
>>> nvec = vector_norm(vec)
>>> np.allclose(nvec, np.sqrt(np.dot(vec, vec)))
True

Module contents¶