Source code for quat

'''
Functions for working with quaternions. Note that all the functions also
work on arrays, and can deal with full quaternions as well as with
quaternion vectors.

'''

'''
author: Thomas Haslwanter
date:   Oct-2013
ver:    2.1
'''

from numpy import sqrt, sum, r_, c_, hstack, cos, sin, atleast_2d, \
     zeros, shape, vstack, prod, min, arcsin, pi, tile, array, copysign, \
     reshape
import matplotlib.pyplot as plt

[docs]def deg2quat(inDeg): ''' Convert axis-angles or plain degree into the corresponding quaternion values. Can be used with a plain number or with an axis angle. Parameters ---------- inDeg : float or (N,3) quaternion magnitude or quaternion vectors. Returns ------- outQuat : float or array (N,3) number or quaternion vector. Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> quat.deg2quat(array([[10,20,30], [20,30,40]])) array([[ 0.08715574, 0.17364818, 0.25881905], [ 0.17364818, 0.25881905, 0.34202014]]) >>> quat.deg2quat(10) 0.087155742747658166 ''' return sin(0.5 * inDeg * pi/180)
[docs]def quatinv(q): ''' Quaternion inversion Parameters ---------- q: array_like, shape ([3,4],) or (N,[3/4]) quaternion or quaternion vectors Returns ------- qinv : inverse quaternion(s) Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> quat.quatinv([0,0,0.1]) array([[-0. , -0. , -0.1]]) >>> quat.quatinv([[cos(0.1),0,0,sin(0.1)], ...: [cos(0.2),0,sin(0.2),0]]) array([[ 0.99500417, -0. , -0. , -0.09983342], [ 0.98006658, -0. , -0.19866933, -0. ]]) ''' q = atleast_2d(q) if q.shape[1]==3: return -q else: qLength = sum(q**2, 1) qConj = q * r_[1, -1,-1,-1] return (qConj.T / qLength).T
[docs]def quatmult(p,q): ''' Quaternion multiplication: Calculates the product of two quaternions r = p * q If one of both of the quaterions have only three columns, the scalar component is calculated such that the length of the quaternion is one. The lengths of the quaternions have to match, or one of the two quaternions has to have the length one. If both p and q only have 3 components, the returned quaternion also only has 3 components (i.e. the quaternion vector) Parameters ---------- p,q : array_like, shape ([3,4],) or (N,[3,4]) quaternions or quaternion vectors Returns ------- r : quaternion or quaternion vector (if both p and q are contain quaternion vectors). Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> p = [cos(0.2), 0, 0, sin(0.2)] >>> q = [[0, 0, 0.1], >>> [0, 0.1, 0]] >>> r = quat.quatmult(p,q) ''' flag3D = False p = atleast_2d(p) q = atleast_2d(q) if p.shape[1]==3 & q.shape[1]==3: flag3D = True if len(p) != len(q): assert (len(p)==1 or len(q)==1), \ 'Both arguments in the quaternion multiplication must have the same number of rows, unless one has only one row.' p = vect2quat(p).T q = vect2quat(q).T if prod(shape(p)) > prod(shape(q)): r=zeros(shape(p)) else: r=zeros(shape(q)) r[0] = p[0]*q[0] - p[1]*q[1] - p[2]*q[2] - p[3]*q[3] r[1] = p[1]*q[0] + p[0]*q[1] + p[2]*q[3] - p[3]*q[2] r[2] = p[2]*q[0] + p[0]*q[2] + p[3]*q[1] - p[1]*q[3] r[3] = p[3]*q[0] + p[0]*q[3] + p[1]*q[2] - p[2]*q[1] if flag3D: # for rotations > 180 deg r[:,r[0]<0] = -r[:,r[0]<0] r = r[1:] r = r.T return r
[docs]def quat2deg(inQuat): '''Calculate the axis-angle corresponding to a given quaternion. Parameters ---------- inQuat: float, or array_like, shape ([3/4],) or (N,[3/4]) quaternion(s) or quaternion vector(s) Returns ------- axAng : corresponding axis angle(s) float, or shape (3,) or (N,3) Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> quat.quat2deg(0.1) array([ 11.47834095]) >>> quat.quat2deg([0.1, 0.1, 0]) array([ 11.47834095, 11.47834095, 0. ]) >>> quat.quat2deg([cos(0.1), 0, sin(0.1), 0]) array([ 0. , 11.4591559, 0. ]) ''' return 2 * arcsin(quat2vect(inQuat)) * 180 / pi
[docs]def quat2rotmat(inQuat): ''' Calculate the rotation matrix corresponding to the quaternion. If "inQuat" contains more than one quaternion, the matrix is flattened (to facilitate the work with rows of quaternions), and can be restored to matrix form by "reshaping" the resulting rows into a (3,3) shape. Parameters ---------- inQuat : array_like, shape ([3,4],) or (N,[3,4]) quaternions or quaternion vectors Returns ------- rotMat : corresponding rotation matrix/matrices (flattened) Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> r = quat.quat2rotmat([0, 0, 0.1]) >>> r.shape (1, 9) >>> r.reshape((3,3)) array([[ 0.98 , -0.19899749, 0. ], [ 0.19899749, 0.98 , 0. ], [ 0. , 0. , 1. ]]) ''' q = vect2quat(inQuat).T R = zeros((9, q.shape[1])) R[0] = q[0]**2 + q[1]**2 - q[2]**2 - q[3]**2 R[1] = 2*(q[1]*q[2] - q[0]*q[3]) R[2] = 2*(q[1]*q[3] + q[0]*q[2]) R[3] = 2*(q[1]*q[2] + q[0]*q[3]) R[4] = q[0]**2 - q[1]**2 + q[2]**2 - q[3]**2 R[5] = 2*(q[2]*q[3] - q[0]*q[1]) R[6] = 2*(q[1]*q[3] - q[0]*q[2]) R[7] = 2*(q[2]*q[3] + q[0]*q[1]) R[8] = q[0]**2 - q[1]**2 - q[2]**2 + q[3]**2 if R.shape[1] == 1: return reshape(R, (3,3)) else: return R.T
[docs]def quat2vect(inQuat): ''' Extract the quaternion vector from a full quaternion. Parameters ---------- inQuat : array_like, shape ([3,4],) or (N,[3,4]) quaternions or quaternion vectors. Returns ------- vect : array, shape (3,) or (N,3) corresponding quaternion vectors Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> quat.quat2vect([[cos(0.2), 0, 0, sin(0.2)],[cos(0.1), 0, sin(0.1), 0]]) array([[ 0. , 0. , 0.19866933], [ 0. , 0.09983342, 0. ]]) ''' inQuat = atleast_2d(inQuat) if inQuat.shape[1] == 4: vect = inQuat[:,1:] else: vect = inQuat if min(vect.shape)==1: vect = vect.flatten() return vect
[docs]def rotmat2quat(rMat): ''' Assumes that R has the shape (3,3), or the matrix elements in columns Parameters ---------- rMat : array, shape (3,3) or (N,9) single rotation matrix, or matrix with rotation-matrix elements. Returns ------- outQuat : array, shape (4,) or (N,4) corresponding quaternion vector(s) Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> rotMat = array([[cos(alpha), -sin(alpha), 0], >>> [sin(alpha), cos(alpha), 0], >>> [0, 0, 1]]) >>> quat.rotmat2quat(rotMat) array([[ 0.99500417, 0. , 0. , 0.09983342]]) ''' if rMat.shape == (3,3) or rMat.shape == (9,): rMat=atleast_2d(rMat.flatten()).T else: rMat = rMat.T q = zeros((4, rMat.shape[1])) R11 = rMat[0] R12 = rMat[1] R13 = rMat[2] R21 = rMat[3] R22 = rMat[4] R23 = rMat[5] R31 = rMat[6] R32 = rMat[7] R33 = rMat[8] q[1] = 0.5 * copysign(sqrt(1+R11-R22-R33), R32-R23) q[2] = 0.5 * copysign(sqrt(1-R11+R22-R33), R13-R31) q[3] = 0.5 * copysign(sqrt(1-R11-R22+R33), R21-R12) q[0] = sqrt(1-(q[1]**2+q[2]**2+q[3]**2)) return q.T
[docs]def rotate_vector(vector, q): ''' Rotates a vector, according to the given quaternions. Note that a single vector can be rotated into many orientations; or a row of vectors can all be rotated by a single quaternion. Parameters ---------- vector : array, shape (3,) or (N,3) vector(s) to be rotated. q : array_like, shape ([3,4],) or (N,[3,4]) quaternions or quaternion vectors. Returns ------- rotated : array, shape (3,) or (N,3) rotated vector(s) Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> mymat = eye(3) >>> myVector = r_[1,0,0] >>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]]) >>> quat.rotate_vector(myVector, quats) array([[ 0.98006658, 0.19866933, 0. ], [ 0.92106099, 0. , -0.38941834]]) >>> quat.rotate_vector(mymat, [0, 0, sin(0.1)]) array([[ 0.98006658, 0.19866933, 0. ], [-0.19866933, 0.98006658, 0. ], [ 0. , 0. , 1. ]]) ''' vector = atleast_2d(vector) qvector = hstack((zeros((vector.shape[0],1)), vector)) vRotated = quatmult(q, quatmult(qvector, quatinv(q))) vRotated = vRotated[:,1:] if min(vRotated.shape)==1: vRotated = vRotated.flatten() return vRotated
[docs]def vect2quat(inData): ''' Utility function, which turns a quaternion vector into a unit quaternion. Parameters ---------- inData : array_like, shape (3,) or (N,3) quaternions or quaternion vectors Returns ------- quats : array, shape (4,) or (N,4) corresponding unit quaternions. Notes ----- More info under http://en.wikipedia.org/wiki/Quaternion Examples -------- >>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]]) >>> quat.vect2quat(quats) array([[ 0.99500417, 0. , 0. , 0.09983342], [ 0.98006658, 0. , 0.19866933, 0. ]]) ''' inData = atleast_2d(inData) (m,n) = inData.shape if (n!=3)&(n!=4): error('Quaternion must have 3 or 4 columns') if n == 3: qLength = 1-sum(inData**2,1) numLimit = 1e-12 # Check for numerical problems if min(qLength) < -numLimit: error('Quaternion is too long!') else: # Correct for numerical problems qLength[qLength<0] = 0 outData = hstack((c_[sqrt(qLength)], inData)) else: outData = inData return outData
[docs]def vel2quat(vel, q0, rate, CStype): ''' Take an angular velocity (in deg/s), and convert it into the corresponding orientation quaternion. Parameters ---------- vel : array, shape (3,) or (N,3) angular velocity. q0 : array (3,) vector-part of quaternion (!!) rate : float sampling rate (in [Hz]) CStype: string coordinate_system, space-fixed ("sf") or body_fixed ("bf") Returns ------- quats : array, shape (N,4) unit quaternion vectors. Notes ----- Take care that you choose a high enough sampling rate! Examples -------- >>> v0 = [0., 0., 100.] >>> vel = tile(v0, (1000,1)) >>> rate = 100 >>> out = quat.vel2quat(vel, [0., 0., 0.], rate, 'sf') >>> out[-1:] array([[-0.76040597, 0. , 0. , 0.64944805]]) ''' # Convert from deg/s to rad/s vel = vel * pi/180 vel_t = sqrt(sum(vel**2, 1)) vel_nonZero = vel_t>0 # initialize the quaternion q_delta = zeros(shape(vel)) q_pos = zeros((len(vel),4)) q_pos[0,:] = vect2quat(q0) # magnitude of position steps dq_total = sin(vel_t[vel_nonZero]/(2*rate)) q_delta[vel_nonZero,:] = vel[vel_nonZero,:] * tile(dq_total/vel_t[vel_nonZero], (3,1)).T for ii in range(len(vel)-1): q1 = vect2quat(q_delta[ii,:]) q2 = q_pos[ii,:] if CStype == 'sf': qm = quatmult(q1,q2) elif CStype == 'bf': qm = quatmult(q2,q1) else: print 'I don''t know this type of coordinate system!' q_pos[ii+1,:] = qm return q_pos
if __name__=='__main__': '''These are some simple tests to see if the functions produce the proper output. More extensive tests are found in tests/test_quat.py''' a = r_[cos(0.1), 0,0,sin(0.1)] b = r_[cos(0.1),0,sin(0.1), 0] c = vstack((a,b)) d = r_[sin(0.1), 0, 0] e = r_[2, 0, sin(0.1), 0] print(quatmult(a,a)) print(quatmult(a,b)) print(quatmult(c,c)) print(quatmult(c,a)) print(quatmult(d,d)) print('The inverse of {0} is {1}'.format(a, quatinv(a))) print('The inverse of {0} is {1}'.format(d, quatinv(d))) print('The inverse of {0} is {1}'.format(e, quatinv(e))) print(quatmult(e, quatinv(e))) print(quat2vect(a)) print('{0} is {1} degree'.format(a, quat2deg(a))) print('{0} is {1} degree'.format(c, quat2deg(c))) print(quat2deg(0.2)) x = r_[1,0,0] vNull = r_[0,0,0] print(rotate_vector(x, a)) v0 = [0., 0., 100.] vel = tile(v0, (1000,1)) rate = 100 out = vel2quat(vel, [0., 0., 0.], rate, 'sf') print(out[-1:]) plt.plot(out[:,1:4]) plt.show() print(deg2quat(15)) print(deg2quat(quat2deg(a))) q = array([[0, 0, sin(0.1)], [0, sin(0.01), 0]]) rMat = quat2rotmat(q) print(rMat[1].reshape((3,3))) qNew = rotmat2quat(rMat) print(qNew)