Quaternions¶
Note that all these functions work with single quaternions and quaternion vectors, as well as with arrays containing these.
Quaternion class¶
quat.Quaternion
... Quaternion class
Functions for working with quaternions¶
quat.quatconj()
... Conjugate quaternionquat.quatinv()
... Quaternion inversionquat.quatmult()
... Quaternion multiplication
Conversion routines¶
quat.deg2quat()
... Convert number or axis angles to quaternion vectorsquat.quat2deg()
... Convert quaternion to corresponding axis anglequat.quat2rotmat()
... Convert quaternion to corresponding rotation matrixquat.quat2vect()
... Extract the vector part from a quaternionquat.quat2vel()
... Calculates the velocity in space from quaternionsquat.rotmat2quat()
... Convert a rotation matrix to the corresponding quaternionquat.vect2quat()
... Extend a quaternion vector to a unit quaternion.quat.vel2quat()
... Calculate orientation from a starting orientation and angular velocity.
Details¶
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.
A “Quaternion” class is defined, with
- operator overloading for mult, div, and inv.
- indexing
-
class
quat.
Quaternion
(inData, inType='vector')[source]¶ Quaternion class, with multiplocation, division, and inverse. A Quaternion can be created from vectors, rotation matrices, or from Fick-angles, Helmholtz-angles, or Euler angles (in deg). It provides
- operator overloading for mult, div, and inv.
- indexing
- access to the data, in the attribute values.
Parameters: inData : ndarray
Contains the data in one of the following formats:
vector : (3 x n) or (4 x n) array, containing the quaternion values
- rotmat : array, shape (3,3) or (N,9)
single rotation matrix, or matrix with rotation-matrix elements.
- Fick : (3 x n) array, containing (psi, phi, theta) rotations about
the (1,2,3) axes [deg] (Fick sequence)
- Helmholtz : (3 x n) array, containing (psi, phi, theta) rotations about
the (1,2,3) axes [deg] (Helmholtz sequence)
- Euler : (3 x n) array, containing (alpha, beta, gamma) rotations about
the (3,1,3) axes [deg] (Euler sequence)
inType : string
Specifies the type of the input and has to have one of the following values ‘vector’[Default], ‘rotmat’, ‘Fick’, ‘Helmholtz’, ‘Euler’
Notes
\[\begin{split}\vec {q}_{Euler} = \left[ {\begin{array}{*{20}{c}} {\cos \frac{\alpha }{2}*\cos \frac{\beta }{2}*\cos \frac{\gamma }{2} - \sin \frac{\alpha }{2}\cos \frac{\beta }{2}\sin \frac{\gamma }{2}} \\ {\cos \frac{\alpha }{2}*\sin \frac{\beta }{2}*\cos \frac{\gamma }{2} + \sin \frac{\alpha }{2}\sin \frac{\beta }{2}\sin \frac{\gamma }{2}} \\ {\cos \frac{\alpha }{2}*\sin \frac{\beta }{2}*\sin \frac{\gamma }{2} - \sin \frac{\alpha }{2}\sin \frac{\beta }{2}\cos \frac{\gamma }{2}} \\ {\cos \frac{\alpha }{2}*\cos \frac{\beta }{2}*\sin \frac{\gamma }{2} + \sin \frac{\alpha }{2}\cos \frac{\beta }{2}\cos \frac{\gamma }{2}} \end{array}} \right]\end{split}\]\[\begin{split}\vec {q}_{Fick} = \left[ {\begin{array}{*{20}{c}} {\cos \frac{\psi }{2}*\cos \frac{\phi }{2}*\cos \frac{\theta }{2} + \sin \frac{\psi }{2}\sin \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\sin \frac{\psi }{2}*\cos \frac{\phi }{2}*\cos \frac{\theta }{2} - \cos \frac{\psi }{2}\sin \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\cos \frac{\psi }{2}*\sin \frac{\phi }{2}*\cos \frac{\theta }{2} + \sin \frac{\psi }{2}\cos \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\cos \frac{\psi }{2}*\cos \frac{\phi }{2}*\sin \frac{\theta }{2} - \sin \frac{\psi }{2}\sin \frac{\phi }{2}\cos \frac{\theta }{2}} \end{array}} \right]\end{split}\]\[\begin{split}\vec {q}_{Helmholtz} = \left[ {\begin{array}{*{20}{c}} {\cos \frac{\psi }{2}*\cos \frac{\phi }{2}*\cos \frac{\theta }{2} - \sin \frac{\psi }{2}\sin \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\sin \frac{\psi }{2}*\cos \frac{\phi }{2}*\cos \frac{\theta }{2} + \cos \frac{\psi }{2}\sin \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\cos \frac{\psi }{2}*\sin \frac{\phi }{2}*\cos \frac{\theta }{2} + \sin \frac{\psi }{2}\cos \frac{\phi }{2}\sin \frac{\theta }{2}} \\ {\cos \frac{\psi }{2}*\cos \frac{\phi }{2}*\sin \frac{\theta }{2} - \sin \frac{\psi }{2}\sin \frac{\phi }{2}\cos \frac{\theta }{2}} \end{array}} \right]\end{split}\]Examples
>>> q = Quaternion(array([[0,0,0.1], [0,0,0.2], [0,0,0.5]])) >>> p = Quaternion(array([0,0,0.2])) >>> fick = Quaternion( array([[0,0,10], [0,10,10]]), 'Fick') >>> combined = p * q >>> divided = q / p >>> extracted = q[1:2] >>> len(q) >>> data = q.values >>> 2 >>> inv(q)
Attributes
values ((4 x n) array) quaternion values Methods
inv() Inverse of the quaterion export(to=’rotmat’) Export to one of the following formats: ‘rotmat’, ‘Euler’, ‘Fick’, ‘Helmholtz’ -
export
(to='rotmat')[source]¶ Conversion to other formats. May be slow for “Fick”, “Helmholtz”, and “Euler”.
Parameters: to : string
content of returned values
- ‘rotmat’ : rotation matrices (default), each flattened to a 9-dim vector
- ‘Euler’ : Euler angles
- ‘Fick’ : Fick angles
- ‘Helmholtz’ : Helmholtz angles
- ‘vector’ : vector part of the quaternion
Returns: ndarray, with the specified content
Examples
>>> q = Quaternion([0,0.2,0.1]) >>> rm = q.export() >>> fick = q.export('Fick')
-
quat.
deg2quat
(inDeg)[source]¶ 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
\[| \vec{q} | = sin(\theta/2)\]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
-
quat.
quat2deg
(inQuat)[source]¶ 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
\[| \vec{q} | = sin(\theta/2)\]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. ])
-
quat.
quat2rotmat
(inQuat)[source]¶ 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
\[\begin{split}{\bf{R}} = \left( {\begin{array}{*{20}{c}} {q_0^2 + q_1^2 - q_2^2 - q_3^2}&{2({q_1}{q_2} - {q_0}{q_3})}&{2({q_1}{q_3} + {q_0}{q_2})}\\ {2({q_1}{q_2} + {q_0}{q_3})}&{q_0^2 - q_1^2 + q_2^2 - q_3^2}&{2({q_2}{q_3} - {q_0}{q_1})}\\ {2({q_1}{q_3} - {q_0}{q_2})}&{2({q_2}{q_3} + {q_0}{q_1})}&{q_0^2 - q_1^2 - q_2^2 + q_3^2} \\ \end{array}} \right)\end{split}\]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. ]])
-
quat.
quat2vect
(inQuat)[source]¶ 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. ]])
-
quat.
quat2vel
(q, rate=1, winSize=5, order=2)[source]¶ Take a quaternion, and convert it into the corresponding angular velocity
Parameters: q : array, shape (N,[3,4])
unit quaternion vectors.
rate : float
sampling rate (in [Hz])
winSize : integer
window size for the calculation of the velocity. Has to be odd.
order : integer
Order of polynomial used by savgol to calculate the first derivative
Returns: vel : array, shape (3,) or (N,3)
angular velocity [rad/s].
Notes
The angular velocity is given by
\[\omega = 2 * \frac{dq}{dt} \circ q^{-1}\]Examples
>>> rate = 1000 >>> t = np.arange(0,10,1/rate) >>> x = 0.1 * np.sin(t) >>> y = 0.2 * np.sin(t) >>> z = np.zeros_like(t) array([[ 0.20000029, 0.40000057, 0. ], [ 0.19999989, 0.39999978, 0. ], [ 0.19999951, 0.39999901, 0. ]]) .......
-
quat.
quatconj
(q)[source]¶ Conjugate quaternion
Parameters: q: array_like, shape ([3,4],) or (N,[3/4])
quaternion or quaternion vectors
Returns: qconj : conjugate quaternion(s)
Examples
>>> quat.quatconj([0,0,0.1]) array([ 0., -0., -0., -1.])
>>> quat.quatconj([[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. ]])
-
quat.
quatinv
(q)[source]¶ Quaternion inversion
Parameters: q: array_like, shape ([3,4],) or (N,[3/4])
quaternion or quaternion vectors
Returns: qinv : inverse quaternion(s)
Notes
\[q^{-1} = \frac{q_0 - \vec{q}}{|q|^2}\]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. ]])
-
quat.
quatmult
(p, q)[source]¶ 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
\[q \circ p = \sum\limits_{i=0}^3 {q_i I_i} * \sum\limits_{j=0}^3 \ {p_j I_j} = (q_0 p_0 - \vec{q} \cdot \vec{p}) + (q_0 \vec{p} + p_0 \ \vec{q} + \vec{q} \times \vec{p}) \cdot \vec{I}\]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)
-
quat.
rotmat2quat
(rMat)[source]¶ 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
\[\begin{split}\vec q = 0.5*copysign\left( {\begin{array}{*{20}{c}} {\sqrt {1 + {R_{11}} - {R_{22}} - {R_{33}}} ,}\\ {\sqrt {1 - {R_{11}} + {R_{22}} - {R_{33}}} ,}\\ {\sqrt {1 - {R_{11}} - {R_{22}} + {R_{33}}} ,} \end{array}\begin{array}{*{20}{c}} {{R_{32}} - {R_{23}}}\\ {{R_{13}} - {R_{31}}}\\ {{R_{21}} - {R_{12}}} \end{array}} \right) \end{split}\]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]])
-
quat.
vect2quat
(inData)[source]¶ 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. ]])
-
quat.
vel2quat
(omega, q0, rate, CStype)[source]¶ Take an angular velocity (in rad/s), and convert it into the corresponding orientation quaternion.
Parameters: omega : array, shape (3,) or (N,3)
angular velocity [rad/s].
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
- The output has the same length as the input. As a consequence, the last velocity vector is ignored.
- For angular velocity with respect to space (“sf”), the orientation is given by
\[q(t) = \Delta q(t_n) \circ \Delta q(t_{n-1}) \circ ... \circ \Delta q(t_2) \circ \Delta q(t_1) \circ q(t_0)\]\[\Delta \vec{q_i} = \vec{n(t)}\sin (\frac{\Delta \phi (t_i)}{2}) = \frac{\vec \omega (t_i)}{\left| {\vec \omega (t_i)} \right|}\sin \left( \frac{\left| {\vec \omega ({t_i})} \right|\Delta t}{2} \right)\]- For angular velocity with respect to the body (“bf”), the sequence of quaternions is inverted.
- Take care that you choose a high enough sampling rate!
Examples
>>> v0 = np.r_[0., 0., 100.] * np.pi/180. >>> omega = np.tile(v0, (1000,1)) >>> rate = 100 >>> out = quat.vel2quat(omega, [0., 0., 0.], rate, 'sf') array([[ 1. , 0. , 0. , 0. ], [ 0.99996192, 0. , 0. , 0.00872654], [ 0.9998477 , 0. , 0. , 0.01745241], ..., [-0.74895572, 0. , 0. , 0.66262005], [-0.75470958, 0. , 0. , 0.65605903], [-0.76040597, 0. , 0. , 0.64944805]])