'''
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)