Quaternion
- class Quaternion(s=None, v=None, check=True)[source]
Bases:
BasePoseListQuaternion 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
ndefault values
X.Alloc(N)creates an instance of the pose classXwithNdefault values, ie.len(X)will beN.Xcan 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,SE3it is an identity matrix, for a twist classTwist2orTwist3it is a zero vector, for aUnitQuaternionorQuaternionit is a zero vector.Example:
>>> x = X.Alloc(10) >>> len(x) 10
where
Xis 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
Xis any of the SMTB classes.
- classmethod Pure(v)[source]
Construct a pure quaternion from a vector
- Parameters:
v (3-element array_like) – vector
- Return type:
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 >
- __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 + right1
N
N
sum[i] = left + right[i]N
1
N
sum[i] = left[i] + rightN
N
N
sum[i] = left[i] + right[i]N
M
ValueErrorA 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 == q2is 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 or ndarray(N)) – scalar part
v (ndarray(3), ndarray(Nx3)) – vector part
Quaternion()constructs a zero quaternionQuaternion(s, v)construct a new quaternion from the scalarsand the vectorvQuaternion(q)construct a new quaternion from the 4-vectorq = [s, v]Quaternion([q1, q2 .. qN])construct a new quaternion withNvalues where each element is a 4-vectorQuaternion([Q1, Q2 .. QN])construct a new quaternion withNvalues where each element is a Quaternion instanceQuaternion(M)construct a new quaternion withNvalues whereQis 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 * q2is the Hamilton product of two quaternionsq * sis the scalar product, wheresis 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 * right1
N
N
prod[i] = left * right[i]N
1
N
prod[i] = left[i] * rightN
N
N
prod[i] = left[i] * right[i]N
M
ValueErrorExample:
>>> 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 != q2is 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 ** Ncomputes the product ofqwith itselfN-1times, whereNmust 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 - right1
N
N
diff[i] = left - right[i]N
1
N
diff[i] = left[i] - rightN
N
N
diff[i] = left[i] - right[i]N
M
ValueErrorA 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
- Return type:
None
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
Xis any of the SMTB classes.
- 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
- Return type:
bool
The value
argcan 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
checkis False a cursory check of shape is made, ifcheckis True the numerical value is inspected) and converted to the required internal format by the_importmethod. The default_importmethod calls theisvalidmethod for checking. This mechanism allows equivalent forms to be passed, ie. 6x1 or 4x4 for an se(3).If
selfis an instance of classA, and an instance of classBis passed andBis an element of theconvertfromargument, 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())
- 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 * YwhereXandYare 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
opare the internal numeric values, ie. as returned by the._Aproperty.The result is always a list, except for the first case above and
list1isFalse.If the right operand is not a
BasePoseListsubclass, but is a numeric scalar or array then thenop2is 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
rightif it is an array. The result is always a list, except for the first case above andlist1isFalse.
- clear() None -- remove all items from S
- conj()[source]
Conjugate of quaternion
- Return type:
Quaternion instance
q.conj()is the quaternionqwith 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.]))
- Reference:
- 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
- Return type:
None
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
Xis any of the SMTB classes.
- 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
- Return type:
None
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
Xis any of the SMTB classes.Note
If
iis beyond the end of the list, the item is appended to the list
- 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
UnitQuaternionthe 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
Xis any of the SMTB classes.
- reverse()
S.reverse() – reverse IN PLACE
- unit()[source]
Unit quaternion
- Return type:
UnitQuaternion instance
q.unit()is the quaternionqnormalized 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: List[ndarray[Any, dtype[ScalarType]]] | ndarray[Any, dtype[ScalarType]]
Array value of an instance (BasePoseList superclass method)
- Returns:
NumPy array value of this instance
- Return type:
ndarray
X.Ais 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: ndarray[Any, dtype[ScalarType]]
Matrix equivalent of quaternion
- Return type:
Numpy array, shape=(4,4)
q.matrixis 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.sis 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: ndarray[Any, dtype[floating]]
Vector part of quaternion
- Returns:
vector part of quaternion
- Return type:
NumPy ndarray
q.vis 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: ndarray[Any, dtype[floating]]
Quaternion as a vector
- Returns:
quaternion expressed as a 4-vector
- Return type:
numpy ndarray, shape=(4,)
q.vecis 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: ndarray[Any, dtype[floating]]
Quaternion as a vector
- Returns:
quaternion expressed as a 4-vector
- Return type:
numpy ndarray, shape=(4,)
q.vec_xyzsis 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]])