"""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
z0 = 0 # so sz=0, cz=1
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
y0 = 0 # so sy=0, cy=1
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
z0 = 0 # so sz=0, cz=1
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
x0 = 0 # so sx=0, cx=1
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
y0 = 0 # so sy=0, cy=1
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
x0 = 0 # so sx=0, cx=1
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) )
source & further reading
gist.github.com — original article
agent-proxy — a zero-dep proxy that shows the bloat in Claude Code's requests (ranked tool table + full readable Markdown of every request)
PowerShell profile and test suite
A plan-of-plans for a production-grade end-to-end e-commerce website.