Last active
September 6, 2019 13:26
-
-
Save jamesgregson/67eb5509af0d8b372f25146d5e3c5149 to your computer and use it in GitHub Desktop.
Euler angles to rotation matrices and vice versa
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
"""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) ) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
import numpy as np | |
from numpy.random import uniform | |
import unittest | |
from euler import * | |
class TestEuler(unittest.TestCase): | |
def __check_result( self, name, rot_func, inv_func, theta, rtol=1e-3, atol=1e-3, check_theta=True ): | |
R = rot_func( theta ) | |
theta0, theta1 = inv_func( R ) | |
R0 = rot_func( theta0 ) | |
R1 = rot_func( theta1 ) | |
if (not np.allclose(R,R0,rtol,atol)) or (not np.allclose(R,R1,rtol,atol)): | |
print('name: {}'.format(name) ) | |
print('Theta: {}'.format(theta) ) | |
print('Theta0: {}'.format(theta0) ) | |
print('Theta1: {}'.format(theta1) ) | |
print('R:\n{}'.format(R) ) | |
print('R0:\n{}'.format(R0) ) | |
print('R1:\n{}'.format(R1) ) | |
self.assertTrue( False ) | |
if check_theta: | |
if not np.allclose(theta,theta0,rtol,atol): | |
if not np.allclose(theta,theta1,rtol,atol): | |
print('name: {}'.format(name) ) | |
print('Theta: {}'.format(theta) ) | |
print('Theta0: {}'.format(theta0) ) | |
print('Theta1: {}'.format(theta1) ) | |
self.assertTrue( False ) | |
def __check_order( self, order, rot_func, inv_func, N=10000, s=0.99 ): | |
a2i = {'x': 0, 'y': 1, 'z': 2 } | |
axes = (a2i[order[0]],a2i[order[1]],a2i[order[2]]) | |
scale = [1.0,1.0,1.0] | |
scale[a2i[order[1]]] = 0.5 | |
xrange = np.array((-np.pi*scale[0], np.pi*scale[0])) | |
yrange = np.array((-np.pi*scale[1], np.pi*scale[1])) | |
zrange = np.array((-np.pi*scale[2], np.pi*scale[2])) | |
ranges = [ xrange, yrange, zrange ] | |
# check the central rotation axis range specifically | |
for i in range(N): | |
# fix the third axis to zero and ensure angles match | |
theta = np.array([uniform(*xrange)*s,uniform(*yrange)*s,uniform(*zrange)*s]) | |
theta[axes[1]] = ranges[axes[1]][0] | |
theta[axes[2]] = 0.0 | |
self.__check_result( order, rot_func, inv_func, theta ) | |
# unfix the third axis to zero and ensure matrices match | |
theta = np.array([uniform(*xrange)*s,uniform(*yrange)*s,uniform(*zrange)*s]) | |
theta[axes[1]] = ranges[axes[1]][0] | |
self.__check_result( order, rot_func, inv_func, theta, check_theta=False ) | |
# fix the third axis to zero and ensure angles match | |
theta = np.array([uniform(*xrange)*s,uniform(*yrange)*s,uniform(*zrange)*s]) | |
theta[axes[1]] = ranges[axes[1]][1] | |
theta[axes[2]] = 0.0 | |
self.__check_result( order, rot_func, inv_func, theta ) | |
# unfix the third axis to zero and ensure matrices match | |
theta = np.array([uniform(*xrange)*s,uniform(*yrange)*s,uniform(*zrange)*s]) | |
theta[axes[1]] = ranges[axes[1]][1] | |
self.__check_result( order, rot_func, inv_func, theta, check_theta=False ) | |
for i in range(N): | |
theta = np.array([uniform(*xrange)*s,uniform(*yrange)*s,uniform(*zrange)*s]) | |
self.__check_result( order, rot_func, inv_func, theta ) | |
def test_rotations( self ): | |
self.__check_order( 'xyz', rotation_xyz, inverse_rotation_xyz ) | |
self.__check_order( 'xzy', rotation_xzy, inverse_rotation_xzy ) | |
self.__check_order( 'yxz', rotation_yxz, inverse_rotation_yxz ) | |
self.__check_order( 'yzx', rotation_yzx, inverse_rotation_yzx ) | |
self.__check_order( 'zxy', rotation_zxy, inverse_rotation_zxy ) | |
self.__check_order( 'zyx', rotation_zyx, inverse_rotation_zyx ) | |
orders = ['xyz', 'xzy', 'yxz', 'yzx', 'zxy', 'zyx' ] | |
for order in orders: | |
self.__check_order( order, lambda theta: rotation(theta, order), lambda M: inverse_rotation( M, order) ) | |
def __check_jacobian( self, order, N=1000 ): | |
eps = 1e-5 | |
def dirac(i): | |
v = np.zeros(3,dtype=float) | |
v[i] = eps | |
return v | |
for i in range(N): | |
p = np.random.uniform(size=3) | |
theta = np.random.uniform(-np.pi,np.pi,size=3) | |
ddx_fd = (rotation(theta+dirac(0),order)-rotation(theta-dirac(0),order))/(2.0*eps) | |
ddy_fd = (rotation(theta+dirac(1),order)-rotation(theta-dirac(1),order))/(2.0*eps) | |
ddz_fd = (rotation(theta+dirac(2),order)-rotation(theta-dirac(2),order))/(2.0*eps) | |
J_fd = np.stack((ddx_fd,ddy_fd,ddz_fd),axis=2) | |
J_an = rotation_jacobian( theta, order ) | |
self.assertTrue( np.linalg.norm( J_fd-J_an ) < 1e-9 ) | |
def test_jacobians( self ): | |
self.__check_jacobian( 'xyz' ) | |
self.__check_jacobian( 'xzy' ) | |
self.__check_jacobian( 'yxz' ) | |
self.__check_jacobian( 'yzx' ) | |
self.__check_jacobian( 'zxy' ) | |
self.__check_jacobian( 'zyx' ) | |
if __name__ == '__main__': | |
np.set_printoptions(precision=4,suppress=True) | |
unittest.main() |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment