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.q_conj() … Conjugate quaternion

• quat.q_inv() … Quaternion inversion

• quat.q_mult() … Quaternion multiplication

• quat.q_scalar() … Extract the scalar part from a quaternion

• quat.q_vector() … Extract the vector part from a quaternion

• quat.unit_q() … Extend a quaternion vector to a unit quaternion.

Conversion routines

• quat.calc_angvel() … Calculates the velocity in space from quaternions

• quat.calc_quat() … Calculate orientation from a starting orientation and angular velocity.

• quat.convert() … Convert quaternion to corresponding rotation matrix or Gibbs vector

• quat.deg2quat() … Convert number or axis angles to quaternion values

• quat.quat2deg() … Convert quaternions to corresponding axis angle

• quat.quat2seq() … Convert quaternions to corresponding rotation angles

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

Quaternion class, with multiplication, division, and inversion. 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

  • rotmatarray, 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’

values

quaternion values

Type

(4 x n) array

inv()

Inverse of the quaterion

export(to='rotmat')

Export to one of the following formats: ‘rotmat’, ‘Euler’, ‘Fick’, ‘Helmholtz’

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)

export(to='rotmat')

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

Return type

ndarray, with the specified content

Examples

>>> q = Quaternion([0,0.2,0.1])
>>> rm = q.export()
>>> fick = q.export('Fick')

inv()

Inverse of a quaternion.

quat.calc_angvel(q, rate=1, winSize=5, order=2)

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

angvel – angular velocity [rad/s].

Return type

array, shape (3,) or (N,3)

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.calc_quat(omega, q0, rate, CStype)

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 – unit quaternion vectors.

Return type

array, shape (N,4)

Notes

1. The output has the same length as the input. As a consequence, the last velocity vector is ignored.

2. 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)\]

3. For angular velocity with respect to the body (“bf”), the sequence of quaternions is inverted.

4. 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.calc_quat(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]])

quat.convert(quat, to='rotmat')

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

• to (string) –

  Has to be one of the following:

  • rotmat : rotation matrix

  • Gibbs : Gibbs vector

Returns

rotMat

Return type

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.convert([0, 0, 0.1], to='rotmat')
>>> r.shape
(1, 9)
>>> r.reshape((3,3))
array([[ 0.98      , -0.19899749,  0.        ],
    [ 0.19899749,  0.98      ,  0.        ],
    [ 0.        ,  0.        ,  1.        ]])

quat.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 – number or quaternion vector.

Return type

float or array (N,3)

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.q_conj(q)

Conjugate quaternion

Parameters

q (array_like, shape ([3,4],) or (N,[3/4])) – quaternion or quaternion vectors

Returns

qconj

Return type

conjugate quaternion(s)

Examples

>>>  quat.q_conj([0,0,0.1])
array([ 0.99498744, -0.        , -0.        , -0.1       ])

>>> quat.q_conj([[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.q_inv(q)

Quaternion inversion

Parameters

q (array_like, shape ([3,4],) or (N,[3/4])) – quaternion or quaternion vectors

Returns

qinv

Return type

inverse quaternion(s)

Notes

\[q^{-1} = \frac{q_0 - \vec{q}}{|q|^2}\]

More info under http://en.wikipedia.org/wiki/Quaternion

Examples

>>>  quat.q_inv([0,0,0.1])
array([-0., -0., -0.1])

>>> quat.q_inv([[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.q_mult(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 (array_like, shape ([3,4],) or (N,[3,4])) – quaternions or quaternion vectors

• q (array_like, shape ([3,4],) or (N,[3,4])) – quaternions or quaternion vectors

Returns

r – p and q are contain quaternion vectors).

Return type

quaternion or quaternion vector (if both

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.q_mult(p,q)

quat.q_scalar(inQuat)

Extract the quaternion scalar from a full quaternion.

Parameters

inQuat (array_like, shape ([3,4],) or (N,[3,4])) – quaternions or quaternion vectors.

Returns

vect – Corresponding quaternion scalar. If the input is only the quaternion-vector, the scalar part for a unit quaternion is calculated and returned.

Return type

array, shape (1,) or (N,1)

Notes

More info under http://en.wikipedia.org/wiki/Quaternion

Examples

>>> quat.q_scalar([[np.cos(0.2), 0, 0, np.sin(0.2)],[np.cos(0.1), 0, np.sin(0.1), 0]])
array([ 0.98006658,  0.99500417])

quat.q_vector(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 – corresponding quaternion vectors

Return type

array, shape (3,) or (N,3)

Notes

More info under http://en.wikipedia.org/wiki/Quaternion

Examples

>>> quat.q_vector([[np.cos(0.2), 0, 0, np.sin(0.2)],[cos(0.1), 0, np.sin(0.1), 0]])
array([[ 0.        ,  0.        ,  0.19866933],
       [ 0.        ,  0.09983342,  0.        ]])

quat.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 – float, or shape (3,) or (N,3)

Return type

corresponding axis angle(s)

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.quat2seq(quats, seq='nautical')

This function takes a quaternion, and calculates the corresponding angles for sequenctial rotations.

Parameters

• quats (ndarray, nx4) – input quaternions

• seq (string) –

  Has to be one the following:

  • Euler … Rz * Rx * Rz

  • Fick … Rz * Ry * Rx

  • nautical … same as “Fick”

  • Helmholtz … Ry * Rz * Rx

Returns

sequence – corresponding angles [deg] same sequence as in the rotation matrices

Return type

ndarray, nx3

Examples

>>> quat.quat2seq([0,0,0.1])
array([[ 11.47834095,  -0.        ,   0.        ]])

>>> quaternions = [[0,0,0.1], [0,0.2,0]]
skin.quat.quat2seq(quaternions)
array([[ 11.47834095,  -0.        ,   0.        ],
       [  0.        ,  23.07391807,   0.        ]])

>>> skin.quat.quat2seq(quaternions, 'nautical')
array([[ 11.47834095,  -0.        ,   0.        ],
       [  0.        ,  23.07391807,   0.        ]])

>>> skin.quat.quat2seq(quaternions, 'Euler')
array([[ 11.47834095,   0.        ,   0.        ],
       [ 90.        ,  23.07391807,  -90.        ]])

quat.unit_q(inData)

Utility function, which turns a quaternion vector into a unit quaternion. If the input is already a full quaternion, the output equals the input.

Parameters

inData (array_like, shape (3,) or (N,3)) – quaternions or quaternion vectors

Returns

quats – corresponding unit quaternions.

Return type

array, shape (4,) or (N,4)

Notes

More info under http://en.wikipedia.org/wiki/Quaternion

Examples

>>> quats = array([[0,0, sin(0.1)],[0, sin(0.2), 0]])
>>> quat.unit_q(quats)
array([[ 0.99500417,  0.        ,  0.        ,  0.09983342],
       [ 0.98006658,  0.        ,  0.19866933,  0.        ]])