Spaces:
Runtime error
Runtime error
""" | |
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 | |
import transforms3d as t3d | |
from pymo.Quaternions import Quaternions | |
def deg2rad(x): | |
return x / 180 * math.pi | |
def rad2deg(x): | |
return x / math.pi * 180 | |
def unroll(rots): | |
new_rot = rots.copy() | |
ang0 = np.linalg.norm(rots, axis=1) + 1e-8 | |
idx = np.where(ang0 > np.pi)[0] | |
ax = rots / np.tile(ang0[:, None], (1, 3)) | |
ang1 = ang0 - 2 * np.pi | |
alt_rot = ax * np.tile(ang1[:, None], (1, 3)) | |
new_rot[idx] = alt_rot[idx] | |
return new_rot | |
def unroll_1(rots): | |
new_rots = rots.copy() | |
# Compute angles and alternative rotation angles | |
angs = np.linalg.norm(rots, axis=1) | |
alt_angs = 2 * np.pi - angs | |
# find discontinuities | |
d_angs = np.diff(angs, axis=0) | |
d_angs2 = alt_angs[1:] - angs[:-1] | |
swps = np.where(np.abs(d_angs2) < np.abs(d_angs))[0] | |
# reshape into intervals where we should unroll the rotations | |
isodd = swps.shape[0] % 2 == 1 | |
if isodd: | |
swps = np.append(swps, rots.shape[0] - 1) | |
intv = 1 + swps.reshape((swps.shape[0] // 2, 2)) | |
for ii in range(intv.shape[0]): | |
new_ax = -rots[intv[ii, 0] : intv[ii, 1], :] / np.tile(angs[intv[ii, 0] : intv[ii, 1], None], (1, 3)) | |
new_angs = alt_angs[intv[ii, 0] : intv[ii, 1]] | |
new_rots[intv[ii, 0] : intv[ii, 1], :] = new_ax * np.tile(new_angs[:, None], (1, 3)) | |
return new_rots | |
def unroll_2(rots): | |
new_rots = rots.copy() | |
# Compute angles and alternative rotation angles | |
angs = np.linalg.norm(rots, axis=1) | |
dotprod = np.einsum("ij,ij->i", rots[:-1, :], rots[1:, :]) | |
# ax = rots/np.tile(angs[:, None], (1,3)) | |
# d_ax = np.linalg.norm(np.diff(ax, axis=0), axis=1) | |
alt_angs = 2 * np.pi - angs | |
# find discontinuities | |
d_angs = np.diff(angs, axis=0) | |
d_angs2 = alt_angs[1:] - angs[:-1] | |
# FIXME should check if dot product is <0 not norm d_ax | |
swps = np.where(dotprod < -1)[0] | |
# swps = np.where((np.abs(d_ax)>0.5))[0] | |
# swps = np.where(np.abs(d_angs2)<np.abs(d_angs))[0] | |
# reshape into intervals where we should unroll the rotations | |
isodd = swps.shape[0] % 2 == 1 | |
if isodd: | |
swps = swps[:-1] | |
# swps = np.append(swps, rots.shape[0]-1) | |
intv = 1 + swps.reshape((swps.shape[0] // 2, 2)) | |
for ii in range(intv.shape[0]): | |
new_ax = -rots[intv[ii, 0] : intv[ii, 1], :] / np.tile(angs[intv[ii, 0] : intv[ii, 1], None], (1, 3)) | |
new_angs = alt_angs[intv[ii, 0] : intv[ii, 1]] | |
new_rots[intv[ii, 0] : intv[ii, 1], :] = new_ax * np.tile(new_angs[:, None], (1, 3)) | |
return new_rots | |
def euler_reorder2(rots, order="XYZ", new_order="XYZ", use_deg=False): | |
if order == new_order: | |
return rots | |
if use_deg: | |
rots = np.deg2rad(rots) | |
quats = Quaternions.from_euler(rots, order=order.lower()) | |
eul = quats.euler(order=new_order.lower()) | |
if use_deg: | |
eul = np.rad2deg(eul) | |
return eul | |
def euler_reorder(rot, order="XYZ", new_order="XYZ", use_deg=False): | |
if order == new_order: | |
return rot | |
if use_deg: | |
rot = np.deg2rad(rot) | |
print("order:" + order) | |
print("new_order:" + new_order) | |
rotmat = t3d.euler.euler2mat(rot[0], rot[1], rot[2], "r" + order.lower()) | |
eul = t3d.euler.mat2euler(rotmat, "r" + new_order.lower()) | |
# quat = t3d.euler.euler2quat(rot[0], rot[1], rot[2], 'r' + order.lower()) | |
# eul = t3d.euler.quat2euler(quat, 'r' + new_order.lower()) | |
if use_deg: | |
eul = np.rad2deg(eul) | |
return eul | |
def offsets_inv(offset, rots, order="XYZ", use_deg=False): | |
if use_deg: | |
offset = np.deg2rad(offset) | |
rots = np.deg2rad(rots) | |
q0 = t3d.euler.euler2quat(rots[0], rots[1], rots[2], "r" + order.lower()) | |
q_off = t3d.euler.euler2quat(offset[0], offset[1], offset[2], "r" + order.lower()) | |
q2 = t3d.euler.quat2euler(t3d.quaternions.qmult(q0, t3d.quaternions.qinverse(q_off)), "r" + order.lower()) | |
# q0=Quaternions.from_euler(rots, order=order.lower()) | |
# q_off=Quaternions.from_euler(offset, order=order.lower()) | |
# q2=((-q_off)*q0).euler(order=order.lower()) | |
if use_deg: | |
q2 = np.rad2deg(q2) | |
return q2 | |
def offsets(offset, rots, order="XYZ", use_deg=False): | |
if use_deg: | |
offset = np.deg2rad(offset) | |
rots = np.deg2rad(rots) | |
q0 = t3d.euler.euler2quat(rots[0], rots[1], rots[2], "r" + order.lower()) | |
q_off = t3d.euler.euler2quat(offset[0], offset[1], offset[2], "r" + order.lower()) | |
q2 = t3d.euler.quat2euler(t3d.quaternions.qmult(q0, q_off), "r" + order.lower()) | |
# q0=Quaternions.from_euler(rots, order=order.lower()) | |
# q_off=Quaternions.from_euler(offset, order=order.lower()) | |
# q2=(q_off*q0).euler(order=order.lower()) | |
if use_deg: | |
q2 = np.rad2deg(q2) | |
return q2 | |
def euler2expmap2(rots, order="XYZ", use_deg=False): | |
if use_deg: | |
rots = np.deg2rad(rots) | |
# print("rot:" + str(rot)) | |
quats = Quaternions.from_euler(rots, order=order.lower()) | |
theta, vec = quats.angle_axis() | |
return unroll(vec * np.tile(theta[:, None], (1, 3))) | |
def euler2expmap(rot, order="XYZ", use_deg=False): | |
if use_deg: | |
rot = np.deg2rad(rot) | |
# print("rot:" + str(rot)) | |
vec, theta = t3d.euler.euler2axangle(rot[0], rot[1], rot[2], "r" + order.lower()) | |
return vec * theta | |
def expmap2euler(rot, order="XYZ", use_deg=False): | |
theta = np.linalg.norm(rot) | |
if theta > 1.0e-10: | |
vector = rot / theta | |
else: | |
vector = np.array([1.0, 0.0, 0.0]) | |
theta = 0.0 | |
eul = t3d.euler.axangle2euler(vector, theta, "r" + order.lower()) | |
if use_deg: | |
return np.rad2deg(eul) | |
else: | |
return eul | |
def euler2vectors(rot, order="XYZ", use_deg=False): | |
if use_deg: | |
rot = np.deg2rad(rot) | |
# order = "r" + (order.lower())[::-1] # try both r and s | |
# rotation_matrix = np.transpose(t3d.euler.euler2mat(rot[0], rot[1], rot[2], axes=order)) | |
rotation_matrix = t3d.euler.euler2mat(rot[0], rot[1], rot[2], "r" + order.lower()) | |
y_vector_x_value = rotation_matrix[0][1] | |
y_vector_y_value = rotation_matrix[1][1] | |
y_vector_z_value = rotation_matrix[2][1] | |
z_vector_x_value = rotation_matrix[0][2] | |
z_vector_y_value = rotation_matrix[1][2] | |
z_vector_z_value = rotation_matrix[2][2] | |
return y_vector_x_value, y_vector_y_value, y_vector_z_value, z_vector_x_value, z_vector_y_value, z_vector_z_value | |
def vectors2euler(axises, order="XYZ", use_deg=False): | |
# create rotation matrix | |
y_vector = [axises[0], axises[1], axises[2]] | |
z_vector = [axises[3], axises[4], axises[5]] | |
x_vector = np.cross(y_vector, z_vector) | |
R = np.column_stack((x_vector, y_vector, z_vector)) | |
# orthogonalize vectors | |
u, s, vt = np.linalg.svd(R) | |
R = np.matmul(u, vt) | |
# to euler | |
eul = t3d.euler.mat2euler(R, "r" + order.lower()) | |
if use_deg: | |
return np.rad2deg(eul) | |
else: | |
return eul | |
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) | |
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) | |
order = "s" + ((params["order"]).lower())[::-1] | |
# Quaternions.from_euler() | |
self.rotmat = np.transpose(t3d.euler.euler2mat(gamma, beta, alpha, axes=order)) | |
# 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) | |
# | |
# order = params['order'] | |
# for i in range(0,len(order)): | |
# if order[i]=='X': | |
# self.rotmat = np.matmul(Rx, self.rotmat) | |
# elif order[i]=='Y': | |
# self.rotmat = np.matmul(Ry, self.rotmat) | |
# elif order[i]=='Z': | |
# self.rotmat = np.matmul(Rz, self.rotmat) | |
# else: | |
# print('unknown rotation axis: ' + order[i]) | |
# | |
# # self.rotmat = np.matmul(np.matmul(Rz, Ry), Rx) | |
# print ("------" + "TRUE") | |
# print (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): | |
axis, theta = t3d.axangles.mat2axangle(self.rotmat, unit_thresh=1e-05) | |
# 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, order="xyz"): | |
order = "s" + order.lower() | |
eulers = t3d.euler.mat2euler(np.transpose(self.rotmat), axes=order) | |
return eulers[::-1] | |
# 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__() | |