Quaternion
- class Quaternion(s=None, v=None, check=True)[source]
Bases:
spatialmath.baseposelist.BasePoseList
Quaternion class
A 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\]- 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 classX
withN
default values, ie.len(X)
will beN
.X
can be considered a vector of pose objects, and those elements can be referencedX[i]
or assigned toX[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 classTwist2
orTwist3
it is a zero vector, for aUnitQuaternion
orQuaternion
it is a zero vector.Example:
>>> x = X.Alloc(10) >>> len(x) 10
where
X
is any of the SMTB classes.
- 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 Pure(v)[source]
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 tov
, 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
- __add__(right)[source]
Overloaded
+
operator- Returns
sum
- Return type
- 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)[source]
Overloaded
==
operator- Returns
Equality of two operands
- Return type
bool or list of bool
q1 == q2
is True ifq1` is elementwise equal to ``q2
.Example:
>>> from spatialmath import Quaternion >>> q1 = Quaternion([1,2,3,4]) >>> q2 = Quaternion([5,6,7,8]) >>> q1 == q1 True >>> q1 == q2 False >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) == q1 [True, False] >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) == q2 [False, True] >>> 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]]) [True, True]
- Seealso
- __init__(s=None, v=None, check=True)[source]
Construct a new quaternion
- Parameters
s (float) – scalar
v (3-element array_like) – vector
Quaternion()
constructs a zero quaternionQuaternion(s, v)
construct a new quaternion from the scalars
and the vectorv
Quaternion(q)
construct a new quaternion from the 4-vectorq = [s, v]
Quaternion([q1, q2 .. qN])
construct a new quaternion withN
values where each element is a 4-vectorQuaternion([Q1, Q2 .. QN])
construct a new quaternion withN
values where each element is a Quaternion instanceQuaternion(M)
construct a new quaternion withN
values whereQ
is a 4xN NumPy array.
Example:
>>> from spatialmath import Quaternion >>> Quaternion() Quaternion(array([0., 0., 0., 0.])) >>> Quaternion(1, [2,3,4]) Quaternion(array([1., 2., 3., 4.])) >>> Quaternion([1,2,3,4]) Quaternion(array([1, 2, 3, 4])) >>> q=Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) >>> len(q) 2 >>> print(q) 1.0000 < 2.0000, 3.0000, 4.0000 > 5.0000 < 6.0000, 7.0000, 8.0000 >
- __mul__(right)[source]
Overloaded
*
operator- Returns
product
- Return type
- Raises
ValueError
q1 * q2
is the Hamilton product of two quaternionsq * s
is the scalar product, wheres
is a scalar
Multiplicands
Product
left
right
type
result
Quaternion
Quaternion
Quaternion
Hamilton product
Quaternion
UnitQuaternion
Quaternion
Hamilton product
Quaternion
scalar
Quaternion
scalar product
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
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
ValueError
Example:
>>> from spatialmath import Quaternion >>> Quaternion([1,2,3,4]) * Quaternion([5,6,7,8]) Quaternion(array([-60., 12., 30., 24.])) >>> Quaternion([1,2,3,4]) * 2 Quaternion(array([2, 4, 6, 8])) >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) * 2 Quaternion([ array([2, 4, 6, 8]), array([10, 12, 14, 16]) ]) >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) * Quaternion([1,2,3,4]) Quaternion([ array([-28., 4., 6., 8.]), array([-60., 20., 14., 32.]) ]) >>> Quaternion([1,2,3,4]) * Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) Quaternion([ array([-28., 4., 6., 8.]), array([-60., 12., 30., 24.]) ]) >>> 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([-28., 4., 6., 8.]), array([-124., 60., 70., 80.]) ])
- Seealso
__rmul__()
__imul__()
qqmul()
- __ne__(right)[source]
Overloaded
!=
operator- Return type
bool
q1 != q2
is True ifq` is elementwise not equal to ``q2
.Example:
>>> from spatialmath import Quaternion >>> q1 = Quaternion([1,2,3,4]) >>> q2 = Quaternion([5,6,7,8]) >>> q1 != q1 False >>> q1 != q2 True >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) != q1 [False, True] >>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) != q2 [True, False]
- Seealso
- __pow__(n)[source]
Overloaded
**
operator- Return type
Quaternion instance
q ** N
computes the product ofq
with itselfN-1
times, whereN
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
- __sub__(right)[source]
Overloaded
-
operator- Returns
difference
- Return type
- 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]) ])
- 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
- arghandler(arg, convertfrom=(), check=True)
Standard constructor support (BasePoseList superclass method)
- Parameters
arg (
Any
) – initial valueconvertfrom (
Tuple
) – list of classes to accept and convert fromcheck (bool) – check value is valid, defaults to True
- Type
tuple of typles
- Raises
ValueError – bad type passed
The value
arg
can be any of:None, an identity value is created
a numpy.ndarray of the appropriate shape and value which is valid for the subclass
a list whose elements all meet the criteria above
an instance of the subclass
a list whose elements are all singelton instances of the subclass
For cases 2 and 3, a NumPy array or a list of NumPy array is passed. Each NumPyarray is tested for validity (if
check
is False a cursory check of shape is made, ifcheck
is True the numerical value is inspected) and converted to the required internal format by the_import
method. The default_import
method calls theisvalid
method for checking. This mechanism allows equivalent forms to be passed, ie. 6x1 or 4x4 for an se(3).If
self
is an instance of classA
, and an instance of classB
is passed andB
is an element of theconvertfrom
argument, thenB.A()
will be invoked to perform the type conversion.Examples:
SE3() SE3(np.identity(4)) SE3([np.identity(4), np.identity(4)]) SE3(SE3()) SE3([SE3(), SE3()]) Twist3(SE3())
- Return type
bool
- binop(right, op, op2=None, list1=True)
Perform binary operation
- Parameters
left (BasePoseList subclass) – left operand
right (BasePoseList subclass, scalar or array) – right operand
op (callable) – binary operation
op2 (callable) – binary operation
list1 (bool) – return single array as a list, default True
- Raises
ValueError – arguments are not compatible
- Returns
list of values
- Return type
list
The is a helper method for implementing binary operation with overloaded operators such as
X * Y
whereX
andY
are both subclasses ofBasePoseList
. Each operand has a list of one or more values and this methods computes a list of result values according to:Inputs
Output
len(left)
len(right)
len
operation
1
1
1
ret = op(left, right)
1
M
M
ret[i] = op(left, right[i])
M
1
M
ret[i] = op(left[i], right)
M
M
M
ret[i] = op(left[i], right[i])
The arguments to
op
are the internal numeric values, ie. as returned by the._A
property.The result is always a list, except for the first case above and
list1
isFalse
.If the right operand is not a
BasePoseList
subclass, but is a numeric scalar or array then thenop2
is invokedFor example:
X._binop(Y, lambda x, y: x + y)
Input
Output
len(left)
len
operation
1
1
ret = op2(left, right)
M
M
ret[i] = op2(left[i], right)
There is no check on the shape of
right
if it is an array. The result is always a list, except for the first case above andlist1
isFalse
.
- clear() None -- remove all items from S
- conj()[source]
Conjugate of quaternion
- Return type
Quaternion instance
q.conj()
is the quaternionq
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
- exp(tol=20)[source]
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 quaternionq
, 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.]))
- 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
- inner(other)[source]
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 ofq.inner(q)
is the same asq.norm ** 2
.Example:
- Seealso
- 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
- static isvalid(x)[source]
Test if vector is valid 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 Quaternion >>> import numpy as np >>> Quaternion.isvalid(np.r_[1, 0, 0, 0]) True >>> Quaternion.isvalid(np.r_[1, 2, 3, 4]) True
- log()[source]
Logarithm of quaternion
- Return type
Quaternion instance
q.log()
is the logarithm of the quaternionq
, 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
- Seealso
- norm()[source]
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
- 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.
- reverse()
S.reverse() – reverse IN PLACE
- unit()[source]
Unit quaternion
- Return type
UnitQuaternion instance
q.unit()
is the quaternionq
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
- unop(op, matrix=False)
Perform unary operation
- Parameters
self (BasePoseList subclass) – operand
op (callable) – unnary operation
matrix (bool) – return array instead of list, default False
- Returns
operation results
- Return type
list or NumPy array
The is a helper method for implementing unary operations where the operand has multiple value. This method computes the value of the operation for all input values and returns the result as either a list or as a matrix which vertically stacks the results.
Input
Output
len(self)
len
operation
1
1
ret = op(self)
M
M
ret[i] = op(self[i])
M
M
ret[i,;] = op(self[i])
The result is:
a list of values if
matrix==False
, ora 2D NumPy stack of values if
matrix==True
, it is assumed that the value is a 1D array.
- 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 byX.shape
.
Note
This assumes that
len(X)
== 1, ie. it is a single-valued instance.
- 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
- 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 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]])