Unit quaternion

class UnitQuaternion(s=None, v=None, norm=True, check=True)

Bases: spatialmath.quaternion.Quaternion

Unit quaternion class

A unit quaternion can be considered an ordered pair \((s, \vec{v})\) where \(s \in \mathbb{R}\) is the scalar part and \(\vec{v} = (v_x, v_y, v_z) \in \mathbb{R}^3\) is the vector part and is often written as

\[\q = s \langle v_x, v_y, v_z \rangle\]

and subject to a unit-length constraint \(s^2+v_x^2+v_y^2+v_z^2 = 1\).

A unit-quaternion can be considered as a rotation \(\theta\) about the vector \(\vec{v}\), so the unit quaternion can also be written as

\[\q = \cos \frac{\theta}{2} \sin \frac{\theta}{2} <v_x v_y v_z>\]

The quaternion \(\q\) and \(-\q\) represent the equivalent rotation, and this is referred to as a double mapping.

The UnitQuaternion class inherits many methods from the Quaternion class

classmethod Alloc(n=1)

Construct an instance with N default values (BasePoseList superclass method)

Parameters

n (int, optional) – Number of values, defaults to 1

Return type

Self

Returns

pose instance with n default values

X.Alloc(N) creates an instance of the pose class X with N default values, ie. len(X) will be N.

X can be considered a vector of pose objects, and those elements can be referenced X[i] or assigned to X[i] = ....

Note

The default value depends on the pose class and is the result of the empty constructor. For SO2, SE2, SO3, SE3 it is an identity matrix, for a twist class Twist2 or Twist3 it is a zero vector, for a UnitQuaternion or Quaternion it is a zero vector.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10

where X is any of the SMTB classes.

classmethod AngVec(theta, v, *, unit='rad')

Construct a new unit quaternion from rotation angle and axis

Parameters

• theta (float) – rotation

• unit (str) – angular units: ‘rad’ [default], or ‘deg’

• v (array_like) – rotation axis, 3-vector

Returns

unit-quaternion

Return type

UnitQuaternion instance

UnitQuaternion.AngVec(θ, v) is a unit quaternion that describes the 3D rotation defined by a rotation of θ about the 3-vector v.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.AngVec(0, [1,0,0]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
>>> print(UQ.AngVec(90, [1,0,0], unit='deg'))
 0.7071 <<  0.7071,  0.0000,  0.0000 >>

Note

\(\theta = 0\) the result in an identity quaternion, otherwise V must have a finite length, ie. \(|V| > 0\).

Seealso

UnitQuaternion.angvec() UnitQuaternion.exp() angvec2r()

classmethod Empty()

Construct an empty instance (BasePoseList superclass method)

Return type

Self

Returns

pose instance with zero values

Example:

>>> x = X.Empty()
>>> len(x)
0

where X is any of the SMTB classes.

classmethod Eul(*angles, unit='rad')

Construct a new unit quaternion from Euler angles

Parameters

• 𝚪 (array_like) – 3-vector of Euler angles

• unit (str) – angular units: ‘rad’ [default], or ‘deg’

Returns

unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion.Eul(𝚪) is a unit quaternion that describes the 3D rotation defined by a 3-vector of Euler angles \(\Gamma = (\phi, \theta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

• UnitQuaternion.Eul(φ, θ, ψ) as above but the angles are provided as three scalars.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Eul([0.1, 0.2, 0.3]))
 0.9752 <<  0.0100,  0.0993,  0.1977 >>

Seealso

UnitQuaternion.RPY() SE3.eul() SE3.Eul() eul2r()

classmethod EulerVec(w)

Construct a new unit quaternion from an Euler rotation vector

Parameters

ω (3-element array_like) – rotation axis

Returns

unit-quaternion

Return type

UnitQuaternion instance

UnitQuaternion.EulerVec(ω) is a unit quaternion that describes the 3D rotation defined by a rotation of \(\theta = \lVert \omega \rVert\) about the unit 3-vector \(\omega / \lVert \omega \rVert\).

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.EulerVec([0.5,0,0]))
 0.9689 <<  0.2474,  0.0000,  0.0000 >>

Note

\(\theta \eq 0\) the result in an identity matrix, otherwise V must have a finite length, ie. \(|V| > 0\).

Seealso

SE3.angvec() angvec2r()

classmethod OA(o, a)

Construct a new unit quaternion from two vectors

Parameters

• o (array_like) – 3-vector parallel to Y- axis

• a (array_like) – 3-vector parallel to the Z-axis

Returns

unit-quaternion

Return type

UnitQuaternion instance

UnitQuaternion.OA(O, A) is a unit quaternion that describes the 3D rotation defined in terms of vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are respectively called the orientation and approach vectors defined such that R = [N O A] and N = O x A.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.OA([0,0,-1], [0,1,0]))
 0.7071 << -0.7071,  0.0000, -0.0000 >>

Note

• Only the A vector is guaranteed to have the same direction in the resulting

