| ''' |
| 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 |
|
|
| class Rotation(): |
| def __init__(self,rot, param_type, **params): |
| self.rotmat = [] |
| if param_type == 'euler': |
| self._from_euler(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) |
| |
| Rx = np.asarray([[1, 0, 0], |
| [0, math.cos(alpha), -math.sin(alpha)], |
| [0, math.sin(alpha), math.cos(alpha)] |
| ]) |
|
|
| Ry = np.asarray([[math.cos(beta), 0, math.sin(beta)], |
| [0, 1, 0], |
| [-math.sin(beta), 0, math.cos(beta)]]) |
|
|
| Rz = np.asarray([[math.cos(gamma), -math.sin(gamma), 0], |
| [math.sin(gamma), math.cos(gamma), 0], |
| [0, 0, 1]]) |
|
|
| self.rotmat = np.matmul(np.matmul(Rz, Ry), Rx).T |
| |
| 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): |
| |
| pass |
| |
| def to_quat(self): |
| |
| pass |
| |
|
|
|
|
|
|
|
|
