Quaternions

These functions create and manipulate quaternions or unit quaternions. The quaternion is represented by a 1D NumPy array with 4 elements: s, x, y, z.

q2r(q, order='sxyz')[source]

Convert unit-quaternion to SO(3) rotation matrix

Parameters

q (Union[ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]], List, Tuple[float, float, float, float]]) – unit-quaternion
order (str) – the order of the quaternion elements. Must be ‘sxyz’ or ‘xyzs’. Defaults to ‘sxyz’.

Returns

corresponding SO(3) rotation matrix

Return type

ndarray(3,3)

Returns an SO(3) rotation matrix corresponding to this unit-quaternion.

>>> from spatialmath.base import q2r
>>> q = [0, 0, 1, 0]  # rotation of 180deg about y-axis
>>> print(q2r(q))
[[-1.  0.  0.]
 [ 0.  1.  0.]
 [ 0.  0. -1.]]

Warning

There is no check that the passed value is a unit-quaternion.

Seealso: r2q()

q2v(q)[source]

Convert unit-quaternion to 3-vector

Parameters: q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – unit-quaternion
Returns: a unique 3-vector
Return type: ndarray(3)

Returns a unique 3-vector representing the input unit-quaternion. The sign of the scalar part is made positive, if necessary by multiplying the entire quaternion by -1, then the vector part is taken.

>>> from spatialmath.base import q2v
>>> from math import sqrt
>>> q = [1 / sqrt(2), 0, 1 / sqrt(2), 0]
>>> print(q2v(q))
[0.     0.7071 0.    ]
>>> q = [-1 / sqrt(2), 0, 1 / sqrt(2), 0]
>>> print(q2v(q))
[-0.     -0.7071 -0.    ]

Warning

There is no check that the passed value is a unit-quaternion.

Seealso: v2q()

qangle(q1, q2)[source]

Angle between two unit-quaternions

Parameters

q0 (array_like(4)) – unit-quaternion
q1 (array_like(4)) – unit-quaternion

Returns

angle between the rotations [radians]

Return type

float

If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second.

>>> from spatialmath.base import qangle
>>> from math import sqrt
>>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0]    # 90deg rotation about x-axis
>>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0]    # 90deg rotation about y-axis
>>> qangle(q1, q2)
2.0943951023931953

References

Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.

Warning

There is no check that the passed values are unit-quaternions.

qconj(q)[source]

Quaternion conjugate

Parameters: q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – quaternion
Returns: conjugate of input quaternion
Return type: ndarray(4)

Conjugate of quaternion, the vector part is negated.

>>> from spatialmath.base import qconj, qprint
>>> q = [1, 2, 3, 4]
>>> qprint(qconj(q))
 1.0000 < -2.0000, -3.0000, -4.0000 >

SymPy: supported

qdot(q, w)[source]

Rate of change of unit-quaternion

Parameters

q0 (array_like(4)) – unit-quaternion
w (array_like(3)) – 3D angular velocity in world frame

Returns

rate of change of unit quaternion

Return type

ndarray(4)

dot(q, w) is the rate of change of the elements of the unit quaternion q which represents the orientation of a body frame with angular velocity w in the world frame.

>>> from spatialmath.base import qdot, qprint
>>> from math import sqrt
>>> q = [1/sqrt(2), 1/sqrt(2), 0, 0]   # 90deg rotation about x-axis
>>> qdot(q, [1, 2, 3])
array([-0.3536,  0.3536,  1.7678,  0.3536])

Warning

There is no check that the passed values are unit-quaternions.

qdotb(q, w)[source]

Rate of change of unit-quaternion

Parameters

q0 (array_like(4)) – unit-quaternion
w (array_like(3)) – 3D angular velocity in body frame

Returns

rate of change of unit quaternion

Return type

ndarray(4)

dotb(q, w) is the rate of change of the elements of the unit quaternion q which represents the orientation of a body frame with angular velocity w in the body frame.

>>> from spatialmath.base import qdotb, qprint
>>> from math import sqrt
>>> q = [1/sqrt(2), 1/sqrt(2), 0, 0]   # 90deg rotation about x-axis
>>> qdotb(q, [1, 2, 3])
array([-0.3536,  0.3536, -0.3536,  1.7678])

Warning

There is no check that the passed values are unit-quaternions.

qeye()[source]

Create an identity quaternion

Returns: an identity quaternion
Return type: ndarray(4)

Creates an identity quaternion, with the scalar part equal to one, and a zero vector value.

>>> from spatialmath.base import qeye, qprint
>>> q = qeye()
>>> qprint(q)
 1.0000 <  0.0000,  0.0000,  0.0000 >

qinner(q1, q2)[source]

Quaternion inner product

Parameters

q0 (: array_like(4)) – quaternion
q1 (array_like(4)) – uaternion

Returns

inner product

Return type

float

This is the inner or dot product of two quaternions, it is the sum of the element-wise product.

The inner product inner(q, q) is the square of the norm of q.
If q0 and q1 are unit quaternions then the inner product is the cosine of the angle between the two orientations.

>>> from spatialmath.base import qinner
>>> from math import sqrt, acos, pi
>>> q1 = [1, 2, 3, 4]
>>> qinner(q1, q1)                     # square of the norm
30.0
>>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0]  # 90deg rotation about x-axis
>>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0]  # 90deg rotation about y-axis
>>> acos(qinner(q1, q2)) * 180 / pi    # angle between q1 and q2
60.00000000000001

Seealso: qvmul

qisequal(q1, q2, tol=20, unitq=False)[source]

Test if quaternions are equal

Parameters

q1 (array_like(4)) – quaternion
q2 (array_like(4)) – quaternion
unitq (bool) – quaternions are unit quaternions
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether quaternions are equal

Return type

bool

Tests if two quaternions are equal.

For unit-quaternions unitq=True the double mapping is taken into account, that is q and -q represent the same orientation and isequal(q, -q, unitq=True) will return True.

>>> from spatialmath.base import qisequal
>>> q1 = [1, 2, 3, 4]
>>> q2 = [-1, -2, -3, -4]
>>> qisequal(q1, q2)
False
>>> qisequal(q1, q2, unitq=True)
True

qisunit(q, tol=20)[source]

Test if quaternion has unit length

Parameters

v (array_like(4)) – quaternion
tol (float) – tolerance in units of eps, defaults to 20

Returns

whether quaternion has unit length

Return type

bool

>>> from spatialmath.base import qeye, qpure, qisunit
>>> q = qeye()
>>> qisunit(q)
False
>>> q = qpure([1, 2, 3])
>>> qisunit(q)
False

Seealso: qunit()

qmatrix(q)[source]

Convert quaternion to 4x4 matrix equivalent

Parameters: q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – quaternion
Returns: equivalent matrix
Return type: ndarray(4)

Hamilton multiplication between two quaternions can be considered as a matrix-vector product, the left-hand quaternion is represented by an equivalent 4x4 matrix and the right-hand quaternion as 4x1 column vector.

>>> from spatialmath.base import qmatrix, qqmul, qprint
>>> q1 = [1, 2, 3, 4]
>>> q2 = [5, 6, 7, 8]
>>> qqmul(q1, q2)    # conventional Hamilton product
array([-60.,  12.,  30.,  24.])
>>> m = qmatrix(q1)
>>> print(m)
[[ 1. -2. -3. -4.]
 [ 2.  1. -4.  3.]
 [ 3.  4.  1. -2.]
 [ 4. -3.  2.  1.]]
>>> v = m @ np.array(q2)
>>> print(v)
[-60.  12.  30.  24.]

Seealso: qqmul

qnorm(q)[source]

Norm of a quaternion

Parameters: q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – quaternion
Returns: norm of the quaternion
Return type: float

Returns the norm (length or magnitude) of the input quaternion which is

\[(s^2 + v_x^2 + v_y^2 + v_z^2)^{1/2}\]

>>> from spatialmath.base import qnorm
>>> q = qnorm([1, 2, 3, 4])
>>> print(q)
5.477225575051661

Seealso: qunit()

qpositive(q)[source]

Quaternion with positive scalar part

Parameters: q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – quaternion
Returns: pure quaternion
Return type: ndarray(4)

If the scalar part is negative return -q.

qpow(q, power)[source]

Raise quaternion to a power

Parameters

q (Union[List, Tuple[float, float, float, float], ndarray[Any, dtype[TypeVar(ScalarType, bound= generic, covariant=True)]]]) – quaternion
power (int) – exponent

Returns

input quaternion raised to the specified power

Return type

ndarray(4)

Raises

ValueError – if exponent is non integer

Raises a quaternion to the specified power using repeated multiplication.

>>> from spatialmath.base import qpow, qqmul, qprint
>>> q = [1, 2, 3, 4]
>>> qprint(qqmul(q, q))
-28.0000 <  4.0000,  6.0000,  8.0000 >
>>> qprint(qpow(q, 2))
-28.0000 <  4.0000,  6.0000,  8.0000 >
>>> qprint(qpow(q, -2)) # conjugate of above
-28.0000 < -4.0000, -6.0000, -8.0000 >