rotation matrix - O and A do not have to be unit-length, they are normalized - O and ``A` do not have to be orthogonal, so long as they are not parallel

Seealso

oa2r()

classmethod Pure(v)

Construct a pure quaternion from a vector

Parameters

v (3-element array_like) – vector

Quaternion.Pure(v) is a Quaternion with a zero scalar part and the vector part set to v, ie. \(q = 0 \langle v_x, v_y, v_z \rangle\)

Example:

>>> from spatialmath import Quaternion
>>> print(Quaternion.Pure([1,2,3]))
 0.0000 <  1.0000,  2.0000,  3.0000 >

Return type

Quaternion

classmethod RPY(*angles, order='zyx', unit='rad')

Construct a new unit quaternion from roll-pitch-yaw angles

Parameters

• 𝚪 (array_like) – 3-vector of roll-pitch-yaw angles

• unit (str) – angular units: ‘rad’ [default], or ‘deg’

• unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

Returns

unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion.RPY(𝚪) is a unit quaternion that describes the 3D rotation defined by a 3-vector of roll, pitch, yaw angles \(\Gamma = (r, p, y)\) which correspond to successive rotations about the axes specified by order:

  • 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, then by roll about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways.

  • 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis, then by roll about the new z-axis. Convention for a robot gripper with z-axis forward and y-axis between the gripper fingers.

  • 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis, then by roll about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows.

• UnitQuaternion.RPY(⍺, β, 𝛾) as above but the angles are provided as three scalars.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.RPY([0.1, 0.2, 0.3]))
 0.9833 <<  0.0343,  0.1060,  0.1436 >>

Seealso

UnitQuaternion.Eul() SE3.rpy() SE3.RPY() rpy2r()

classmethod Rand(N=1)

Construct a new random unit quaternion

Parameters

N (int) – number of random rotations

Returns

random unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion.Rand() is a uniformly distributed random unit quaternion value.

• SO3.Rand(N) is a unit quaternion instance containing a sequence of N random unit quaternion values.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rand())
 0.1270 <<  0.6215, -0.4363, -0.6382 >>
>>> print(UQ.Rand(3))
 0.4048 <<  0.3095,  0.8225,  0.2526 >>
 0.9060 << -0.0478, -0.0623,  0.4159 >>
 0.6917 << -0.3128, -0.5239,  0.3864 >>

Seealso

UnitQuaternion.Rand()

classmethod Rx(angles, unit='rad')

Construct a UnitQuaternion object representing rotation about the X-axis

Parameters

• θ (array_like) – rotation angle

• unit (str) – rotation unit ‘rad’ [default] or ‘deg’

Returns

unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion(θ) constructs a unit quaternion representing a rotation of θ radians about the X-axis.

• UnitQuaternion(θ, 'deg') constructs a unit quaternion representing a rotation of θ degrees about the X-axis.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3))
 0.9888 <<  0.1494,  0.0000,  0.0000 >>
>>> print(UQ.Rx([0, 0.3, 0.6]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 0.9888 <<  0.1494,  0.0000,  0.0000 >>
 0.9553 <<  0.2955,  0.0000,  0.0000 >>

classmethod Ry(angles, unit='rad')

Construct a UnitQuaternion object representing rotation about the Y-axis

Parameters

• θ (array_like) – rotation angle

• unit (str) – rotation unit ‘rad’ [default] or ‘deg’

Returns

unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion(θ) constructs a unit quaternion representing a rotation of θ radians about the Y-axis.

• UnitQuaternion(θ, 'deg') constructs a unit quaternion representing a rotation of θ degrees about the Y-axis.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Ry(0.3))
 0.9888 <<  0.0000,  0.1494,  0.0000 >>
>>> print(UQ.Ry([0, 0.3, 0.6]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 0.9888 <<  0.0000,  0.1494,  0.0000 >>
 0.9553 <<  0.0000,  0.2955,  0.0000 >>

classmethod Rz(angles, unit='rad')

Construct a UnitQuaternion object representing rotation about the Z-axis

Parameters

• θ (array_like) – rotation angle

• unit (str) – rotation unit ‘rad’ [default] or ‘deg’

Returns

unit-quaternion

Return type

UnitQuaternion instance

• UnitQuaternion(θ) constructs a unit quaternion representing a rotation of θ radians about the Z-axis.

• UnitQuaternion(θ, 'deg') constructs a unit quaternion representing a rotation of θ degrees about the Z-axis.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rz(0.3))
 0.9888 <<  0.0000,  0.0000,  0.1494 >>
>>> print(UQ.Rz([0, 0.3, 0.6]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 0.9888 <<  0.0000,  0.0000,  0.1494 >>
 0.9553 <<  0.0000,  0.0000,  0.2955 >>

SE3()

Unit quaternion as SE3 instance

Returns

an SE(3) representation

Return type

SE3 instance

q.SE3() is an SE3 instance representing the same rotation as the unit quaternion q and with zero translation.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).SE3()
SE3(array([[ 0.9553, -0.2955,  0.    ,  0.    ],
           [ 0.2955,  0.9553,  0.    ,  0.    ],
           [ 0.    ,  0.    ,  1.    ,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))

SO3()

Unit quaternion as SO3 instance

Returns

an SO(3) representation

Return type

SO3 instance

q.SO3() is an SO3 instance representing the same rotation as the unit quaternion q.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).SO3()
SO3(array([[ 0.9553, -0.2955,  0.    ],
           [ 0.2955,  0.9553,  0.    ],
           [ 0.    ,  0.    ,  1.    ]]))

classmethod Vec3(vec)

Construct a new unit quaternion from its vector part

Parameters

vec (3-element array_like) – vector part of unit quaternion

UnitQuaternion.Vec(v) is a new unit quaternion with the specified vector part and the scalar part is

\[s = \sqrt{1 - v_x^2 - v_y^2 - v_z^2}\]

The unit quaternion will always have a positive scalar part.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rz(-4)
>>> print(q)
 0.4161 << -0.0000, -0.0000,  0.9093 >>
>>> q.vec3
array([-0.    , -0.    ,  0.9093])
>>> q2 = UQ.Vec3(q.vec3)
>>> print(q2)
 0.4161 << -0.0000, -0.0000,  0.9093 >>
>>> q == q2
True

Seealso

UnitQuaternion.vec3()

Return type

UnitQuaternion

__add__(right)

Overloaded + operator

Returns

sum

Return type

Quaternion, UnitQuaternion

Raises

ValueError

Operands		Sum
left	right	type	result
Quaternion	Quaternion	Quaternion	elementwise sum
Quaternion	UnitQuaternion	Quaternion	elementwise sum
Quaternion	scalar	Quaternion	add to each element
UnitQuaternion	Quaternion	Quaternion	elementwise sum
UnitQuaternion	UnitQuaternion	Quaternion	elementwise sum
UnitQuaternion	scalar	Quaternion	add to each element

Any other input combinations result in a ValueError.

Note that left and right can have a length greater than 1 in which case:

left	right	len	operation
1	1	1	sum = left + right
1	N	N	sum[i] = left + right[i]
N	1	N	sum[i] = left[i] + right
N	N	N	sum[i] = left[i] + right[i]
N	M		ValueError

A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]) + Quaternion([5,6,7,8])
Quaternion(array([ 6,  8, 10, 12]))
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) + Quaternion([1,2,3,4])
Quaternion([
  array([2, 4, 6, 8]),
  array([ 6,  8, 10, 12]) ])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) + Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
Quaternion([
  array([2, 4, 6, 8]),
  array([10, 12, 14, 16]) ])

__eq__(right)

Overloaded == operator

Return type

bool

q1 == q2 is True if q1 is elementwise equal to q2 and accounts for the double mapping. Supports broadcasting.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(0.3)
>>> q1 == q1
True
>>> q1 == (-q1)
True
>>> q1 == q2
False
>>> UQ([q1, q2]) == q1
[True, False]
>>> UQ([q1, q2]) == q2
[False, True]
>>> UQ([q1, q2]) == UQ([q1, q2])
[True, True]

Seealso

__ne__() qisequal()

__init__(s=None, v=None, norm=True, check=True)

Construct a UnitQuaternion instance

Parameters

• norm (bool) – explicitly normalize the quaternion [default True]

• check (bool) – explicitly check validity of argument [default True]

Returns

unit-quaternion

Return type

UnitQuaternion instance

Raises

ValueError

• UnitQuaternion() constructs the identity quaternion 1<0,0,0>

• UnitQuaternion(s, v) constructs a unit quaternion with specified real s and v vector parts. v is a 3-vector given as a list, tuple, or ndarray(3). If norm is True the resulting quaternion is normalized.

• UnitQuaternion(v) constructs a unit quaternion with specified elements from v which is a 4-vector given as a list, tuple, or ndarray(4). Also known as the Euler parameters.

• UnitQuaternion(M) construct a new unit quaternion with N values where Q is a Nx4 NumPy array whose rows are the quaternion in vector form

• UnitQuaternion(R) constructs a unit quaternion from an SO(3) rotation matrix given as a ndarray(3,3). If check is True test the rotation submatrix for orthogonality.

• UnitQuaternion(X) constructs a unit quaternion from the rotational part of X which is an SO3 or SE3 instance. If len(X) > 1 then the resulting unit quaternion is of the same length.

• UnitQuaternion([q1, q2 .. qN]) construct a new unit quaternion with N values where each element is a 4-vector

• UnitQuaternion([Q1, Q2 .. QN]) construct a new unit quaternion with N values where each element is a UnitQuaternion instance

• UnitQuaternion([X1, X2 .. XN]) construct a new unit quaternion with N values where each element is an SO3 or SE3 instance

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ()
>>> q         # repr()
UnitQuaternion(array([1., 0., 0., 0.]))
>>> print(q)  # str()
 1.0000 <<  0.0000,  0.0000,  0.0000 >>

__mul__(right)

Multiply unit quaternion

Returns

product

Return type

Quaternion, UnitQuaternion

Raises

ValueError

Multiplicands		Product
left	right	type	result
UnitQuaternion	Quaternion	Quaternion	Hamilton product
UnitQuaternion	UnitQuaternion	UnitQuaternion	Hamilton product
UnitQuaternion	scalar	Quaternion	scalar product
UnitQuaternion	3-vector	3-vector	vector rotation
UnitQuaternion	3xN array	3xN array	vector rotations

Any other input combinations result in a ValueError.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) * UQ.Rx(0.4))
 0.9394 <<  0.3429,  0.0000,  0.0000 >>
>>> print(UQ.Rx(0.3) * 2)
 1.9775 <  0.2989,  0.0000,  0.0000 >
>>> print(UQ.Rx(0.3) * [1, 2, 3])
[1.     1.0241 3.457 ]

Note that left and right can have a length greater than 1 in which case:

left	right	len	operation
1	1	1	prod = left * right
1	N	N	prod[i] = left * right[i]
N	1	N	prod[i] = left[i] * right
N	N	N	prod[i] = left[i] * right[i]
N	M	n/a	ValueError

A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

Example:

Seealso

Quaternion.__mul__()

__ne__(right)

Overloaded != operator

Return type

bool

q1 != q2 is True if q1 is elementwise not equal to q2 and accounts for the double mapping. Supports broadcasting.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(0.3)
>>> q1 != q1
False
>>> q1 != (-q1)
False
>>> q1 != q2
True
>>> UQ([q1, q2]) == q1
[True, False]
>>> UQ([q1, q2]) == q2
[False, True]
>>> UQ([q1, q2]) == UQ([q1, q2])
[True, True]

Seealso

__eq__() qisequal()

__pow__(n)

Overloaded ** operator

Return type

Quaternion instance

q ** N computes the product of q with itself N-1 times, where N must be an integer. If ``N``<0 the result is conjugated.

Example:

>>> from spatialmath import Quaternion
>>> print(Quaternion([1,2,3,4]) ** 2)
-28.0000 <  4.0000,  6.0000,  8.0000 >
>>> print(Quaternion([1,2,3,4]) ** -1)
 1.0000 < -2.0000, -3.0000, -4.0000 >
>>> print(Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) ** 2)
-28.0000 <  4.0000,  6.0000,  8.0000 >
-124.0000 <  60.0000,  70.0000,  80.0000 >

Seealso

qpow()

__sub__(right)

Overloaded - operator

Returns

difference

Return type

Quaternion, UnitQuaternion

Raises

ValueError

Operands		Difference
left	right	type	result
Quaternion	Quaternion	Quaternion	elementwise sum
Quaternion	UnitQuaternion	Quaternion	elementwise sum
Quaternion	scalar	Quaternion	subtract from each element
UnitQuaternion	Quaternion	Quaternion	elementwise sum
UnitQuaternion	UnitQuaternion	Quaternion	elementwise sum
UnitQuaternion	scalar	Quaternion	subtract from each element

Any other input combinations result in a ValueError.

Note that left and right can have a length greater than 1 in which case:

left	right	len	operation
1	1	1	diff = left - right
1	N	N	diff[i] = left - right[i]
N	1	N	diff[i] = left[i] - right
N	N	N	diff[i] = left[i] - right[i]
N	M		ValueError

A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]) - Quaternion([5,6,7,8])
Quaternion(array([-4, -4, -4, -4]))
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) - Quaternion([1,2,3,4])
Quaternion([
  array([0, 0, 0, 0]),
  array([4, 4, 4, 4]) ])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) - Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
Quaternion([
  array([0, 0, 0, 0]),
  array([0, 0, 0, 0]) ])

__truediv__(right)

Overloaded / operator

Return type

Quaternion or UnitQuaternion

• q1 / q2 is equivalent to q1 * q1.inv().

• q / s performs elementwise division of the elements of q by s. This is not a group operation so the result will be a Quaternion.

Multiplicands		Quotient
left	right	type	result
UnitQuaternion	UnitQuaternion	UnitQuaternion	Hamilton product by inverse
UnitQuaternion	scalar	Quaternion	element-wise division

Any other input combinations result in a ValueError.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) / UQ.Rx(0.3))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
>>> print(UQ.Rx(0.3) / 2)
 0.4944 <  0.0747,  0.0000,  0.0000 >

For pose composition either or both operands may hold more than one value which results in the composition holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	quo = left * right.inv()
1	M	M	quo[i] = left * right[i].inv()
N	1	M	quo[i] = left[i] * right.inv()
M	M	M	quo[i] = left[i] * right[i].inv()

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) / UQ.Rx(0.3))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
>>> print(UQ.Rx([0.3, 0.6]) / UQ.Rx(0.3))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 0.9888 <<  0.1494,  0.0000,  0.0000 >>
>>> print(UQ.Rx(0.3) / UQ.Rx([0.3, 0.6]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 0.9888 << -0.1494,  0.0000,  0.0000 >>
>>> print(UQ.Rx([0.3, 0.6]) / UQ.Rx([0.3, 0.6]))
 1.0000 <<  0.0000,  0.0000,  0.0000 >>
 1.0000 <<  0.0000,  0.0000,  0.0000 >>

angdist(other, metric=3)

Angular distance metric between unit quaternions

Parameters

• other (UnitQuaternion instance) – second unit quaternion

• metric (int) – metric, default is 3

Raises

TypeError – if other is not a UnitQuaternion

Returns

angle in radians

Return type

float

q1.angdist(q2) is the geodesic norm, or geodesic distance between two unit quaternions. We can consider it as the angle between two quaternions.

Several metrics are supported:

Metric	Details
0	\(1 - | \q_1 \bullet \q_2 | \in [0, 1]\)
1	\(\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]\)
2	\(\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]\)
3	\(2 \tan^{-1} \| \q_1 \pm \q_2\| / \|\q_1 \mp \q_2\| \in [0, \pi/2]\)
4	\(\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]\)

Metric 3 computes the sum and difference of the quaternions and uses the largest value in the denominator.

Example:

>>> from spatialmath import UnitQuaternion
>>> q1 = UnitQuaternion.Rx(0.3)
>>> q2 = UnitQuaternion.Ry(0.3)
>>> print(q1.angdist(q1))
0.0
>>> print(q1.angdist(q2))
0.2117327177378023

Note

• metrics 1, 2, 4 can throw ValueError “math domain error” due to numeric errors which push the argument of acos() marginally outside its domain [0, 1].

• metrics 2 and 3 are equivalent, but 3 is more robust

• SMTB-MATLAB uses metric 3 for UnitQuaternion.angle()

• MATLAB’s quaternion.dist() uses metric 4

angvec(unit='rad')

Unit quaternion as angle and rotation vector

Parameters

• unit (str) – angular units: ‘rad’ [default], or ‘deg’

• check (bool) – check that rotation matrix is valid

Returns

\((\theta, {\bf v})\)

Return type

float, ndarray(3)

q.angvec() is a tuple \((\theta, v)\) containing the rotation angle and a rotation axis which is equivalent to the rotation of the unit quaternion q.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).angvec()
(0.3000000000000001, array([0., 0., 1.]))

Seealso

Quaternion.AngVec() UnitQuaternion.log() angvec2r()

animate(*args, **kwargs)

Plot unit quaternion as an animated coordinate frame

Parameters

• start (UnitQuaternion) – initial pose, defaults to null/identity

• **kwargs – plotting options

• q.animate() displays the orientation q as a coordinate frame moving from the origin in either 3D. There are many options, see the links below.

• q.animate(*args, start=q1) displays the orientation q as a coordinate frame moving from orientation q11, in 3D. There are many options, see the links below.

Example:

>>> X = UQ.Rx(0.3)
>>> X.animate(frame='A', color='green')
>>> X.animate(start=UQ.Ry(0.2))

:see tranimate() trplot()

append(item)

Append a value to an instance (BasePoseList superclass method)

Parameters

x (Quaternion or UnitQuaternion instance) – the value to append

Raises

ValueError – incorrect type of appended object

Appends the argument to the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.append(X())   # append to the list
>>> len(x)
11

where X is any of the SMTB classes.

Return type

None

clear() → None -- remove all items from S

conj()

Conjugate of quaternion

Return type

Quaternion instance

q.conj() is the quaternion q with the vector part negated, ie. \(q = s \langle -v_x, -v_y, -v_z \rangle\)

Example:

>>> from spatialmath import Quaternion
>>> print(Quaternion.Pure([1,2,3]).conj())
 0.0000 < -1.0000, -2.0000, -3.0000 >

Seealso

qconj()

dot(omega)

Rate of change of a unit quaternion in world frame

Parameters

ω (3-element array_like) – angular velocity in world frame

Returns

rate of change of unit quaternion

Return type

ndarray(4)

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

Seealso

qdot()

dotb(omega)

Rate of change of a unit quaternion in body frame

Parameters

ω (3-element array_like) – angular velocity in body frame

Returns

rate of change of unit quaternion

Return type

ndarray(4)

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

Seealso

qdotb()

eul(unit='rad')

Unit quaternion as Euler angles

Parameters

unit (str) – angular units: ‘rad’ [default], or ‘deg’

Returns

3-vector of Euler angles

Return type

ndarray(3)

q.eul is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, \theta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

If len(x) is:

• 1, return an ndarray with shape=(3,)

• N>1, return ndarray with shape=(N,3)

• ndarray with shape=(3,), if len(R) == 1

• ndarray with shape=(N,3), if len(R) = N > 1

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).eul()
array([0. , 0. , 0.3])
>>> UQ.Ry([0.3, 0.4]).eul()
array([[0. , 0.3, 0. ],
       [0. , 0.4, 0. ]])

Seealso

SE3.Eul() tr2eul()

exp(tol=20)

Exponential of quaternion

Parameters

tol (float, optional) – Tolerance when checking for pure quaternion, in multiples of eps, defaults to 20

Return type

Quaternion instance

q.exp() is the exponential of the quaternion q, ie.

\[e^s \cos \| v \|, \langle e^s \frac{\vec{v}}{\| \vec{v} \|} \sin \| \vec{v} \| \rangle\]

For a pure quaternion with vector value \(\vec{v}\) the the result is a unit quaternion equivalent to a rotation defined by \(2\vec{v}\) intepretted as an Euler vector, that is, parallel to the axis of rotation and whose norm is the magnitude of rotation.

Example:

>>> from spatialmath import Quaternion
>>> from math import pi
>>> q = Quaternion([1, 2, 3, 4])
>>> print(q.exp())
 1.6939 < -0.7896, -1.1843, -1.5791 >
>>> q = Quaternion.Pure([pi / 4, 0, 0])
>>> print(q.exp())  # result is a UnitQuaternion
 0.7071 <<  0.7071,  0.0000,  0.0000 >>
>>> print(q.exp().angvec())
(1.5707963267948963, array([1., 0., 0.]))

Reference

Wikipedia

Seealso

Quaternion.log() UnitQuaternion.log() UnitQuaternion.AngVec() UnitQuaternion.EulerVec()

extend(iterable)

Extend sequence of values in an instance (BasePoseList superclass method)

Parameters

x (instance of same type) – the value to extend

Raises

ValueError – incorrect type of appended object

Appends the argument’s values to the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.append(X.Alloc(5))   # extend the list
>>> len(x)
15

where X is any of the SMTB classes.

Return type

None

increment(w, normalize=False)

Quaternion incremental update

Parameters

• w (array_like(3)) – angular displacement, Euler vector

• normalize (bool, optional) – normalize the result, defaults to False

Note

The object state is updated

Return type

None

inner(other)

Inner product of quaternions

Return type

float

q1.inner(q2) is the dot product of the equivalent vectors, ie. numpy.dot(q1.vec, q2.vec). The value of q.inner(q) is the same as q.norm ** 2.

Example:

Seealso

qinner()

insert(i, item)

Insert a value to an instance (BasePoseList superclass method)

Parameters

• i (int) – element to insert value before

• item (instance of same type) – the value to insert

Raises

ValueError – incorrect type of inserted value

Inserts the argument into the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.insert(0, X())   # insert at start of list
>>> len(x)
11
>>> x.insert(10, X())   # append to the list
>>> len(x)
11

where X is any of the SMTB classes.

Note

If i is beyond the end of the list, the item is appended to the list

Return type

None

interp(end, s=0, shortest=False)

Interpolate between two unit quaternions

Parameters

• end (UnitQuaternion) – final unit quaternion

• shortest (Optional[bool]) – Take the shortest path along the great circle

• s (array_like or int) – interpolation coefficient, range 0 to 1, or number of steps

Returns

interpolated unit quaternion

Return type

UnitQuaternion instance

• q0.interp(q1, s) is a unit quaternion that is interpolated between q0 when s=0 and q1 when s=1. Spherical linear interpolation (slerp) is used. If s is an ndarray(n) then the result will be a UnitQuaternion with n values.

• q0.interp(q1, N) interpolate between q0 and q1 in N steps.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3); q2 = UQ.Rz(-0.4)
>>> print(q1)
 0.9888 <<  0.1494,  0.0000,  0.0000 >>
>>> print(q2)
 0.9801 <<  0.0000,  0.0000, -0.1987 >>
>>> q1.interp(q2, 0)    # this is q1
UnitQuaternion(array([0.9888, 0.1494, 0.    , 0.    ]))
>>> q1.interp(q2, 1,)   # this is q2
UnitQuaternion(array([ 0.9801,  0.    ,  0.    , -0.1987]))
>>> q1.interp(q2, 0.5)  # this is in between
UnitQuaternion(array([ 0.9921,  0.0753,  0.    , -0.1001]))
>>> q = q1.interp(q2, 11)  # in 11 steps
>>> len(q)
11
>>> q[0]                # this is q1
UnitQuaternion(array([0.9888, 0.1494, 0.    , 0.    ]))
>>> q[5]                # this is in between
UnitQuaternion(array([ 0.9921,  0.0753,  0.    , -0.1001]))

Note

values of s are silently clipped to the range [0, 1]

Seealso

qslerp()

interp1(s=0, shortest=False)

Interpolate a unit quaternion

Parameters

• shortest (Optional[bool]) – Take the shortest path along the great circle

• s (array_like or int) – interpolation coefficient, range 0 to 1, or number of steps

Returns

interpolated unit quaternion

Return type

UnitQuaternion instance

• q.interp1(s) is a unit quaternion that is interpolated between identity when s=0 and q when s=1. Spherical linear interpolation (slerp) is used. If s is an ndarray(n) then the result will be a UnitQuaternion with n values.

• q.interp1(N) interpolate between identity and q1 in N steps.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rx(0.3)
>>> print(q)
 0.9888 <<  0.1494,  0.0000,  0.0000 >>
>>> q.interp1(0)    # this is identity
UnitQuaternion(array([1., 0., 0., 0.]))
>>> q.interp1(1)    # this is q
UnitQuaternion(array([0.9888, 0.1494, 0.    , 0.    ]))
>>> q.interp1(0.5)  # this is in between
UnitQuaternion(array([0.9972, 0.0749, 0.    , 0.    ]))
>>> qi = q.interp1(11)  # in 11 steps
>>> len(qi)
11
>>> qi[0]                # this is q1
UnitQuaternion(array([1., 0., 0., 0.]))
>>> qi[5]                # this is in between
UnitQuaternion(array([0.9972, 0.0749, 0.    , 0.    ]))

Note

values of s are silently clipped to the range [0, 1]

Seealso

qslerp()

inv()

Inverse of unit quaternion

Returns

unit-quaternion

Return type

UnitQuaternion instance

q.inv() is the inverse of the unit-quaternion. This is a group operation and the product of the unit-quaternion and its inverse is the identity quaternion.

Example:

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

Seealso

qinv()

static isvalid(x, check=True)

Test if vector is valid unit quaternion

Parameters

• x (numpy.ndarray) – vector to test

• check (bool) – explicitly check vector is unit length [default True]

Returns

True if the matrix has shape (4,).

Return type

bool

Example:

>>> from spatialmath import UnitQuaternion
>>> import numpy as np
>>> UnitQuaternion.isvalid(np.r_[1, 0, 0, 0])
True
>>> UnitQuaternion.isvalid(np.r_[1, 2, 3, 4])
False

log()

Logarithm of quaternion

Return type

Quaternion instance

q.log() is the logarithm of the quaternion q, ie.

\[\ln \| q \|, \langle \frac{\vec{v}}{\| \vec{v} \|} \cos^{-1} \frac{s}{\| q \|} \rangle\]

For a UnitQuaternion the logarithm is a pure quaternion whose vector part \(\vec{v}\) and \(\vec{v}/2\) is a Euler vector: parallel to the axis of rotation and whose norm is the magnitude of rotation.

Example:

>>> from spatialmath import Quaternion, UnitQuaternion
>>> from math import pi
>>> q = Quaternion([1, 2, 3, 4])
>>> print(q.log())
 1.7006 <  0.5152,  0.7728,  1.0304 >
>>> q = UnitQuaternion.Rx(pi / 2)
>>> print(q.log())
 0.0000 <  0.7854,  0.0000,  0.0000 >

Reference

Wikipedia

Seealso

Quaternion.exp() Quaternion.log() UnitQuaternion.angvec()

norm()

Norm of quaternion

Return type

float

q.norm() is the norm or length of the quaternion \(\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}\)

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).norm()
5.477225575051661
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).norm()
array([ 5.4772, 13.1909])

Seealso

qnorm()

plot(*args, **kwargs)

Plot unit quaternion as a coordinate frame

Parameters

**kwargs – plotting options

• q.plot() displays the orientation q as a coordinate frame in 3D. There are many options, see the links below.

Example:

>>> q = UQ.Rx(0.3)
>>> q.plot(frame='A', color='green')

Seealso

trplot()

pop(i=- 1)

Pop value from an instance (BasePoseList superclass method)

Parameters

i (int) – item in the list to pop, default is last

Returns

the popped value

Return type

instance of same type

Raises

IndexError – if there are no values to pop

Removes a value from the value list and returns it. The original instance is modified.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> y = x.pop()  # pop the last value x[9]
>>> len(x)
9
>>> y = x.pop(0)  # pop the first value x[0]
>>> len(x)
8

where X is any of the SMTB classes.

static qvmul(qv1, qv2)

Multiply unit quaternions defined by unique vector parts

Parameters

• qv1 (ndarray(3)) – vector representation of first multiplicand

• qv1 – vector representation of second multiplicand

UnitQuaternion(qv1, qv2) is the Hamilton product of two unit quaternions represented in minimal vector form.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(-0.3)
>>> qv1 = q1.vec3
>>> qv1
array([0.1494, 0.    , 0.    ])
>>> qv2 = q2.vec3
>>> qv = UQ.qvmul(qv1, qv2)
>>> qv
array([ 0.1478, -0.1478, -0.0223])
>>> print(UQ.Vec3(qv))
 0.9777 <<  0.1478, -0.1478, -0.0223 >>
>>> print(UQ.Rx(0.3) * UQ.Ry(-0.3))
 0.9777 <<  0.1478, -0.1478, -0.0223 >>

Seealso

UnitQuaternion.vec3() UnitQuaternion.Vec3()

Return type

ndarray[Any, dtype[floating]]

reverse()

S.reverse() – reverse IN PLACE

rpy(unit='rad', order='zyx')

Unit quaternion as roll-pitch-yaw angles

Parameters

• order (str) – angle sequence order, default to ‘zyx’

• unit (str) – angular units: ‘rad’ [default], or ‘deg’

Returns

3-vector of roll-pitch-yaw angles

Return type

ndarray(3) or ndarray(n,3)

q.rpy is the roll-pitch-yaw angle representation of the 3D rotation. The angles are a 3-vector \((r, p, y)\) which correspond to successive rotations about the axes specified by order:

• ‘zyx’ [default], rotate by yaw about the z-axis, then by pitch about the new y-axis, then by roll about the new x-axis. Convention for a mobile robot with x-axis forward and y-axis sideways.

• ‘xyz’, rotate by yaw about the x-axis, then by pitch about the new y-axis, then by roll about the new z-axis. Convention for a robot gripper with z-axis forward and y-axis between the gripper fingers.

• ‘yxz’, rotate by yaw about the y-axis, then by pitch about the new x-axis, then by roll about the new z-axis. Convention for a camera with z-axis parallel to the optic axis and x-axis parallel to the pixel rows.

If len(x) is:

• 1, return an ndarray with shape=(3,)

• N>1, return ndarray with shape=(N,3)

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rx(0.3).rpy()
array([ 0.3, -0. ,  0. ])
>>> UQ.Rz([0.2, 0.3]).rpy()
array([[ 0. , -0. ,  0.2],
       [ 0. , -0. ,  0.3]])

Seealso

SE3.RPY() tr2rpy()

unit()

Unit quaternion

Return type

UnitQuaternion instance

q.unit() is the quaternion q normalized to have a unit length.

Example:

>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> print(q)
 1.0000 <  2.0000,  3.0000,  4.0000 >
>>> print(q.unit())
 0.1826 <<  0.3651,  0.5477,  0.7303 >>
>>> print(Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).unit())
 0.1826 <<  0.3651,  0.5477,  0.7303 >>
 0.3790 <<  0.4549,  0.5307,  0.6065 >>

Note that the return type is different, a UnitQuaternion, which is distinguished by the use of double angle brackets to delimit the vector part.

Seealso

qnorm()

property A: Union[List[numpy.ndarray[Any, numpy.dtype[numpy.typing._generic_alias.ScalarType]]], numpy.ndarray[Any, numpy.dtype[numpy.typing._generic_alias.ScalarType]]]

Array value of an instance (BasePoseList superclass method)

Returns

NumPy array value of this instance

Return type

ndarray

• X.A is a NumPy array that represents the value of this instance, and has a shape given by X.shape.

Note

This assumes that len(X) == 1, ie. it is a single-valued instance.

property R: SO3Array

Unit quaternion as a rotation matrix

Returns

equivalent rotational matrix

Return type

ndarray(3,3)

q.R returns the rotation matrix which describes the equivalent rotation. If len(x) is:

• 1, return an ndarray with shape=(3,3)

• N>1, return ndarray with shape=(N,3,3)

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rx(0.3)
>>> q.R
array([[ 1.    ,  0.    ,  0.    ],
       [ 0.    ,  0.9553, -0.2955],
       [ 0.    ,  0.2955,  0.9553]])
>>> q = UQ.Rx([0.3, 0.4])
>>> q.R
array([[[ 1.    ,  0.    ,  0.    ],
        [ 0.    ,  0.9553, -0.2955],
        [ 0.    ,  0.2955,  0.9553]],

       [[ 1.    ,  0.    ,  0.    ],
        [ 0.    ,  0.9211, -0.3894],
        [ 0.    ,  0.3894,  0.9211]]])

Warning

The i’th rotation matrix is x[i,:,:] or simply x[i]. This is different to the MATLAB version where the i’th rotation matrix is x(:,:,i).

property matrix: numpy.ndarray[Any, numpy.dtype[numpy.typing._generic_alias.ScalarType]]

Matrix equivalent of quaternion

Return type

Numpy array, shape=(4,4)

q.matrix is a 4x4 matrix which encodes the arithmetic rules of Hamilton multiplication. This matrix, multiplied by the 4-vector equivalent of a second quaternion, results in the 4-vector equivalent of the Hamilton product.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).matrix
array([[ 1., -2., -3., -4.],
       [ 2.,  1., -4.,  3.],
       [ 3.,  4.,  1., -2.],
       [ 4., -3.,  2.,  1.]])
>>> Quaternion([1,2,3,4]) * Quaternion([5,6,7,8])   # Hamilton product
Quaternion(array([-60.,  12.,  30.,  24.]))
>>> Quaternion([1,2,3,4]).matrix @ Quaternion([5,6,7,8]).vec  # matrix-vector product
array([-60.,  12.,  30.,  24.])

Seealso

qmatrix()

property s: float

Scalar part of quaternion

Returns

scalar part of quaternion

Return type

float or numpy.ndarray

q.s is the scalar part. If len(q) is:

• 1, return a scalar float

• N>1, return a NumPy array shape=(N,) is returned.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).s
1
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).s
array([1, 5])

property shape: Tuple[int]

Shape of the object’s interal matrix representation

Returns

(4,)

Return type

tuple

property v: numpy.ndarray[Any, numpy.dtype[numpy.floating]]

Vector part of quaternion

Returns

vector part of quaternion

Return type

NumPy ndarray

q.v is the vector part. If len(q) is:

• 1, return a NumPy array shape=(3,)

• N>1, return a NumPy array shape=(N,3).

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).v
array([2, 3, 4])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).v
array([[2, 3, 4],
       [6, 7, 8]])

property vec: numpy.ndarray[Any, numpy.dtype[numpy.floating]]

Quaternion as a vector

Returns

quaternion expressed as a 4-vector

Return type

numpy ndarray, shape=(4,)

q.vec is the quaternion as a vector. If len(q) is:

• 1, return a NumPy array shape=(4,)

• N>1, return a NumPy array shape=(N,4).

The quaternion coefficients are in the order (s, vx, vy, vz), ie. with the scalar (real part) first.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).vec
array([1, 2, 3, 4])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).vec
array([[1, 2, 3, 4],
       [5, 6, 7, 8]])

property vec3: numpy.ndarray[Any, numpy.dtype[numpy.floating]]

Unit quaternion unique vector part

Returns

vector part of unit quaternion

Return type

numpy array, shape=(3,)

q.vec3 is the vector part of a unit quaternion. If q has a negative scalar part we take the vector part of -q, since q and -q represent the same rotation.

This vector part is a minimal unique representation of the unit quaternion and can be used in optimization procedures such as bundle adjustment.

Example:

>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rz(-4)
>>> print(q)
 0.4161 << -0.0000, -0.0000,  0.9093 >>
>>> q.vec3
array([-0.    , -0.    ,  0.9093])
>>> q2 = UQ.Vec3(q.vec3)
>>> print(q2)
 0.4161 << -0.0000, -0.0000,  0.9093 >>
>>> q == q2
True

Seealso

UnitQuaternion.Vec3()

property vec_xyzs: numpy.ndarray[Any, numpy.dtype[numpy.floating]]

Quaternion as a vector

Returns

quaternion expressed as a 4-vector

Return type

numpy ndarray, shape=(4,)

q.vec_xyzs is the quaternion as a vector. If len(q) is:

• 1, return a NumPy array shape=(4,)

• N>1, return a NumPy array shape=(N,4).

The quaternion coefficients are in the order (vx, vy, vz, s), ie. with the scalar (real part) last. This is useful when exporting to other packages like three.js or pybullet.

Example:

>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).vec_xyzs
array([2, 3, 4, 1])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).vec_xyzs
array([[2, 3, 4, 1],
       [6, 7, 8, 5]])