rofunc.utils.robolab.coord.transform

Homogeneous Transformation Matrices and Quaternions. A library for calculating 4x4 matrices for translating, rotating, reflecting, scaling, shearing, projecting, orthogonalizing, and superimposing arrays of 3D homogeneous coordinates as well as for converting between rotation matrices, Euler angles, and quaternions. Also includes an Arcball control object and functions to decompose transformation matrices. :Authors:

Christoph Gohlke, Laboratory for Fluorescence Dynamics, University of California, Irvine

Version:

20090418

1.  Notes

Matrices (M) can be inverted using np.linalg.inv(M), concatenated using np.dot(M0, M1), or used to transform homogeneous coordinates (v) using np.dot(M, v) for shape (4, *) “point of arrays”, respectively np.dot(v, M.T) for shape (*, 4) “array of points”. Calculations are carried out with np.float64 precision. This Python implementation is not optimized for speed. Vector, point, quaternion, and matrix math arguments are expected to be “array like”, i.e. tuple, list, or np arrays. Return types are np arrays unless specified otherwise. Angles are in radians unless specified otherwise. Quaternions ix+jy+kz+w are represented as [x, y, z, w]. Use the transpose of transformation matrices for OpenGL glMultMatrixd(). A triple of Euler angles can be applied/interpreted in 24 ways, which can be specified using a 4 character string or encoded 4-tuple:

Axes 4-string: e.g. ‘sxyz’ or ‘ryxy’ - first character : rotations are applied to ‘s’tatic or ‘r’otating frame - remaining characters : successive rotation axis ‘x’, ‘y’, or ‘z’ Axes 4-tuple: e.g. (0, 0, 0, 0) or (1, 1, 1, 1) - inner axis: code of axis (‘x’:0, ‘y’:1, ‘z’:2) of rightmost matrix. - parity : even (0) if inner axis ‘x’ is followed by ‘y’, ‘y’ is followed

by ‘z’, or ‘z’ is followed by ‘x’. Otherwise odd (1).

• repetition : first and last axis are same (1) or different (0).

• frame : rotations are applied to static (0) or rotating (1) frame.

2.  References

1. Matrices and transformations. Ronald Goldman. In “Graphics Gems I”, pp 472-475. Morgan Kaufmann, 1990.

2. More matrices and transformations: shear and pseudo-perspective. Ronald Goldman. In “Graphics Gems II”, pp 320-323. Morgan Kaufmann, 1991.

3. Decomposing a matrix into simple transformations. Spencer Thomas. In “Graphics Gems II”, pp 320-323. Morgan Kaufmann, 1991.

4. Recovering the data from the transformation matrix. Ronald Goldman. In “Graphics Gems II”, pp 324-331. Morgan Kaufmann, 1991.

5. Euler angle conversion. Ken Shoemake. In “Graphics Gems IV”, pp 222-229. Morgan Kaufmann, 1994.

6. Arcball rotation control. Ken Shoemake. In “Graphics Gems IV”, pp 175-192. Morgan Kaufmann, 1994.

7. Representing attitude: Euler angles, unit quaternions, and rotation vectors. James Diebel. 2006.

8. A discussion of the solution for the best rotation to relate two sets of vectors. W Kabsch. Acta Cryst. 1978. A34, 827-828.

9. Closed-form solution of absolute orientation using unit quaternions. BKP Horn. J Opt Soc Am A. 1987. 4(4), 629-642.

10. Quaternions. Ken Shoemake. http://www.sfu.ca/~jwa3/cmpt461/files/quatut.pdf

11. From quaternion to matrix and back. JMP van Waveren. 2005. http://www.intel.com/cd/ids/developer/asmo-na/eng/293748.htm

12. Uniform random rotations. Ken Shoemake. In “Graphics Gems III”, pp 124-132. Morgan Kaufmann, 1992.

3.  Examples

>>> alpha, beta, gamma = 0.123, -1.234, 2.345
>>> origin, xaxis, yaxis, zaxis = (0, 0, 0), (1, 0, 0), (0, 1, 0), (0, 0, 1)
>>> I = identity_matrix()
>>> Rx = rotation_matrix(alpha, xaxis)
>>> Ry = rotation_matrix(beta, yaxis)
>>> Rz = rotation_matrix(gamma, zaxis)
>>> R = concatenate_matrices(Rx, Ry, Rz)
>>> euler = euler_from_homo_matrix(R, 'rxyz')
>>> np.allclose([alpha, beta, gamma], euler)
True
>>> Re = homo_matrix_from_euler(alpha, beta, gamma, 'rxyz')
>>> is_same_transform(R, Re)
True
>>> al, be, ga = euler_from_homo_matrix(Re, 'rxyz')
>>> is_same_transform(Re, homo_matrix_from_euler(al, be, ga, 'rxyz'))
True
>>> qx = quaternion_about_axis(alpha, xaxis)
>>> qy = quaternion_about_axis(beta, yaxis)
>>> qz = quaternion_about_axis(gamma, zaxis)
>>> q = quaternion_multiply(qx, qy)
>>> q = quaternion_multiply(q, qz)
>>> Rq = homo_matrix_from_quaternion(q)
>>> is_same_transform(R, Rq)
True
>>> S = scale_matrix(1.23, origin)
>>> T = translation_matrix((1, 2, 3))
>>> Z = shear_matrix(beta, xaxis, origin, zaxis)
>>> R = random_homo_matrix(np.random.rand(3))
>>> M = concatenate_matrices(T, R, Z, S)
>>> scale, shear, angles, trans, persp = decompose_matrix(M)
>>> np.allclose(scale, 1.23)
True
>>> np.allclose(trans, (1, 2, 3))
True
>>> np.allclose(shear, (0, math.tan(beta), 0))
True
>>> is_same_transform(R, homo_matrix_from_euler(axes='sxyz', *angles))
True
>>> M1 = compose_matrix(scale, shear, angles, trans, persp)
>>> is_same_transform(M, M1)
True