Seealso: qqmul()
SymPy: supported for q but not power.

qprint(q, delim=('<', '>'), fmt='{: .4f}', file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>)[source]

Format a quaternion

Parameters

q (array_like(4)) – unit-quaternion
delim (list or tuple of strings) – 2-list of delimeters [default (‘<’, ‘>’)]
fmt (str) – printf-style format soecifier [default ‘{: .4f}’]
file (file object) – destination for formatted string [default sys.stdout]

Returns

formatted string

Return type

str

Format the quaternion in a human-readable form as:

S  D1  VX VY VZ D2

where S, VX, VY, VZ are the quaternion elements, and D1 and D2 are a pair of delimeters given by delim.

By default the string is written to sys.stdout.

If file=None then a string is returned.

>>> from spatialmath.base import qprint, qrand
>>> q = [1, 2, 3, 4]
>>> qprint(q)
 1.0000 <  2.0000,  3.0000,  4.0000 >
>>> q = qrand()   # a unit quaternion
>>> qprint(q, delim=('<<', '>>'))
-0.5498 <<  0.3563, -0.1463, -0.7412 >>

qpure(v)[source]

Create a pure quaternion

Parameters: v (array_like(3)) – 3D vector
Returns: pure quaternion
Return type: ndarray(4)

Creates a pure quaternion, with a zero scalar value and the vector part equal to the passed vector value.

  File "<input>", line 1, in <module>
NameError: name 'qpure' is not defined
Traceback (most recent call last):
  File "<input>", line 1, in <module>
NameError: name 'qprint' is not defined

qqmul(q1, q2)[source]

Quaternion multiplication

Parameters

q0 (: array_like(4)) – left-hand quaternion
q1 (array_like(4)) – right-hand quaternion

Returns

quaternion product

Return type

ndarray(4)

This is the quaternion or Hamilton product. If both operands are unit-quaternions then the product will be a unit-quaternion.

>>> from spatialmath.base import qqmul
>>> q1 = [1, 2, 3, 4]
>>> q2 = [5, 6, 7, 8]
>>> qqmul(q1, q2)    # conventional Hamilton product
array([-60.,  12.,  30.,  24.])

Seealso: qvmul, qinner, vvmul

qrand()[source]

Random unit-quaternion

Returns: random unit-quaternion
Return type: ndarray(4)

Computes a uniformly distributed random unit-quaternion which can be considered equivalent to a random SO(3) rotation.

>>> from spatialmath.base import qrand, qprint
>>> qprint(qrand())
-0.3974 < -0.0791, -0.1223, -0.9060 >

qslerp(q0, q1, s, shortest=False, tol=20)[source]

Quaternion conjugate

Parameters

q0 (array_like(4)) – initial unit quaternion
q1 (array_like(4)) – final unit quaternion
s (float) – interpolation coefficient in the range [0,1]
shortest (bool) – choose shortest distance [default False]
tol (float, optional) – Tolerance when checking for identical quaternions, in multiples of eps, defaults to 20

Returns

interpolated unit-quaternion

Return type

ndarray(4)

Raises

ValueError – s is outside interval [0, 1]

An interpolated quaternion between q0 when s = 0 to q1 when s = 1.

Interpolation is performed on a great circle on a 4D hypersphere. This is a rotation about a single fixed axis in space which yields the straightest and shortest path between two points.

For large rotations the path may be the long way around the circle, the option 'shortest' ensures always the shortest path.

>>> from spatialmath.base import qslerp, qprint
>>> from math import sqrt
>>> q0 = [1/sqrt(2), 1/sqrt(2), 0, 0]  # 90deg rotation about x-axis
>>> q1 = [1/sqrt(2), 0, 1/sqrt(2), 0]  # 90deg rotation about y-axis
>>> qprint(qslerp(q0, q1, 0))           # this is q0
 0.7071 <  0.7071,  0.0000,  0.0000 >
>>> qprint(qslerp(q0, q1, 1))           # this is q1
 0.7071 <  0.0000,  0.7071,  0.0000 >
>>> qprint(qslerp(q0, q1, 0.5))         # this is in "half way" between
 0.8165 <  0.4082,  0.4082,  0.0000 >

Warning

There is no check that the passed values are unit-quaternions.

qunit(q, tol=20)[source]

Create a unit quaternion

Parameters

v (array_like(4)) – quaterion
tol (float, optional) – Tolerance in multiples of eps, defaults to 20

