Euler angles to rotation matrices and vice versa

This Python module provides utility functions for converting between 4x4 homogeneous transformation matrices and Euler angles, supporting all six rotation orders ('xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx') for body-axis rotations. The two primary functions are `rotation()`, which generates a rotation matrix from three angle values, and `inverse_rotation()`, which extracts two possible sets of Euler angles from a pure rotation matrix. The module also includes a Jacobian function for computing the derivative of the rotation with respect to the angle parameters.

"""Euler-angle to matrix and matrix to Euler-angle utility routines This module provides utility routines to convert between 4x4 homogeneous transformation matrices and Euler angles and vice versa . It supports all six rotation orders 'xyz','xzy','yxz','yzx','zxy','zyx' for rotations around body axes. Two primary functions are provided: - rotation theta, order='xyz' : given input rotation angles in radians and order of rotations, return the corresponding 4x4 rotation matrix - inverse rotation M, order='xyz' : given a matrix with a pure rotation in the upper-left 3x3 submatrix, return the two sets of Euler angles that reproduce the rotation matrix see Notes The transformation matrices occupy the upper-left 3x3 submatrix of the 4x4 output. The transformations are suitable for column-vector homogeneous points, e.g.: res = rotation theta x,theta y,theta z , order='zyx' @np.array x,y,z,1 transforms the point x,y,z by rotating first around the z axis, then around the y axis and finally around the x axis. Note that the order of the theta argument is always theta x, theta y, theta z regardless of the order argument. An additional function is provided to compute the Jacobian of the rotation w.r.t. the angle parameters. It returns a 4x4x3 ndarray, where the third axis indexes the angles. E.g.: p = np.random.uniform size=3 ,1 theta = np.random.uniform -np.pi,np.pi,3 w = euler.rotation theta, order='xzy' @p J = euler.rotation jacobian theta, order='xzy' dwdthetax = J :,:,0 @p dwdthetay = J :,:,1 @p dwdthetaz = J :,:,2 @p Arguments: - theta 3-element sequence : rotation angles in radians, packed as theta x, theta y, theta z regardless of the order parameter - order one of 'xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx' indicating the order in which rotations are applied to points, read left-to-right 'yzx' rotates by y first, then z and finally x - M at least 3x3 ndarray : matrix from which to extract angles, must be a pure rotation in a right-handed coordinate system Raises: - ValueError if matrices input to inverse rotation do not have determinant equal to 1 or have M :3,:3 .T@M :3,:3 == I to a high tolerance Notes: - Two sets of angles are returned by inverse rotation, both reproduce the the rotation matrix. First set has the 'middle' angle in range -pi/2,pi/2 and remaining axes in -pi,pi , second result subtracts the middle angle from pi and recomputes the remaining angles. Applications should pick the appropriate return value based on either legal range of angles or proximity to known values. - The order of axes in the 'order' argument and the order in which matrices are multiplied is reversed, e.g. order 'xyz' results in: res = Rz theta z Ry theta y Rx theta x @np.array x,y,z,1 The order argument consequently specifies the order that rotations are applied, rather than the order of multiplication. """ import numpy as np def rotation theta, order='xyz' : """Return the 4x4 homogeneous transform for a given triplet of angles in radians and rotation order See module docs for more info """ if order == 'xyz': return rotation xyz theta elif order == 'xzy': return rotation xzy theta elif order == 'yxz': return rotation yxz theta elif order == 'yzx': return rotation yzx theta elif order == 'zxy': return rotation zxy theta elif order == 'zyx': return rotation zyx theta raise ValueError 'Invalid rotation order {}'.format order def inverse rotation M, order='xyz' : """Return two sets of Euler angles in radians that reproduce a given at least 3x3 rotation matrix according to the target rotation order See module docs for more info """ if order == 'xyz': return inverse rotation xyz M elif order == 'xzy': return inverse rotation xzy M elif order == 'yxz': return inverse rotation yxz M elif order == 'yzx': return inverse rotation yzx M elif order == 'zxy': return inverse rotation zxy M elif order == 'zyx': return inverse rotation zyx M raise ValueError 'Invalid rotation order {}'.format order def rotation jacobian theta, order='xyz' : """Returns the 4x4x3 Jacobian of the rotation by the given angles with specified order See module docs for more info """ if order == 'xyz': return rotation jacobian xyz theta elif order == 'xzy': return rotation jacobian xzy theta elif order == 'yxz': return rotation jacobian yxz theta elif order == 'yzx': return rotation jacobian yzx theta elif order == 'zxy': return rotation jacobian zxy theta elif order == 'zyx': return rotation jacobian zyx theta def rotation xyz theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz, -cx sz + cz sx sy, cx cz sy + sx sz, 0 , cy sz, cx cz + sx sy sz, cx sy sz - cz sx, 0 , -sy, cy sx, cx cy, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation xyz M, thresh=0.9999999 : check rotation matrix M if np.abs M 2,0 thresh: sy = -np.sign M 2,0 y0 = sy np.pi/2 arbitrarily set z=0 z0 = 0 so sz=0, cz=1 compute x = arctan2 M 0,1 /sy, M 02 /sy x0 = np.arctan2 M 0,1 /sy, M 0,2 /sy return np.array x0,y0,z0 , np.array x0,y0,z0 else: y0 = np.arcsin -M 2,0 y1 = np.pi - y0 c0 = np.cos y0 c1 = np.cos y1 x0 = np.arctan2 M 2,1 /c0, M 2,2 /c0 x1 = np.arctan2 M 2,1 /c1, M 2,2 /c1 z0 = np.arctan2 M 1,0 /c0, M 0,0 /c0 z1 = np.arctan2 M 1,0 /c1, M 0,0 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def rotation xzy theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz, -cx cy sz + sx sy, cx sy + cy sx sz, 0 , sz, cx cz, -cz sx, 0 , -cz sy, cx sy sz + cy sx, cx cy - sx sy sz, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation xzy M, thresh=0.9999999 : check rotation matrix M if np.abs M 1,0 thresh: sz = np.sign M 1,0 z0 = sz np.pi/2 arbitrarily set z=0 y0 = 0 so sy=0, cy=1 compute x = arctan2 M 0,1 /sy, M 02 /sy x0 = np.arctan2 M 0,2 /sz, -M 0,1 /sz return np.array x0,y0,z0 , np.array x0,y0,z0 else: z0 = np.arcsin M 1,0 z1 = np.pi - z0 c0 = np.cos z0 c1 = np.cos z1 x0 = np.arctan2 -M 1,2 /c0, M 1,1 /c0 x1 = np.arctan2 -M 1,2 /c1, M 1,1 /c1 y0 = np.arctan2 -M 2,0 /c0, M 0,0 /c0 y1 = np.arctan2 -M 2,0 /c1, M 0,0 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def rotation yxz theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz - sx sy sz, -cx sz, cy sx sz + cz sy, 0 , cy sz + cz sx sy, cx cz, -cy cz sx + sy sz, 0 , -cx sy, sx, cx cy, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation yxz M, thresh=0.9999999 : check rotation matrix M if np.abs M 2,1 thresh: sx = np.sign M 2,1 x0 = sx np.pi/2 arbitrarily set z=0 z0 = 0 so sz=0, cz=1 compute x = arctan2 M 0,1 /sy, M 02 /sy y0 = np.arctan2 M 1,0 /sx, -M 1,2 /sx return np.array x0,y0,z0 , np.array x0,y0,z0 else: x0 = np.arcsin M 2,1 x1 = np.pi - x0 c0 = np.cos x0 c1 = np.cos x1 y0 = np.arctan2 -M 2,0 /c0, M 2,2 /c0 y1 = np.arctan2 -M 2,0 /c1, M 2,2 /c1 z0 = np.arctan2 -M 0,1 /c0, M 1,1 /c0 z1 = np.arctan2 -M 0,1 /c1, M 1,1 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def rotation yzx theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz, -sz, cz sy, 0 , cx cy sz + sx sy, cx cz, cx sy sz - cy sx, 0 , -cx sy + cy sx sz, cz sx, cx cy + sx sy sz, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation yzx M, thresh=0.9999999 : check rotation matrix M if np.abs M 0,1 thresh: sz = np.sign -M 0,1 z0 = sz np.pi/2 arbitrarily set z=0 x0 = 0 so sx=0, cx=1 compute x = arctan2 M 0,1 /sy, M 02 /sy y0 = np.arctan2 M 1,2 /sz, M 1,0 /sz return np.array x0,y0,z0 , np.array x0,y0,z0 else: z0 = np.arcsin -M 0,1 z1 = np.pi - z0 c0 = np.cos z0 c1 = np.cos z1 y0 = np.arctan2 M 0,2 /c0, M 0,0 /c0 y1 = np.arctan2 M 0,2 /c1, M 0,0 /c1 x0 = np.arctan2 M 2,1 /c0, M 1,1 /c0 x1 = np.arctan2 M 2,1 /c1, M 1,1 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def rotation zxy theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz + sx sy sz, -cy sz + cz sx sy, cx sy, 0 , cx sz, cx cz, -sx, 0 , cy sx sz - cz sy, cy cz sx + sy sz, cx cy, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation zxy M, thresh=0.9999999 : check rotation matrix M if np.abs M 1,2 thresh: sx = np.sign -M 1,2 x0 = sx np.pi/2 arbitrarily set z=0 y0 = 0 so sy=0, cy=1 compute x = arctan2 M 0,1 /sy, M 02 /sy z0 = np.arctan2 M 2,0 /sx, M 2,1 /sx return np.array x0,y0,z0 , np.array x0,y0,z0 else: x0 = np.arcsin -M 1,2 x1 = np.pi - x0 c0 = np.cos x0 c1 = np.cos x1 y0 = np.arctan2 M 0,2 /c0, M 2,2 /c0 y1 = np.arctan2 M 0,2 /c1, M 2,2 /c1 z0 = np.arctan2 M 1,0 /c0, M 1,1 /c0 z1 = np.arctan2 M 1,0 /c1, M 1,1 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def rotation zyx theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta return np.array cy cz, -cy sz, sy, 0 , cx sz + cz sx sy, cx cz - sx sy sz, -cy sx, 0 , -cx cz sy + sx sz, cx sy sz + cz sx, cx cy, 0 , 0, 0, 0, 1 ,dtype=float def inverse rotation zyx M, thresh=0.9999999 : check rotation matrix M if np.abs M 0,2 thresh: sy = np.sign M 0,2 y0 = sy np.pi/2 arbitrarily set z=0 x0 = 0 so sx=0, cx=1 compute x = arctan2 M 0,1 /sy, M 02 /sy z0 = np.arctan2 M 2,1 /sy, -M 2,0 /sy return np.array x0,y0,z0 , np.array x0,y0,z0 else: y0 = np.arcsin M 0,2 y1 = np.pi - y0 c0 = np.cos y0 c1 = np.cos y1 x0 = np.arctan2 -M 1,2 /c0, M 2,2 /c0 x1 = np.arctan2 -M 1,2 /c1, M 2,2 /c1 z0 = np.arctan2 -M 0,1 /c0, M 0,0 /c0 z1 = np.arctan2 -M 0,1 /c1, M 0,0 /c1 return np.array x0,y0,z0 , np.array x1,y1,z1 def check rotation matrix M, orth thresh=1e-5, det thresh=1e-5 : if np.linalg.norm M :3,:3 .T@M :3,:3 - np.eye 3 orth thresh: raise ValueError 'Input matrix is not a pure rotation' if np.abs np.linalg.det M :3,:3 -1.0 det thresh: raise ValueError 'Input matrix is not a pure rotation' def rotation jacobian xyz theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array 0, -dcx sz + cz dsx sy, dcx cz sy + dsx sz, 0 , 0, dcx cz + dsx sy sz, dcx sy sz - cz dsx, 0 , 0, cy dsx, dcx cy, 0 , 0, 0, 0, 0 , dcy cz, cz sx dsy, cx cz dsy, 0 , dcy sz, sx dsy sz, cx dsy sz, 0 , -dsy, dcy sx, cx dcy, 0 , 0, 0, 0, 0 , cy dcz, -cx dsz + dcz sx sy, cx dcz sy + sx dsz, 0 , cy dsz, cx dcz + sx sy dsz, cx sy dsz - dcz sx, 0 , 0, 0, 0, 0 , 0, 0, 0, 0 , 1,2,0 def rotation jacobian xzy theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array 0, -dcx cy sz + dsx sy, dcx sy + cy dsx sz, 0 , 0, dcx cz, -cz dsx, 0 , 0, dcx sy sz + cy dsx, dcx cy - dsx sy sz, 0 , 0, 0, 0, 0 , dcy cz, -cx dcy sz + sx dsy, cx dsy + dcy sx sz, 0 , 0, 0, 0, 0 , -cz dsy, cx dsy sz + dcy sx, cx dcy - sx dsy sz, 0 , 0, 0, 0, 0 , cy dcz, -cx cy dsz, cy sx dsz, 0 , dsz, cx dcz, -dcz sx, 0 , -dcz sy, cx sy dsz, -sx sy dsz, 0 , 0, 0, 0, 0 , 1,2,0 def rotation jacobian yxz theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array -dsx sy sz, -dcx sz, cy dsx sz, 0 , cz dsx sy, dcx cz, -cy cz dsx, 0 , -dcx sy, dsx, dcx cy, 0 , 0, 0, 0, 0 , dcy cz - sx dsy sz, 0, dcy sx sz + cz dsy, 0 , dcy sz + cz sx dsy, 0, -dcy cz sx + dsy sz, 0 , -cx dsy, 0, cx dcy, 0 , 0, 0, 0, 0 , cy dcz - sx sy dsz, -cx dsz, cy sx dsz + dcz sy, 0 , cy dsz + dcz sx sy, cx dcz, -cy dcz sx + sy dsz, 0 , 0, 0, 0, 0 , 0, 0, 0, 0 , 1,2,0 def rotation jacobian yzx theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array 0, 0, 0, 0 , dcx cy sz + dsx sy, dcx cz, dcx sy sz - cy dsx, 0 , -dcx sy + cy dsx sz, cz dsx, dcx cy + dsx sy sz, 0 , 0, 0, 0, 0 , dcy cz, 0, cz dsy, 0 , cx dcy sz + sx dsy, 0, cx dsy sz - dcy sx, 0 , -cx dsy + dcy sx sz, 0, cx dcy + sx dsy sz, 0 , 0, 0, 0, 0 , cy dcz, -dsz, dcz sy, 0 , cx cy dsz, cx dcz, cx sy dsz, 0 , cy sx dsz, dcz sx, sx sy dsz, 0 , 0, 0, 0, 0 , 1,2,0 def rotation jacobian zxy theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array dsx sy sz, cz dsx sy, dcx sy, 0 , dcx sz, dcx cz, -dsx, 0 , cy dsx sz, cy cz dsx, dcx cy, 0 , 0, 0, 0, 0 , dcy cz + sx dsy sz, -dcy sz + cz sx dsy, cx dsy, 0 , 0, 0, 0, 0 , dcy sx sz - cz dsy, dcy cz sx + dsy sz, cx dcy, 0 , 0, 0, 0, 0 , cy dcz + sx sy dsz, -cy dsz + dcz sx sy, 0, 0 , cx dsz, cx dcz, 0, 0 , cy sx dsz - dcz sy, cy dcz sx + sy dsz, 0, 0 , 0, 0, 0, 0 , 1,2,0 def rotation jacobian zyx theta : cx,cy,cz = np.cos theta sx,sy,sz = np.sin theta dcx,dsx = -sx,cx dcy,dsy = -sy,cy dcz,dsz = -sz,cz return np.transpose np.array 0, 0, 0, 0 , dcx sz + cz dsx sy, dcx cz - dsx sy sz, -cy dsx, 0 , -dcx cz sy + dsx sz, dcx sy sz + cz dsx, dcx cy, 0 , 0, 0, 0, 0 , dcy cz, -dcy sz, dsy, 0 , cz sx dsy, -sx dsy sz, -dcy sx, 0 , -cx cz dsy, cx dsy sz, cx dcy, 0 , 0, 0, 0, 0 , cy dcz, -cy dsz, 0, 0 , cx dsz + dcz sx sy, cx dcz - sx sy dsz, 0, 0 , -cx dcz sy + sx dsz, cx sy dsz + dcz sx, 0, 0 , 0, 0, 0, 0 , 1,2,0