4.  Module Contents

4.1.  Classes

Arcball

Virtual Trackball Control.

4.2.  Functions

identity_matrix	Return 4x4 identity/unit matrix.
unit_vector	Return ndarray normalized by length, i.e. eucledian norm, along axis.
random_vector	Return array of random doubles in the half-open interval [0.0, 1.0).
random_quaternion	Return uniform random unit quaternion.
random_rot_matrix	Return uniform random rotation matrix.
random_homo_matrix	Return uniform random rotation matrix.
vector_norm	Return length, i.e. eucledian norm, of ndarray along axis.
inverse_matrix	Return inverse of square transformation matrix.
translation_matrix	Return matrix to translate by direction vector.
translation_from_matrix	Return translation vector from translation matrix.
reflection_matrix	Return matrix to mirror at plane defined by point and normal vector.
reflection_from_matrix	Return mirror plane point and normal vector from reflection matrix.
rotation_matrix	Return matrix to rotate about axis defined by point and direction.
rotation_from_matrix	Return rotation angle and axis from rotation matrix.
scale_matrix	Return matrix to scale by factor around origin in direction. Use factor -1 for point symmetry.
scale_from_matrix	Return scaling factor, origin and direction from scaling matrix.
projection_matrix	Return matrix to project onto plane defined by point and normal. Using either perspective point, projection direction, or none of both. If pseudo is True, perspective projections will preserve relative depth such that Perspective = dot(Orthogonal, PseudoPerspective).
projection_from_matrix	Return projection plane and perspective point from projection matrix. Return values are same as arguments for projection_matrix math: point, normal, direction, perspective, and pseudo.
clip_matrix	Return matrix to obtain normalized device coordinates from frustrum. The frustrum bounds are axis-aligned along x (left, right), y (bottom, top) and z (near, far). Normalized device coordinates are in range [-1, 1] if coordinates are inside the frustrum. If perspective is True the frustrum is a truncated pyramid with the perspective point at origin and direction along z axis, otherwise an orthographic canonical view volume (a box). Homogeneous coordinates transformed by the perspective clip matrix need to be dehomogenized (devided by w coordinate).
shear_matrix	Return matrix to shear by angle along direction vector on shear plane. The shear plane is defined by a point and normal vector. The direction vector must be orthogonal to the plane’s normal vector. A point P is transformed by the shear matrix into P” such that the vector P-P” is parallel to the direction vector and its extent is given by the angle of P-P’-P”, where P’ is the orthogonal projection of P onto the shear plane.
shear_from_matrix	Return shear angle, direction and plane from shear matrix.
decompose_matrix	Return sequence of transformations from transformation matrix.
compose_matrix	Return transformation matrix from sequence of transformations. This is the inverse of the decompose_matrix math.
orthogonalization_matrix	Return orthogonalization matrix for crystallographic cell coordinates. Angles are expected in degrees. The de-orthogonalization matrix is the inverse.
superimposition_matrix	Return matrix to transform given vector set into second vector set. v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 vectors. If usesvd is True, the weighted sum of squared deviations (RMSD) is minimized according to the algorithm by W. Kabsch [8]. Otherwise the quaternion based algorithm by B. Horn [9] is used (slower when using this Python implementation). The returned matrix performs rotation, translation and uniform scaling (if specified).
homo_matrix_from_euler	Return homogeneous rotation matrix (4x4) 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
homo_matrix_from_rot_matrix	Construct homogeneous matrix from rotation matrix
homo_matrix_from_quaternion	Return homogeneous rotation matrix from quaternion.
euler_from_homo_matrix	Return Euler angles from rotation matrix for specified axis sequence.
euler_from_quaternion	Return Euler angles from quaternion for specified axis sequence.
quaternion_from_euler	Return quaternion from Euler angles and axis sequence.
quaternion_about_axis	Return quaternion for rotation about axis.
quaternion_from_rot_matrix	Return quaternion from rotation matrix.
quaternion_from_homo_matrix	Return quaternion from homo matrix.
quaternion_multiply	Return multiplication of two quaternions.
quaternion_multiply_tensor	Return multiplication of two quaternions in the form of tensor.
quaternion_multiply_tensor_multirow	Return multiplication of two quaternions with multiple rows in the form of tensor.
quaternion_multiply_tensor_multirow2	Return multiplication of two quaternions with multiple rows in the form of tensor.
quaternion_conjugate	Return conjugate of quaternion.
quaternion_inverse	Return inverse of quaternion.
quaternion_slerp	Return spherical linear interpolation between two quaternions.
rot_matrix_from_euler	Return rotation matrix from quaternion.
rot_matrix_from_quaternion	Return rotation matrix from quaternion.
concatenate_matrices	Return concatenation of series of transformation matrices.
is_same_transform	Return True if two matrices perform same transformation.
check_rot_matrix	Input validation of a rotation matrix.
arcball_map_to_sphere	Return unit sphere coordinates from window coordinates.
arcball_constrain_to_axis	Return sphere point perpendicular to axis.
arcball_nearest_axis	Return axis, which arc is nearest to point.