Returns

a pure quaternion

Return type

ndarray(4)

Raises

ValueError – quaternion has (near) zero norm

Creates a unit quaternion, with unit norm, by scaling the input quaternion.

>>> from spatialmath.base import qunit, qprint
>>> q = qunit([1, 2, 3, 4])
>>> qprint(q)
 0.1826 <  0.3651,  0.5477,  0.7303 >

Note

Scalar part is always positive.

Note

If the quaternion norm is less than tol * eps an exception is raised.

Seealso: qnorm()

qvmul(q, v)[source]

Vector rotation

Parameters

q (array_like(4)) – unit-quaternion
v (array_like(3)) – 3-vector to be rotated

Returns

rotated 3-vector

Return type

ndarray(3)

The vector v is rotated about the origin by the SO(3) equivalent of the unit quaternion.

>>> from spatialmath.base import qvmul
>>> from math import sqrt
>>> q = [1/sqrt(2), 1/sqrt(2), 0, 0]  # 90deg rotation about x-axis
>>> qvmul(q, [1, 2, 3])              # rotated vector
array([ 1., -3.,  2.])

Warning

There is no check that the passed value is a unit-quaternions.

Seealso: qvmul

r2q(R, check=False, tol=20, order='sxyz', shortest=False)[source]

Convert SO(3) rotation matrix to unit-quaternion

Parameters

R (ndarray(3,3)) – SO(3) rotation matrix
check (bool) – check validity of rotation matrix, default False
tol (float) – tolerance in units of eps, defaults to 20
order (str) – the order of the returned quaternion elements. Must be ‘sxyz’ or ‘xyzs’. Defaults to ‘sxyz’.
shortest (bool, default to False) – ensures the quaternion has non-negative scalar part.

Returns

unit-quaternion as Euler parameters

Return type

ndarray(4)

Raises

ValueError – for non SO(3) argument

Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.

>>> from spatialmath.base import r2q, qprint, rotx
>>> R = rotx(90, 'deg') # rotation of 90deg about x-axis
>>> print(R)
[[ 1.  0.  0.]
 [ 0.  0. -1.]
 [ 0.  1.  0.]]
>>> qprint(r2q(R))
 0.7071 <  0.7071,  0.0000,  0.0000 >

Warning

There is no check that the passed matrix is a valid rotation matrix.

Note

Scalar part is always positive
implements Cayley’s method

Reference

Sarabandi, S., and Thomas, F. (March 1, 2019). “A Survey on the Computation of Quaternions From Rotation Matrices.” ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006. doi.org/10.1115/1.4041889

Seealso

q2r()

v2q(v)[source]

Convert 3-vector to unit-quaternion

Parameters: v (array_like(3)) – vector part of unit quaternion
Returns: a unit quaternion
Return type: ndarray(4)

Returns a unit-quaternion reconsituted from just its vector part. Assumes that the scalar part was positive, so \(s = \sqrt{1-||v||}\).

>>> from spatialmath.base import v2q, qprint
>>> from math import sqrt
>>> v = [0, 1 / sqrt(2), 0]
>>> qprint(v2q(v))
 0.7071 <  0.0000,  0.7071,  0.0000 >
>>> v = [0, -1 / sqrt(2), 0]
>>> qprint(v2q(v))
 0.7071 <  0.0000, -0.7071,  0.0000 >

Warning

There is no check that the value is the vector part of a unit-quaternion, and this can lead to a math domain error.

Seealso: q2v()

vvmul(qa, qb)[source]

Quaternion multiplication

Parameters

qa (: array_like(3)) – left-hand quaternion
qb (array_like(3)) – right-hand quaternion

Returns

quaternion product

Return type

ndarray(3)

This is the quaternion or Hamilton product of unit-quaternions defined only by their vector components. The product will be a unit-quaternion, defined only by its vector component.

>>> from spatialmath.base import vvmul, v2q, q2v, qqmul, qprint
>>> from math import sqrt
>>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0]  # 90deg rotation about x-axis
>>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0]  # 90deg rotation about y-axis
>>> qprint(qqmul(q1, q2))              # normal Hamilton product
 0.5000 <  0.5000,  0.5000,  0.5000 >
>>> v1 = q2v(q1); v2 = q2v(q2)
>>> vp = vvmul(v1, v2)                 # product using 3-vectors
>>> qprint(v2q(vp))                    # same answer as Hamilton product
 0.5000 <  0.5000,  0.5000,  0.5000 >

Seealso: q2v() v2q() qvmul()