144 lines
4.6 KiB
Python
144 lines
4.6 KiB
Python
# -*- coding: utf-8 -*-
|
|
"""
|
|
Created on Fri Jun 28 10:42:37 2013
|
|
|
|
@author: cforster
|
|
"""
|
|
|
|
import math
|
|
import numpy as np
|
|
|
|
# epsilon for testing whether a number is close to zero
|
|
_EPS = np.finfo(float).eps * 4.0
|
|
|
|
# axis sequences for Euler angles
|
|
_NEXT_AXIS = [1, 2, 0, 1]
|
|
|
|
# map axes strings to/from tuples of inner axis, parity, repetition, frame
|
|
_AXES2TUPLE = {
|
|
'sxyz': (0, 0, 0, 0), 'sxyx': (0, 0, 1, 0), 'sxzy': (0, 1, 0, 0),
|
|
'sxzx': (0, 1, 1, 0), 'syzx': (1, 0, 0, 0), 'syzy': (1, 0, 1, 0),
|
|
'syxz': (1, 1, 0, 0), 'syxy': (1, 1, 1, 0), 'szxy': (2, 0, 0, 0),
|
|
'szxz': (2, 0, 1, 0), 'szyx': (2, 1, 0, 0), 'szyz': (2, 1, 1, 0),
|
|
'rzyx': (0, 0, 0, 1), 'rxyx': (0, 0, 1, 1), 'ryzx': (0, 1, 0, 1),
|
|
'rxzx': (0, 1, 1, 1), 'rxzy': (1, 0, 0, 1), 'ryzy': (1, 0, 1, 1),
|
|
'rzxy': (1, 1, 0, 1), 'ryxy': (1, 1, 1, 1), 'ryxz': (2, 0, 0, 1),
|
|
'rzxz': (2, 0, 1, 1), 'rxyz': (2, 1, 0, 1), 'rzyz': (2, 1, 1, 1)}
|
|
|
|
_TUPLE2AXES = dict((v, k) for k, v in _AXES2TUPLE.items())
|
|
|
|
|
|
def quat2dcm(quaternion):
|
|
"""Convert Quaternion [x y z w] to Direct Cosine Matrix representation """
|
|
q = np.array(quaternion[:4], dtype=np.float64, copy=True)
|
|
nq = np.dot(q, q)
|
|
if nq < _EPS:
|
|
return np.identity(4)
|
|
q *= math.sqrt(2.0 / nq)
|
|
q = np.outer(q, q)
|
|
return np.array([ [1.0-q[1, 1]-q[2, 2], q[0, 1]-q[2, 3], q[0, 2]+q[1, 3]],
|
|
[ q[0, 1]+q[2, 3], 1.0-q[0, 0]-q[2, 2], q[1, 2]-q[0, 3]],
|
|
[ q[0, 2]-q[1, 3], q[1, 2]+q[0, 3], 1.0-q[0, 0]-q[1, 1]]],
|
|
dtype=np.float64)
|
|
|
|
def dcm2quat(matrix):
|
|
"""Convert Direct Cosine Matrix to Quaternion
|
|
|
|
This algorithm comes from "Quaternion Calculus and Fast Animation",
|
|
Ken Shoemake, 1987 SIGGRAPH course notes
|
|
"""
|
|
q = np.empty((4, ), dtype=np.float64)
|
|
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
|
|
t = np.trace(M)
|
|
if (t > 0):
|
|
t = math.sqrt(t + 1.0);
|
|
q[3] = 0.5*t;
|
|
t = 0.5/t;
|
|
q[0] = (M[2,1] - M[1,2]) * t;
|
|
q[1] = (M[0,2] - M[2,0]) * t;
|
|
q[2] = (M[1,0] - M[0,1]) * t;
|
|
else:
|
|
i = 0;
|
|
if (M[1,1] > M[0,0]):
|
|
i = 1;
|
|
if (M[2,2] > M[i,i]):
|
|
i = 2;
|
|
j = (i+1)%3;
|
|
k = (j+1)%3;
|
|
|
|
t = math.sqrt(M[i,i]-M[j,j]-M[k,k] + 1.0);
|
|
q[i] = 0.5 * t;
|
|
t = 0.5/t;
|
|
q[3] = (M[k,j]-M[j,k])*t;
|
|
q[j] = (M[j,i]+M[i,j])*t;
|
|
q[k] = (M[k,i]+M[i,k])*t;
|
|
return q
|
|
|
|
def dcm2rpy(matrix, axes='sxyz'):
|
|
"""Return Euler angles from rotation matrix for specified axis sequence.
|
|
|
|
axes : One of 24 axis sequences as string or encoded tuple
|
|
|
|
Note that many Euler angle triplets can describe one matrix.
|
|
|
|
>>> R0 = euler_matrix(1, 2, 3, 'syxz')
|
|
>>> al, be, ga = euler_from_matrix(R0, 'syxz')
|
|
>>> R1 = euler_matrix(al, be, ga, 'syxz')
|
|
>>> numpy.allclose(R0, R1)
|
|
True
|
|
>>> angles = (4.0*math.pi) * (numpy.random.random(3) - 0.5)
|
|
>>> for axes in _AXES2TUPLE.keys():
|
|
... R0 = euler_matrix(axes=axes, *angles)
|
|
... R1 = euler_matrix(axes=axes, *euler_from_matrix(R0, axes))
|
|
... if not numpy.allclose(R0, R1): print axes, "failed"
|
|
|
|
"""
|
|
try:
|
|
firstaxis, parity, repetition, frame = _AXES2TUPLE[axes.lower()]
|
|
except (AttributeError, KeyError):
|
|
_ = _TUPLE2AXES[axes]
|
|
firstaxis, parity, repetition, frame = axes
|
|
|
|
i = firstaxis
|
|
j = _NEXT_AXIS[i+parity]
|
|
k = _NEXT_AXIS[i-parity+1]
|
|
|
|
M = np.array(matrix, dtype=np.float64, copy=False)[:3, :3]
|
|
if repetition:
|
|
sy = math.sqrt(M[i, j]*M[i, j] + M[i, k]*M[i, k])
|
|
if sy > _EPS:
|
|
ax = math.atan2( M[i, j], M[i, k])
|
|
ay = math.atan2( sy, M[i, i])
|
|
az = math.atan2( M[j, i], -M[k, i])
|
|
else:
|
|
ax = math.atan2(-M[j, k], M[j, j])
|
|
ay = math.atan2( sy, M[i, i])
|
|
az = 0.0
|
|
else:
|
|
cy = math.sqrt(M[i, i]*M[i, i] + M[j, i]*M[j, i])
|
|
if cy > _EPS:
|
|
ax = math.atan2( M[k, j], M[k, k])
|
|
ay = math.atan2(-M[k, i], cy)
|
|
az = math.atan2( M[j, i], M[i, i])
|
|
else:
|
|
ax = math.atan2(-M[j, k], M[j, j])
|
|
ay = math.atan2(-M[k, i], cy)
|
|
az = 0.0
|
|
|
|
if parity:
|
|
ax, ay, az = -ax, -ay, -az
|
|
if frame:
|
|
ax, az = az, ax
|
|
return ax, ay, az
|
|
|
|
def quat2rpy(quaternion, axes='sxyz'):
|
|
"""Return Euler angles from quaternion for specified axis sequence.
|
|
|
|
>>> angles = euler_from_quaternion([0.06146124, 0, 0, 0.99810947])
|
|
>>> numpy.allclose(angles, [0.123, 0, 0])
|
|
True
|
|
|
|
"""
|
|
return dcm2rpy(quat2dcm(quaternion), axes)
|
|
|