4.3.  API

rofunc.utils.robolab.coord.transform.identity_matrix()

Return 4x4 identity/unit matrix.

>>> I = identity_matrix()
>>> np.allclose(I, np.dot(I, I))
True
>>> np.sum(I), np.trace(I)
(4.0, 4.0)
>>> np.allclose(I, np.identity(4, dtype=np.float64))
True

rofunc.utils.robolab.coord.transform.unit_vector(data, axis=None, out=None)

Return ndarray normalized by length, i.e. eucledian norm, along axis.

>>> v0 = np.random.random(3)
>>> v1 = unit_vector(v0)
>>> np.allclose(v1, v0 / np.linalg.norm(v0))
True
>>> v0 = np.random.rand(5, 4, 3)
>>> v1 = unit_vector(v0, axis=-1)
>>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=2)), 2)
>>> np.allclose(v1, v2)
True
>>> v1 = unit_vector(v0, axis=1)
>>> v2 = v0 / np.expand_dims(np.sqrt(np.sum(v0*v0, axis=1)), 1)
>>> np.allclose(v1, v2)
True
>>> v1 = np.empty((5, 4, 3), dtype=np.float64)
>>> unit_vector(v0, axis=1, out=v1)
>>> np.allclose(v1, v2)
True
>>> list(unit_vector([]))
[]
>>> list(unit_vector([1.0]))
[1.0]

Parameters:

• data – array

• axis – int

• out – array

:return nomalized data

rofunc.utils.robolab.coord.transform.random_vector(size)

Return array of random doubles in the half-open interval [0.0, 1.0).

>>> v = random_vector(10000)
>>> np.all(v >= 0.0) and np.all(v < 1.0)
True
>>> v0 = random_vector(10)
>>> v1 = random_vector(10)
>>> np.any(v0 == v1)
False

Parameters:

size – int

rofunc.utils.robolab.coord.transform.random_quaternion(rand=None)

Return uniform random unit quaternion.

Three independent random variables that are uniformly distributed between 0 and 1.

>>> q = random_quaternion()
>>> np.allclose(1.0, vector_norm(q))
True
>>> q = random_quaternion(np.random.random(3))
>>> q.shape
(4,)

Parameters:

rand – array like or None

:return quaternion

rofunc.utils.robolab.coord.transform.random_rot_matrix(rand=None)

Return uniform random rotation matrix.

Three independent random variables that are uniformly distributed between 0 and 1 for each returned quaternion.

>>> R = random_rot_matrix()
>>> np.allclose(np.dot(R.T, R), np.identity(3))
True

Parameters:

rand – array like or None

:return rotation matrix

rofunc.utils.robolab.coord.transform.random_homo_matrix(rand=None)

Return uniform random rotation matrix.

Three independent random variables that are uniformly distributed between 0 and 1 for each returned quaternion.

>>> R = random_homo_matrix()
>>> np.allclose(np.dot(R.T, R), np.identity(4))
True

Parameters:

rand – array like or None

:return homo matrix

rofunc.utils.robolab.coord.transform.vector_norm(data, axis=None, out=None)

Return length, i.e. eucledian norm, of ndarray along axis.

>>> v = np.random.random(3)
>>> n = vector_norm(v)
>>> np.allclose(n, np.linalg.norm(v))
True
>>> v = np.random.rand(6, 5, 3)
>>> n = vector_norm(v, axis=-1)
>>> np.allclose(n, np.sqrt(np.sum(v*v, axis=2)))
True
>>> n = vector_norm(v, axis=1)
>>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
True
>>> v = np.random.rand(5, 4, 3)
>>> n = np.empty((5, 3), dtype=np.float64)
>>> vector_norm(v, axis=1, out=n)
>>> np.allclose(n, np.sqrt(np.sum(v*v, axis=1)))
True
>>> vector_norm([])
0.0
>>> vector_norm([1.0])
1.0

rofunc.utils.robolab.coord.transform.inverse_matrix(matrix)

Return inverse of square transformation matrix.

