File size: 4,446 Bytes
2d47d90 |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 |
'''
Tools for Manipulating and Converting 3D Rotations
By Omid Alemi
Created: June 12, 2017
Adapted from that matlab file...
'''
import math
import numpy as np
def deg2rad(x):
return x/180*math.pi
def rad2deg(x):
return x/math.pi*180
class Rotation():
def __init__(self,rot, param_type, rotation_order, **params):
self.rotmat = []
self.rotation_order = rotation_order
if param_type == 'euler':
self._from_euler(rot[0],rot[1],rot[2], params)
elif param_type == 'expmap':
self._from_expmap(rot[0], rot[1], rot[2], params)
def _from_euler(self, alpha, beta, gamma, params):
'''Expecting degress'''
if params['from_deg']==True:
alpha = deg2rad(alpha)
beta = deg2rad(beta)
gamma = deg2rad(gamma)
ca = math.cos(alpha)
cb = math.cos(beta)
cg = math.cos(gamma)
sa = math.sin(alpha)
sb = math.sin(beta)
sg = math.sin(gamma)
Rx = np.asarray([[1, 0, 0],
[0, ca, sa],
[0, -sa, ca]
])
Ry = np.asarray([[cb, 0, -sb],
[0, 1, 0],
[sb, 0, cb]])
Rz = np.asarray([[cg, sg, 0],
[-sg, cg, 0],
[0, 0, 1]])
self.rotmat = np.eye(3)
############################ inner product rotation matrix in order defined at BVH file #########################
for axis in self.rotation_order :
if axis == 'X' :
self.rotmat = np.matmul(Rx, self.rotmat)
elif axis == 'Y':
self.rotmat = np.matmul(Ry, self.rotmat)
else :
self.rotmat = np.matmul(Rz, self.rotmat)
################################################################################################################
def _from_expmap(self, alpha, beta, gamma, params):
if (alpha == 0 and beta == 0 and gamma == 0):
self.rotmat = np.eye(3)
return
#TODO: Check exp map params
theta = np.linalg.norm([alpha, beta, gamma])
expmap = [alpha, beta, gamma] / theta
x = expmap[0]
y = expmap[1]
z = expmap[2]
s = math.sin(theta/2)
c = math.cos(theta/2)
self.rotmat = np.asarray([
[2*(x**2-1)*s**2+1, 2*x*y*s**2-2*z*c*s, 2*x*z*s**2+2*y*c*s],
[2*x*y*s**2+2*z*c*s, 2*(y**2-1)*s**2+1, 2*y*z*s**2-2*x*c*s],
[2*x*z*s**2-2*y*c*s, 2*y*z*s**2+2*x*c*s , 2*(z**2-1)*s**2+1]
])
def get_euler_axis(self):
R = self.rotmat
theta = math.acos((self.rotmat.trace() - 1) / 2)
axis = np.asarray([R[2,1] - R[1,2], R[0,2] - R[2,0], R[1,0] - R[0,1]])
axis = axis/(2*math.sin(theta))
return theta, axis
def to_expmap(self):
theta, axis = self.get_euler_axis()
rot_arr = theta * axis
if np.isnan(rot_arr).any():
rot_arr = [0, 0, 0]
return rot_arr
def to_euler(self, use_deg=False):
eulers = np.zeros((2, 3))
if np.absolute(np.absolute(self.rotmat[2, 0]) - 1) < 1e-12:
#GIMBAL LOCK!
print('Gimbal')
if np.absolute(self.rotmat[2, 0]) - 1 < 1e-12:
eulers[:,0] = math.atan2(-self.rotmat[0,1], -self.rotmat[0,2])
eulers[:,1] = -math.pi/2
else:
eulers[:,0] = math.atan2(self.rotmat[0,1], -elf.rotmat[0,2])
eulers[:,1] = math.pi/2
return eulers
theta = - math.asin(self.rotmat[2,0])
theta2 = math.pi - theta
# psi1, psi2
eulers[0,0] = math.atan2(self.rotmat[2,1]/math.cos(theta), self.rotmat[2,2]/math.cos(theta))
eulers[1,0] = math.atan2(self.rotmat[2,1]/math.cos(theta2), self.rotmat[2,2]/math.cos(theta2))
# theta1, theta2
eulers[0,1] = theta
eulers[1,1] = theta2
# phi1, phi2
eulers[0,2] = math.atan2(self.rotmat[1,0]/math.cos(theta), self.rotmat[0,0]/math.cos(theta))
eulers[1,2] = math.atan2(self.rotmat[1,0]/math.cos(theta2), self.rotmat[0,0]/math.cos(theta2))
if use_deg:
eulers = rad2deg(eulers)
return eulers
def to_quat(self):
#TODO
pass
def __str__(self):
return "Rotation Matrix: \n " + self.rotmat.__str__()
|