# Euler angles to rotation matrices and vice versa

> Source: <https://gist.github.com/jamesgregson/67eb5509af0d8b372f25146d5e3c5149>
> Published: 2019-09-05 01:19:45+00:00

"""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) )