>>> M0 = random_homo_matrix()
>>> M1 = inverse_matrix(M0.T)
>>> np.allclose(M1, np.linalg.inv(M0.T))
True
>>> for size in range(1, 7):
...     M0 = np.random.rand(size, size)
...     M1 = inverse_matrix(M0)
...     if not np.allclose(M1, np.linalg.inv(M0)): print size

rofunc.utils.robolab.coord.transform.translation_matrix(direction)

Return matrix to translate by direction vector.

>>> v = np.random.random(3) - 0.5
>>> np.allclose(v, translation_matrix(v)[:3, 3])
True

rofunc.utils.robolab.coord.transform.translation_from_matrix(matrix)

Return translation vector from translation matrix.

>>> v0 = np.random.random(3) - 0.5
>>> v1 = translation_from_matrix(translation_matrix(v0))
>>> np.allclose(v0, v1)
True

rofunc.utils.robolab.coord.transform.reflection_matrix(point, normal)

Return matrix to mirror at plane defined by point and normal vector.

>>> v0 = np.random.random(4) - 0.5
>>> v0[3] = 1.0
>>> v1 = np.random.random(3) - 0.5
>>> R = reflection_matrix(v0, v1)
>>> np.allclose(2., np.trace(R))
True
>>> np.allclose(v0, np.dot(R, v0))
True
>>> v2 = v0.copy()
>>> v2[:3] += v1
>>> v3 = v0.copy()
>>> v2[:3] -= v1
>>> np.allclose(v2, np.dot(R, v3))
True

rofunc.utils.robolab.coord.transform.reflection_from_matrix(matrix)

Return mirror plane point and normal vector from reflection matrix.

>>> v0 = np.random.random(3) - 0.5
>>> v1 = np.random.random(3) - 0.5
>>> M0 = reflection_matrix(v0, v1)
>>> point, normal = reflection_from_matrix(M0)
>>> M1 = reflection_matrix(point, normal)
>>> is_same_transform(M0, M1)
True

rofunc.utils.robolab.coord.transform.rotation_matrix(angle, direction, point=None)

Return matrix to rotate about axis defined by point and direction.

>>> angle = (random.random() - 0.5) * (2*math.pi)
>>> direc = np.random.random(3) - 0.5
>>> point = np.random.random(3) - 0.5
>>> R0 = rotation_matrix(angle, direc, point)
>>> R1 = rotation_matrix(angle-2*math.pi, direc, point)
>>> is_same_transform(R0, R1)
True
>>> R0 = rotation_matrix(angle, direc, point)
>>> R1 = rotation_matrix(-angle, -direc, point)
>>> is_same_transform(R0, R1)
True
>>> I = np.identity(4, np.float64)
>>> np.allclose(I, rotation_matrix(math.pi*2, direc))
True
>>> np.allclose(2., np.trace(rotation_matrix(math.pi/2,
...                                                direc, point)))
True

rofunc.utils.robolab.coord.transform.rotation_from_matrix(matrix)

Return rotation angle and axis from rotation matrix.

>>> angle = (random.random() - 0.5) * (2*math.pi)
>>> direc = np.random.random(3) - 0.5
>>> point = np.random.random(3) - 0.5
>>> R0 = rotation_matrix(angle, direc, point)
>>> angle, direc, point = rotation_from_matrix(R0)
>>> R1 = rotation_matrix(angle, direc, point)
>>> is_same_transform(R0, R1)
True

rofunc.utils.robolab.coord.transform.scale_matrix(factor, origin=None, direction=None)

Return matrix to scale by factor around origin in direction. Use factor -1 for point symmetry.

>>> v = (np.random.rand(4, 5) - 0.5) * 20.0
>>> v[3] = 1.0
>>> S = scale_matrix(-1.234)
>>> np.allclose(np.dot(S, v)[:3], -1.234*v[:3])
True
>>> factor = random.random() * 10 - 5
>>> origin = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> S = scale_matrix(factor, origin)
>>> S = scale_matrix(factor, origin, direct)

rofunc.utils.robolab.coord.transform.scale_from_matrix(matrix)

Return scaling factor, origin and direction from scaling matrix.

>>> factor = random.random() * 10 - 5
>>> origin = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> S0 = scale_matrix(factor, origin)
>>> factor, origin, direction = scale_from_matrix(S0)
>>> S1 = scale_matrix(factor, origin, direction)
>>> is_same_transform(S0, S1)
True
>>> S0 = scale_matrix(factor, origin, direct)
>>> factor, origin, direction = scale_from_matrix(S0)
>>> S1 = scale_matrix(factor, origin, direction)
>>> is_same_transform(S0, S1)
True

rofunc.utils.robolab.coord.transform.projection_matrix(point, normal, direction=None, perspective=None, pseudo=False)

Return matrix to project onto plane defined by point and normal. Using either perspective point, projection direction, or none of both. If pseudo is True, perspective projections will preserve relative depth such that Perspective = dot(Orthogonal, PseudoPerspective).

>>> P = projection_matrix((0, 0, 0), (1, 0, 0))
>>> np.allclose(P[1:, 1:], np.identity(4)[1:, 1:])
True
>>> point = np.random.random(3) - 0.5
>>> normal = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> persp = np.random.random(3) - 0.5
>>> P0 = projection_matrix(point, normal)
>>> P1 = projection_matrix(point, normal, direction=direct)
>>> P2 = projection_matrix(point, normal, perspective=persp)
>>> P3 = projection_matrix(point, normal, perspective=persp, pseudo=True)
>>> is_same_transform(P2, np.dot(P0, P3))
True
>>> P = projection_matrix((3, 0, 0), (1, 1, 0), (1, 0, 0))
>>> v0 = (np.random.rand(4, 5) - 0.5) * 20.0
>>> v0[3] = 1.0
>>> v1 = np.dot(P, v0)
>>> np.allclose(v1[1], v0[1])
True
>>> np.allclose(v1[0], 3.0-v1[1])
True

rofunc.utils.robolab.coord.transform.projection_from_matrix(matrix, pseudo=False)

Return projection plane and perspective point from projection matrix. Return values are same as arguments for projection_matrix math: point, normal, direction, perspective, and pseudo.

>>> point = np.random.random(3) - 0.5
>>> normal = np.random.random(3) - 0.5
>>> direct = np.random.random(3) - 0.5
>>> persp = np.random.random(3) - 0.5
>>> P0 = projection_matrix(point, normal)
>>> result = projection_from_matrix(P0)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, direct)
>>> result = projection_from_matrix(P0)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=False)
>>> result = projection_from_matrix(P0, pseudo=False)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True
>>> P0 = projection_matrix(point, normal, perspective=persp, pseudo=True)
>>> result = projection_from_matrix(P0, pseudo=True)
>>> P1 = projection_matrix(*result)
>>> is_same_transform(P0, P1)
True

rofunc.utils.robolab.coord.transform.clip_matrix(left, right, bottom, top, near, far, perspective=False)

Return matrix to obtain normalized device coordinates from frustrum. The frustrum bounds are axis-aligned along x (left, right), y (bottom, top) and z (near, far). Normalized device coordinates are in range [-1, 1] if coordinates are inside the frustrum. If perspective is True the frustrum is a truncated pyramid with the perspective point at origin and direction along z axis, otherwise an orthographic canonical view volume (a box). Homogeneous coordinates transformed by the perspective clip matrix need to be dehomogenized (devided by w coordinate).

>>> frustrum = np.random.rand(6)
>>> frustrum[1] += frustrum[0]
>>> frustrum[3] += frustrum[2]
>>> frustrum[5] += frustrum[4]
>>> M = clip_matrix(*frustrum, perspective=False)
>>> np.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
array([-1., -1., -1.,  1.])
>>> np.dot(M, [frustrum[1], frustrum[3], frustrum[5], 1.0])
array([ 1.,  1.,  1.,  1.])
>>> M = clip_matrix(*frustrum, perspective=True)
>>> v = np.dot(M, [frustrum[0], frustrum[2], frustrum[4], 1.0])
>>> v / v[3]
array([-1., -1., -1.,  1.])
>>> v = np.dot(M, [frustrum[1], frustrum[3], frustrum[4], 1.0])
>>> v / v[3]
array([ 1.,  1., -1.,  1.])

rofunc.utils.robolab.coord.transform.shear_matrix(angle, direction, point, normal)

Return matrix to shear by angle along direction vector on shear plane. The shear plane is defined by a point and normal vector. The direction vector must be orthogonal to the plane’s normal vector. A point P is transformed by the shear matrix into P” such that the vector P-P” is parallel to the direction vector and its extent is given by the angle of P-P’-P”, where P’ is the orthogonal projection of P onto the shear plane.

>>> angle = (random.random() - 0.5) * 4*math.pi
>>> direct = np.random.random(3) - 0.5
>>> point = np.random.random(3) - 0.5
>>> normal = np.cross(direct, np.random.random(3))
>>> S = shear_matrix(angle, direct, point, normal)
>>> np.allclose(1.0, np.linalg.det(S))
True

rofunc.utils.robolab.coord.transform.shear_from_matrix(matrix)

Return shear angle, direction and plane from shear matrix.

>>> angle = (random.random() - 0.5) * 4*math.pi
>>> direct = np.random.random(3) - 0.5
>>> point = np.random.random(3) - 0.5
>>> normal = np.cross(direct, np.random.random(3))
>>> S0 = shear_matrix(angle, direct, point, normal)
>>> angle, direct, point, normal = shear_from_matrix(S0)
>>> S1 = shear_matrix(angle, direct, point, normal)
>>> is_same_transform(S0, S1)
True

rofunc.utils.robolab.coord.transform.decompose_matrix(matrix)

Return sequence of transformations from transformation matrix.

>>> T0 = translation_matrix((1, 2, 3))
>>> scale, shear, angles, trans, persp = decompose_matrix(T0)
>>> T1 = translation_matrix(trans)
>>> np.allclose(T0, T1)
True
>>> S = scale_matrix(0.123)
>>> scale, shear, angles, trans, persp = decompose_matrix(S)
>>> scale[0]
0.123
>>> R0 = homo_matrix_from_euler(1, 2, 3)
>>> scale, shear, angles, trans, persp = decompose_matrix(R0)
>>> R1 = homo_matrix_from_euler(*angles)
>>> np.allclose(R0, R1)
True

Parameters:

matrix – array_like, Non-degenerative homogeneous transformation matrix

Return scale:

vector of 3 scaling factors

Return shear:

list of shear factors for x-y, x-z, y-z axes

Return angles:

list of Euler angles about static x, y, z axes

Return translate:

translation vector along x, y, z axes

Return perspective:

perspective partition of matrix

rofunc.utils.robolab.coord.transform.compose_matrix(scale=None, shear=None, angles=None, translate=None, perspective=None)

Return transformation matrix from sequence of transformations. This is the inverse of the decompose_matrix math.

>>> scale = np.random.random(3) - 0.5
>>> shear = np.random.random(3) - 0.5
>>> angles = (np.random.random(3) - 0.5) * (2*math.pi)
>>> trans = np.random.random(3) - 0.5
>>> persp = np.random.random(4) - 0.5
>>> M0 = compose_matrix(scale, shear, angles, trans, persp)
>>> result = decompose_matrix(M0)
>>> M1 = compose_matrix(*result)
>>> is_same_transform(M0, M1)
True

Parameters:

• scale – vector of 3 scaling factors

• shear – list of shear factors for x-y, x-z, y-z axes

• angles – list of Euler angles about static x, y, z axes

• translate – translation vector along x, y, z axes

• perspective – perspective partition of matrix

rofunc.utils.robolab.coord.transform.orthogonalization_matrix(lengths, angles)

Return orthogonalization matrix for crystallographic cell coordinates. Angles are expected in degrees. The de-orthogonalization matrix is the inverse.

>>> O = orthogonalization_matrix((10., 10., 10.), (90., 90., 90.))
>>> np.allclose(O[:3, :3], np.identity(3, float) * 10)
True
>>> O = orthogonalization_matrix([9.8, 12.0, 15.5], [87.2, 80.7, 69.7])
>>> np.allclose(np.sum(O), 43.063229)
True

rofunc.utils.robolab.coord.transform.superimposition_matrix(v0, v1, scaling=False, usesvd=True)

Return matrix to transform given vector set into second vector set. v0 and v1 are shape (3, *) or (4, *) arrays of at least 3 vectors. If usesvd is True, the weighted sum of squared deviations (RMSD) is minimized according to the algorithm by W. Kabsch [8]. Otherwise the quaternion based algorithm by B. Horn [9] is used (slower when using this Python implementation). The returned matrix performs rotation, translation and uniform scaling (if specified).

>>> v0 = np.random.rand(3, 10)
>>> M = superimposition_matrix(v0, v0)
>>> np.allclose(M, np.identity(4))
True
>>> R = random_homo_matrix(np.random.random(3))
>>> v0 = ((1,0,0), (0,1,0), (0,0,1), (1,1,1))
>>> v1 = np.dot(R, v0)
>>> M = superimposition_matrix(v0, v1)
>>> np.allclose(v1, np.dot(M, v0))
True
>>> v0 = (np.random.rand(4, 100) - 0.5) * 20.0
>>> v0[3] = 1.0
>>> v1 = np.dot(R, v0)
>>> M = superimposition_matrix(v0, v1)
>>> np.allclose(v1, np.dot(M, v0))
True
>>> S = scale_matrix(random.random())
>>> T = translation_matrix(np.random.random(3)-0.5)
>>> M = concatenate_matrices(T, R, S)
>>> v1 = np.dot(M, v0)
>>> v0[:3] += np.random.normal(0.0, 1e-9, 300).reshape(3, -1)
>>> M = superimposition_matrix(v0, v1, scaling=True)
>>> np.allclose(v1, np.dot(M, v0))
True
>>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
>>> np.allclose(v1, np.dot(M, v0))
True
>>> v = np.empty((4, 100, 3), dtype=np.float64)
>>> v[:, :, 0] = v0
>>> M = superimposition_matrix(v0, v1, scaling=True, usesvd=False)
>>> np.allclose(v1, np.dot(M, v[:, :, 0]))
True

rofunc.utils.robolab.coord.transform.homo_matrix_from_euler(ai, aj, ak, axes='sxyz', translation=None)

Return homogeneous rotation matrix (4x4) 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

>>> R = homo_matrix_from_euler(1, 2, 3, 'syxz')
>>> np.allclose(np.sum(R[0]), -1.34786452)
True
>>> R = homo_matrix_from_euler(1, 2, 3, (0, 1, 0, 1))
>>> np.allclose(np.sum(R[0]), -0.383436184)
True
>>> ai, aj, ak = (4.0*math.pi) * (np.random.random(3) - 0.5)
>>> for axes in _AXES2TUPLE.keys():
...    R = homo_matrix_from_euler(ai, aj, ak, axes)
>>> for axes in _TUPLE2AXES.keys():
...    R = homo_matrix_from_euler(ai, aj, ak, axes)

rofunc.utils.robolab.coord.transform.homo_matrix_from_rot_matrix(rot_matrix, translation=None)

Construct homogeneous matrix from rotation matrix

>>> R = random_rot_matrix()
>>> T = homo_matrix_from_rot_matrix(R, [1, 2, 3])

Parameters:

• rot_matrix – R -> [3, 3] array

• translation – p -> [3, ] array

rofunc.utils.robolab.coord.transform.homo_matrix_from_quaternion(quaternion, translation=None)

Return homogeneous rotation matrix from quaternion.

>>> R = homo_matrix_from_quaternion([0.06146124, 0, 0, 0.99810947])
>>> np.allclose(R, rotation_matrix(0.123, (1, 0, 0)))
True

rofunc.utils.robolab.coord.transform.euler_from_homo_matrix(homo_matrix, axes='sxyz')

Return Euler angles from rotation matrix for specified axis sequence.

axes : One of 24 axis sequences as string or encoded tuple Note that many Euler angle triplets can describe one matrix. >>> R0 = homo_matrix_from_euler(1, 2, 3, ‘syxz’) >>> al, be, ga = euler_from_homo_matrix(R0, ‘syxz’) >>> R1 = homo_matrix_from_euler(al, be, ga, ‘syxz’) >>> np.allclose(R0, R1) True >>> angles = (4.0*math.pi) * (np.random.random(3) - 0.5) >>> for axes in _AXES2TUPLE.keys(): … R0 = homo_matrix_from_euler(axes=axes, *angles) … R1 = homo_matrix_from_euler(axes=axes, *euler_from_homo_matrix(R0, axes)) … if not np.allclose(R0, R1): print axes, “failed”

rofunc.utils.robolab.coord.transform.euler_from_quaternion(quaternion, axes='sxyz')

Return Euler angles from quaternion for specified axis sequence.

>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
>>> np.allclose(angles, [0.123, 0, 0])
True

rofunc.utils.robolab.coord.transform.quaternion_from_euler(ai, aj, ak, axes='sxyz')

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 >>> q = quaternion_from_euler(1, 2, 3, ‘ryxz’) >>> np.allclose(q, [0.310622, -0.718287, 0.444435, 0.435953]) True

rofunc.utils.robolab.coord.transform.quaternion_about_axis(angle, axis)

Return quaternion for rotation about axis.

>>> q = quaternion_about_axis(0.123, (1, 0, 0))
>>> np.allclose(q, [0.06146124, 0, 0, 0.99810947])
True

rofunc.utils.robolab.coord.transform.quaternion_from_rot_matrix(rot_matrix)

Return quaternion from rotation matrix.

>>> R = random_rot_matrix()
>>> q = quaternion_from_rot_matrix(R)
>>> np.allclose(R, rot_matrix_from_quaternion(q))
True

Parameters:

rot_matrix – [3, 3] array

Returns:

[4, ] array

rofunc.utils.robolab.coord.transform.quaternion_from_homo_matrix(homo_matrix)

Return quaternion from homo matrix.

>>> R = rotation_matrix(0.123, (1, 2, 3))
>>> q = quaternion_from_homo_matrix(R)
>>> np.allclose(q, [0.0164262, 0.0328524, 0.0492786, 0.9981095])
True

rofunc.utils.robolab.coord.transform.quaternion_multiply(quaternion1, quaternion0)

Return multiplication of two quaternions.

>>> q = quaternion_multiply([1, -2, 3, 4], [-5, 6, 7, 8])
>>> np.allclose(q, [-44, -14, 48, 28])
True

rofunc.utils.robolab.coord.transform.quaternion_multiply_tensor(quaternion1, quaternion0)

Return multiplication of two quaternions in the form of tensor.

>>> q = quaternion_multiply_tensor([1, -2, 3, 4], [-5, 6, 7, 8])
>>> np.allclose(q, [[-44, -14, 48, 28]])
True

rofunc.utils.robolab.coord.transform.quaternion_multiply_tensor_multirow(quaternion1, quaternion0)

Return multiplication of two quaternions with multiple rows in the form of tensor.

>>> q = quaternion_multiply_tensor([1, -2, 3, 4], [-5, 6, 7, 8])
>>> np.allclose(q, [[-44, -14, 48, 28]])
True

rofunc.utils.robolab.coord.transform.quaternion_multiply_tensor_multirow2(quaternion1, quaternion0)

Return multiplication of two quaternions with multiple rows in the form of tensor.

>>> q = quaternion_multiply_tensor([1, -2, 3, 4], [-5, 6, 7, 8])
>>> np.allclose(q, [[-44, -14, 48, 28]])
True

rofunc.utils.robolab.coord.transform.quaternion_conjugate(quaternion)

Return conjugate of quaternion.

>>> q0 = random_quaternion()
>>> q1 = quaternion_conjugate(q0)
>>> q1[3] == q0[3] and all(q1[:3] == -q0[:3])
True

rofunc.utils.robolab.coord.transform.quaternion_inverse(quaternion)

Return inverse of quaternion.

>>> q0 = random_quaternion()
>>> q1 = quaternion_inverse(q0)
>>> np.allclose(quaternion_multiply(q0, q1), [0, 0, 0, 1])
True

rofunc.utils.robolab.coord.transform.quaternion_slerp(quat0, quat1, fraction, spin=0, shortestpath=True)

Return spherical linear interpolation between two quaternions.

>>> q0 = random_quaternion()
>>> q1 = random_quaternion()
>>> q = quaternion_slerp(q0, q1, 0.0)
>>> np.allclose(q, q0)
True
>>> q = quaternion_slerp(q0, q1, 1.0, 1)
>>> np.allclose(q, q1)
True
>>> q = quaternion_slerp(q0, q1, 0.5)
>>> angle = math.acos(np.dot(q0, q))
>>> np.allclose(2.0, math.acos(np.dot(q0, q1)) / angle) or         np.allclose(2.0, math.acos(-np.dot(q0, q1)) / angle)
True

rofunc.utils.robolab.coord.transform.rot_matrix_from_euler(ai, aj, ak, axes='sxyz')

Return rotation matrix from quaternion.

>>> R = rot_matrix_from_euler(1, 2, 3, 'syxz')

rofunc.utils.robolab.coord.transform.rot_matrix_from_quaternion(quaternion)

Return rotation matrix from quaternion.

>>> R = rot_matrix_from_quaternion([0.06146124, 0, 0, 0.99810947])

rofunc.utils.robolab.coord.transform.concatenate_matrices(*matrices)

Return concatenation of series of transformation matrices.

>>> M = np.random.rand(16).reshape((4, 4)) - 0.5
>>> np.allclose(M, concatenate_matrices(M))
True
>>> np.allclose(np.dot(M, M.T), concatenate_matrices(M, M.T))
True

rofunc.utils.robolab.coord.transform.is_same_transform(matrix0, matrix1)

Return True if two matrices perform same transformation.

>>> is_same_transform(np.identity(4), np.identity(4))
True
>>> is_same_transform(np.identity(4), random_homo_matrix())
False

rofunc.utils.robolab.coord.transform.check_rot_matrix(R, tolerance=1e-06, strict_check=True)

Input validation of a rotation matrix.

We check whether R multiplied by its inverse is approximately the identity matrix

\[\boldsymbol{R}\boldsymbol{R}^T = \boldsymbol{I}\]

and whether the determinant is positive

\[det(\boldsymbol{R}) > 0\]

Rarray-like, shape (3, 3)

Rotation matrix

tolerancefloat, optional (default: 1e-6)

Tolerance threshold for checks. Default tolerance is the same as in assert_rotation_matrix(R).

strict_checkbool, optional (default: True)

Raise a ValueError if the rotation matrix is not numerically close enough to a real rotation matrix. Otherwise we print a warning.

Rarray, shape (3, 3)

Validated rotation matrix

ValueError

If input is invalid

class rofunc.utils.robolab.coord.transform.Arcball(initial=None)

Bases: object

Virtual Trackball Control.

>>> ball = Arcball()
>>> ball = Arcball(initial=np.identity(4))
>>> ball.place([320, 320], 320)
>>> ball.down([500, 250])
>>> ball.drag([475, 275])
>>> R = ball.matrix()
>>> np.allclose(np.sum(R), 3.90583455)
True
>>> ball = Arcball(initial=[0, 0, 0, 1])
>>> ball.place([320, 320], 320)
>>> ball.setaxes([1,1,0], [-1, 1, 0])
>>> ball.setconstrain(True)
>>> ball.down([400, 200])
>>> ball.drag([200, 400])
>>> R = ball.matrix()
>>> np.allclose(np.sum(R), 0.2055924)
True
>>> ball.next()

Initialization

Initialize virtual trackball control. initial : quaternion or rotation matrix

place(center, radius)

Place Arcball, e.g. when window size changes. center : sequence[2]

Window coordinates of trackball center.

radiusfloat

Radius of trackball in window coordinates.

setaxes(*axes)

Set axes to constrain rotations.

setconstrain(constrain)

Set state of constrain to axis mode.

getconstrain()

Return state of constrain to axis mode.

down(point)

Set initial cursor window coordinates and pick constrain-axis.

drag(point)

Update current cursor window coordinates.

next(acceleration=0.0)

Continue rotation in direction of last drag.

matrix()

Return homogeneous rotation matrix.

rofunc.utils.robolab.coord.transform.arcball_map_to_sphere(point, center, radius)

Return unit sphere coordinates from window coordinates.

rofunc.utils.robolab.coord.transform.arcball_constrain_to_axis(point, axis)

Return sphere point perpendicular to axis.

rofunc.utils.robolab.coord.transform.arcball_nearest_axis(point, axes)

Return axis, which arc is nearest to point.