"""
Classes to abstract quaternions and unit-quaternions.
To use::
from spatialmath.quaternion import *
T = UnitQuaternion.Rx(0.3)
import spatialmath as sm
T = sm.UnitQuaternion.Rx(0.3)
.. inheritance-diagram:: spatialmath.quaternion
:top-classes: collections.UserList
:parts: 1
"""
# pylint: disable=invalid-name
from __future__ import annotations
import math
import numpy as np
from typing import Any, Type
import spatialmath.base as smb
from spatialmath.pose3d import SO3, SE3
from spatialmath.baseposelist import BasePoseList
from spatialmath.base.types import *
_eps = np.finfo(np.float64).eps
[docs]class Quaternion(BasePoseList):
r"""
Quaternion class
A quaternion can be considered an ordered pair :math:`(s, \vec{v})`
where :math:`s \in \mathbb{R}` is the *scalar* part and :math:`\vec{v} = (v_x, v_y, v_z) \in \mathbb{R}^3`
is the *vector* part and is often written as
.. math:: \q = s \langle v_x, v_y, v_z \rangle
.. inheritance-diagram:: spatialmath.quaternion.Quaternion
:top-classes: collections.UserList
:parts: 1
"""
[docs] def __init__(self, s: Any = None, v=None, check: Optional[bool] = True):
r"""
Construct a new quaternion
:param s: scalar
:type s: float
:param v: vector
:type v: 3-element array_like
- ``Quaternion()`` constructs a zero quaternion
- ``Quaternion(s, v)`` construct a new quaternion from the scalar ``s``
and the vector ``v``
- ``Quaternion(q)`` construct a new quaternion from the 4-vector
``q = [s, v]``
- ``Quaternion([q1, q2 .. qN])`` construct a new quaternion with ``N``
values where each element is a 4-vector
- ``Quaternion([Q1, Q2 .. QN])`` construct a new quaternion with ``N``
values where each element is a Quaternion instance
- ``Quaternion(M)`` construct a new quaternion with ``N`` values where
``Q`` is a 4xN NumPy array.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion()
>>> Quaternion(1, [2,3,4])
>>> Quaternion([1,2,3,4])
>>> q=Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
>>> len(q)
>>> print(q)
"""
super().__init__()
if s is None and smb.isvector(v, 4):
v,s = (s,v)
if v is None:
# single argument
if super().arghandler(s, check=False):
return
elif smb.isvector(s, 4):
self.data = [smb.getvector(s)]
elif smb.isscalar(s) and smb.isvector(v, 3):
# Quaternion(s, v)
self.data = [np.r_[s, smb.getvector(v)]]
else:
raise ValueError("bad argument to Quaternion constructor")
[docs] @classmethod
def Pure(cls, v: ArrayLike3) -> Quaternion:
r"""
Construct a pure quaternion from a vector
:param v: vector
:type v: 3-element array_like
``Quaternion.Pure(v)`` is a Quaternion with a zero scalar part and the
vector part set to ``v``,
ie. :math:`q = 0 \langle v_x, v_y, v_z \rangle`
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> print(Quaternion.Pure([1,2,3]))
"""
return cls(s=0, v=smb.getvector(v, 3))
@staticmethod
def _identity():
return np.zeros((4,))
@property
def shape(self) -> Tuple[int]:
"""
Shape of the object's interal matrix representation
:return: (4,)
:rtype: tuple
"""
return (4,)
[docs] @staticmethod
def isvalid(x: ArrayLike4) -> bool:
"""
Test if vector is valid quaternion
:param x: vector to test
:type x: numpy.ndarray
:arg check: explicitly check vector is unit length [default True]
:type check: bool
:return: True if the matrix has shape (4,).
:rtype: bool
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> import numpy as np
>>> Quaternion.isvalid(np.r_[1, 0, 0, 0])
>>> Quaternion.isvalid(np.r_[1, 2, 3, 4])
"""
return x.shape == (4,)
@property
def s(self) -> float:
"""
Scalar part of quaternion
:return: scalar part of quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).s
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).s
"""
if len(self) == 1:
return self._A[0]
else:
return np.array([q.s for q in self])
@property
def v(self) -> R3:
"""
Vector part of quaternion
:return: vector part of quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).v
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).v
"""
if len(self) == 1:
return self._A[1:4]
else:
return np.array([q.v for q in self])
@property
def vec(self) -> R4:
"""
Quaternion as a vector
:return: quaternion expressed as a 4-vector
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).vec
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).vec
"""
if len(self) == 1:
return self._A
else:
return np.array([q._A for q in self])
@property
def vec_xyzs(self) -> R4:
"""
Quaternion as a vector
:return: quaternion expressed as a 4-vector
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).vec_xyzs
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).vec_xyzs
"""
if len(self) == 1:
return np.roll(self._A, -1)
else:
return np.array([np.roll(q._A, -1) for q in self])
@property
def matrix(self) -> R4x4:
"""
Matrix equivalent of quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).matrix
>>> Quaternion([1,2,3,4]) * Quaternion([5,6,7,8]) # Hamilton product
>>> Quaternion([1,2,3,4]).matrix @ Quaternion([5,6,7,8]).vec # matrix-vector product
:seealso: :func:`~spatialmath.base.quaternions.qmatrix`
"""
return smb.qmatrix(self._A)
[docs] def conj(self) -> Quaternion:
r"""
Conjugate of quaternion
:rtype: Quaternion instance
``q.conj()`` is the quaternion ``q`` with the vector part negated, ie.
:math:`q = s \langle -v_x, -v_y, -v_z \rangle`
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> print(Quaternion.Pure([1,2,3]).conj())
:seealso: :func:`~spatialmath.base.quaternions.qconj`
"""
return self.__class__([smb.qconj(q._A) for q in self])
[docs] def norm(self) -> float:
r"""
Norm of quaternion
:rtype: float
``q.norm()`` is the norm or length of the quaternion
:math:`\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}`
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).norm()
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).norm()
:seealso: :func:`~spatialmath.base.quaternions.qnorm`
"""
if len(self) == 1:
return smb.qnorm(self._A)
else:
return np.array([smb.qnorm(q._A) for q in self])
[docs] def unit(self) -> UnitQuaternion:
r"""
Unit quaternion
:rtype: UnitQuaternion instance
``q.unit()`` is the quaternion ``q`` normalized to have a unit length.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> print(q)
>>> print(q.unit())
>>> print(Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]).unit())
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: :func:`~spatialmath.base.quaternions.qnorm`
"""
return UnitQuaternion([smb.qunit(q._A) for q in self], norm=False)
[docs] def log(self) -> Quaternion:
r"""
Logarithm of quaternion
:rtype: Quaternion instance
``q.log()`` is the logarithm of the quaternion ``q``, ie.
.. math::
\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 :math:`\vec{v}` and :math:`\vec{v}/2` is a Euler vector: parallel
to the axis of rotation and whose norm is the magnitude of rotation.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion, UnitQuaternion
>>> from math import pi
>>> q = Quaternion([1, 2, 3, 4])
>>> print(q.log())
>>> q = UnitQuaternion.Rx(pi / 2)
>>> print(q.log())
:reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
:seealso: :meth:`Quaternion.exp` :meth:`Quaternion.log` :meth:`UnitQuaternion.angvec`
"""
norm = self.norm()
s = math.log(norm)
v = math.acos(np.clip(self.s / norm, -1, 1)) * smb.unitvec(self.v)
return Quaternion(s=s, v=v)
[docs] def exp(self, tol: float = 20) -> Quaternion:
r"""
Exponential of quaternion
:param tol: Tolerance when checking for pure quaternion, in multiples of eps, defaults to 20
:type tol: float, optional
:rtype: Quaternion instance
``q.exp()`` is the exponential of the quaternion ``q``, ie.
.. math::
e^s \cos \| v \|, \langle e^s \frac{\vec{v}}{\| \vec{v} \|} \sin \| \vec{v} \| \rangle
For a pure quaternion with vector value :math:`\vec{v}` the the result
is a unit quaternion equivalent to a rotation defined by
:math:`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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> from math import pi
>>> q = Quaternion([1, 2, 3, 4])
>>> print(q.exp())
>>> q = Quaternion.Pure([pi / 4, 0, 0])
>>> print(q.exp()) # result is a UnitQuaternion
>>> print(q.exp().angvec())
:reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
:seealso: :meth:`Quaternion.log` :meth:`UnitQuaternion.log` :meth:`UnitQuaternion.AngVec` :meth:`UnitQuaternion.EulerVec`
"""
exp_s = math.exp(self.s)
norm_v = smb.norm(self.v)
s = exp_s * math.cos(norm_v)
v = exp_s * self.v / norm_v * math.sin(norm_v)
if abs(self.s) < tol * _eps:
# result will be a unit quaternion
return UnitQuaternion(s=s, v=v)
else:
return Quaternion(s=s, v=v)
[docs] def inner(self, other) -> float:
"""
Inner product of quaternions
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]).inner(Quaternion([5,6,7,8]))
>>> numpy.dot([1,2,3,4], [5,6,7,8])
:seealso: :func:`~spatialmath.base.quaternions.qinner`
"""
assert isinstance(
other, Quaternion
), "operands to inner must be Quaternion subclass"
return self.binop(other, smb.qinner, list1=False)
# -------------------------------------------- operators
[docs] def __eq__(
left, right: Quaternion
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``==`` operator
:return: Equality of two operands
:rtype: bool or list of bool
``q1 == q2`` is True if ``q1` is elementwise equal to ``q2``.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q1 = Quaternion([1,2,3,4])
>>> q2 = Quaternion([5,6,7,8])
>>> q1 == q1
>>> q1 == q2
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) == q1
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) == q2
>>> 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]])
:seealso: :func:`__ne__` :func:`~spatialmath.base.quaternions.qisequal`
"""
assert isinstance(left, type(right)), "operands to == are of different types"
return left.binop(right, smb.qisequal, list1=False)
[docs] def __ne__(
left, right: Quaternion
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``!=`` operator
:rtype: bool
``q1 != q2`` is True if ``q` is elementwise not equal to ``q2``.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q1 = Quaternion([1,2,3,4])
>>> q2 = Quaternion([5,6,7,8])
>>> q1 != q1
>>> q1 != q2
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) != q1
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) != q2
:seealso: :func:`__ne__` :func:`~spatialmath.base.quaternions.qisequal`
"""
assert isinstance(left, type(right)), "operands to == are of different types"
return left.binop(right, lambda x, y: not smb.qisequal(x, y), list1=False)
[docs] def __mul__(
left, right: Quaternion
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:return: product
:rtype: Quaternion
:raises: ValueError
- ``q1 * q2`` is the Hamilton product of two quaternions
- ``q * s`` is the scalar product, where ``s`` 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]) * Quaternion([5,6,7,8])
>>> Quaternion([1,2,3,4]) * 2
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) * 2
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) * Quaternion([1,2,3,4])
>>> Quaternion([1,2,3,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([np.r_[1,2,3,4], np.r_[5,6,7,8]])
:seealso: :func:`__rmul__` :func:`__imul__` :func:`~spatialmath.base.quaternions.qqmul`
"""
if isinstance(right, left.__class__):
# quaternion * [unit]quaternion case
return Quaternion(left.binop(right, smb.qqmul))
elif smb.isscalar(right):
# quaternion * scalar case
# print('scalar * quat')
return Quaternion([right * q._A for q in left])
else:
raise ValueError("operands to * are of different types")
def __rmul__(
right, left: Quaternion
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``*`` operator
:return: product
:rtype: Quaternion
:raises: ValueError
``s * q`` is the scalar product, where ``s`` is a scalar.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> 2 * Quaternion([1,2,3,4])
>>> 2 * Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
:seealso: :func:`__mul__`
"""
# scalar * quaternion case
return Quaternion([left * q._A for q in right])
def __imul__(
left, right: Quaternion
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``*=`` operator
:return: product
:rtype: Quaternion
:raises: ValueError
``q1 *= q2`` sets ``q1 := q1 * q2``
``q1 *= s`` sets ``q1 := q1 * s`` where ``s`` is a scalar
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> q *= Quaternion([5,6,7,8])
>>> print(q)
>>> q *= 2
>>> print(q)
:seealso: :func:`__mul__`
"""
return left.__mul__(right)
[docs] def __pow__(self, n: int) -> Quaternion:
"""
Overloaded ``**`` operator
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> print(Quaternion([1,2,3,4]) ** 2)
>>> print(Quaternion([1,2,3,4]) ** -1)
>>> print(Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) ** 2)
:seealso: :func:`~spatialmath.base.quaternions.qpow`
"""
return self.__class__([smb.qpow(q._A, n) for q in self])
def __ipow__(self, n: int) -> Quaternion:
"""
Overloaded ``=**`` operator
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> q **= 2
>>> q
>>> q = Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
>>> q **= 2
>>> q
:seealso: :func:`__pow__`
"""
return self.__pow__(n)
[docs] def __truediv__(self, other: Quaternion):
return NotImplemented # Quaternion division not supported
[docs] def __add__(
left, right: Quaternion
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``+`` operator
:return: sum
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]) + Quaternion([5,6,7,8])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) + Quaternion([1,2,3,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]])
"""
# results is not in the group, return an array, not a class
assert isinstance(left, type(right)), "operands to + are of different types"
return Quaternion(left.binop(right, lambda x, y: x + y))
[docs] def __sub__(
left, right: Quaternion
) -> Quaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``-`` operator
:return: difference
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> Quaternion([1,2,3,4]) - Quaternion([5,6,7,8])
>>> Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]]) - Quaternion([1,2,3,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]])
"""
# results is not in the group, return an array, not a class
# TODO allow class +/- a conformant array
assert isinstance(left, type(right)), "operands to - are of different types"
return Quaternion(left.binop(right, lambda x, y: x - y))
def __neg__(self) -> Quaternion:
r"""
Overloaded unary ``-`` operator
:rtype: Quaternion or UnitQuaternion
``-q`` is a quaternion with all its components negated.
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> -Quaternion([1,2,3,4])
>>> -Quaternion([np.r_[1,2,3,4], np.r_[5,6,7,8]])
"""
return UnitQuaternion(
[-x for x in self.data]
) # pylint: disable=invalid-unary-operand-type
def __repr__(self) -> str:
"""
Readable representation of pose (superclass method)
:return: readable representation of the pose as a list of arrays
:rtype: str
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> q
"""
name = type(self).__name__
if len(self) == 0:
return name + "([])"
elif len(self) == 1:
# need to indent subsequent lines of the native repr string by 4 spaces
return name + "(" + self._A.__repr__() + ")"
else:
# format this as a list of ndarrays
return (
name
+ "([\n "
+ ",\n ".join([v.__repr__() for v in self.data])
+ " ])"
)
def _repr_pretty_(self, p, cycle):
"""
Pretty string for IPython (superclass method)
:param p: pretty printer handle (ignored)
:param cycle: pretty printer flag (ignored)
Print colorized output when variable is displayed in IPython, ie. on a line by
itself.
Example::
In [1]: x
"""
print(self.__str__())
def __str__(self) -> str:
"""
Pretty string representation of quaternion
:return: readable representation of quaternion
:rtype: str
Format the quaternion elements into a single line format. For example::
Example:
.. runblock:: pycon
>>> from spatialmath import Quaternion
>>> q = Quaternion([1,2,3,4])
>>> print(x)
>> q = UnitQuaternion.Rx(0.3)
Note that unit quaternions are denoted by different delimiters for
the vector part.
:seealso: :func:`~spatialmath.base.quaternions.qnorm`
"""
if isinstance(self, UnitQuaternion):
delim = ("<<", ">>")
else:
delim = ("<", ">")
return "\n".join([smb.qprint(q, file=None, delim=delim) for q in self.data])
# ========================================================================= #
[docs]class UnitQuaternion(Quaternion):
r"""
Unit quaternion class
A unit quaternion can be considered an ordered pair :math:`(s, \vec{v})`
where :math:`s \in \mathbb{R}` is the *scalar* part and :math:`\vec{v} = (v_x, v_y, v_z) \in \mathbb{R}^3`
is the *vector* part and is often written as
.. math:: \q = s \langle v_x, v_y, v_z \rangle
and subject to a unit-length constraint :math:`s^2+v_x^2+v_y^2+v_z^2 = 1`.
A unit-quaternion can be considered as a rotation :math:`\theta` about the
vector :math:`\vec{v}`, so the unit quaternion can also be
written as
.. math:: \q = \cos \frac{\theta}{2} \sin \frac{\theta}{2} <v_x v_y v_z>
The quaternion :math:`\q` and :math:`-\q` represent the equivalent rotation, and this is referred to
as a double mapping.
.. inheritance-diagram:: spatialmath.quaternion.UnitQuaternion
:top-classes: collections.UserList
:parts: 1
The ``UnitQuaternion`` class inherits many methods from the ``Quaternion`` class
"""
[docs] def __init__(
self,
s: Any = None,
v=None,
norm: Optional[bool] = True,
check: Optional[bool] = True,
):
"""
Construct a UnitQuaternion instance
:param norm: explicitly normalize the quaternion [default True]
:type norm: bool
:param check: explicitly check validity of argument [default True]
:type check: bool
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ()
>>> q # repr()
>>> print(q) # str()
"""
super().__init__()
# handle: UnitQuaternion(v)`` constructs a unit quaternion with specified elements
# from ``v`` which is a 4-vector given as a list, tuple, or ndarray(4)
if s is None and smb.isvector(v, 4):
v,s = (s,v)
if v is None:
# single argument
if super().arghandler(s, check=check):
# create unit quaternion
self.data = [smb.qunit(q) for q in self.data]
elif isinstance(s, np.ndarray):
# passed a NumPy array, it could be:
# an SO(3) or SE(3) matrix
# a quaternion as a 1D array
# an array of quaternions as an nx4 array
if s.shape == (3, 3):
if smb.isrot(s, check=check):
# UnitQuaternion(R) R is 3x3 rotation matrix
self.data = [smb.r2q(s)]
else:
raise ValueError(
"invalid rotation matrix provided to UnitQuaternion constructor"
)
elif s.shape == (4,):
# passed a 4-vector
if norm:
self.data = [smb.qunit(s)]
else:
self.data = [s]
elif s.ndim == 2 and s.shape[1] == 4:
if norm:
self.data = [smb.qunit(x) for x in s]
else:
# self.data = [smb.qpositive(x) for x in s]
self.data = [x for x in s]
else:
raise ValueError("array could not be interpreted as UnitQuaternion")
elif isinstance(s, SO3):
# UnitQuaternion(x) x is SO3 or SE3 (since SE3 is subclass of SO3)
self.data = [smb.r2q(x.R) for x in s]
elif isinstance(s[0], SO3):
# list of SO3 or SE3
self.data = [smb.r2q(x.R) for x in s]
else:
raise ValueError("bad argument to UnitQuaternion constructor")
elif smb.isscalar(s) and smb.isvector(v, 3):
# UnitQuaternion(s, v) s is scalar, v is 3-vector
q = np.r_[s, smb.getvector(v)]
if norm:
q = smb.qunit(q)
self.data = [q]
else:
raise ValueError("bad argument to UnitQuaternion constructor")
@staticmethod
def _identity():
return smb.qeye()
[docs] @staticmethod
def isvalid(x: ArrayLike, check: Optional[bool] = True) -> bool:
"""
Test if vector is valid unit quaternion
:param x: vector to test
:type x: numpy.ndarray
:arg check: explicitly check vector is unit length [default True]
:type check: bool
:return: True if the matrix has shape (4,).
:rtype: bool
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion
>>> import numpy as np
>>> UnitQuaternion.isvalid(np.r_[1, 0, 0, 0])
>>> UnitQuaternion.isvalid(np.r_[1, 2, 3, 4])
"""
return x.shape == (4,) and (not check or smb.isunitvec(x))
@property
def R(self) -> SO3Array:
"""
Unit quaternion as a rotation matrix
:return: equivalent rotational matrix
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rx(0.3)
>>> q.R
>>> q = UQ.Rx([0.3, 0.4])
>>> q.R
.. 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)``.
"""
if len(self) > 1:
return np.array([smb.q2r(q) for q in self.data])
else:
return smb.q2r(self._A)
@property
def vec3(self) -> R3:
r"""
Unit quaternion unique vector part
:return: vector part of unit quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rz(-4)
>>> print(q)
>>> q.vec3
>>> q2 = UQ.Vec3(q.vec3)
>>> print(q2)
>>> q == q2
:seealso: :meth:`UnitQuaternion.Vec3`
"""
return smb.q2v(self._A)
# -------------------------------------------- constructor variants
[docs] @classmethod
def Rx(cls, angles: ArrayLike, unit: Optional[str] = "rad") -> UnitQuaternion:
"""
Construct a UnitQuaternion object representing rotation about the X-axis
:arg θ: rotation angle
:type θ: array_like
:arg unit: rotation unit 'rad' [default] or 'deg'
:type unit: str
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3))
>>> print(UQ.Rx([0, 0.3, 0.6]))
"""
angles = smb.getunit(angles, unit)
return cls(
[np.r_[math.cos(a / 2), math.sin(a / 2), 0, 0] for a in angles], check=False
)
[docs] @classmethod
def Ry(cls, angles: ArrayLike, unit: Optional[str] = "rad") -> UnitQuaternion:
"""
Construct a UnitQuaternion object representing rotation about the Y-axis
:arg θ: rotation angle
:type θ: array_like
:arg unit: rotation unit 'rad' [default] or 'deg'
:type unit: str
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Ry(0.3))
>>> print(UQ.Ry([0, 0.3, 0.6]))
"""
angles = smb.getunit(angles, unit)
return cls(
[np.r_[math.cos(a / 2), 0, math.sin(a / 2), 0] for a in angles], check=False
)
[docs] @classmethod
def Rz(cls, angles: ArrayLike, unit: Optional[str] = "rad") -> UnitQuaternion:
"""
Construct a UnitQuaternion object representing rotation about the Z-axis
:arg θ: rotation angle
:type θ: array_like
:arg unit: rotation unit 'rad' [default] or 'deg'
:type unit: str
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rz(0.3))
>>> print(UQ.Rz([0, 0.3, 0.6]))
"""
angles = smb.getunit(angles, unit)
return cls(
[np.r_[math.cos(a / 2), 0, 0, math.sin(a / 2)] for a in angles], check=False
)
[docs] @classmethod
def Rand(cls, N: int = 1) -> UnitQuaternion:
"""
Construct a new random unit quaternion
:param N: number of random rotations
:type N: int
:return: random unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rand())
>>> print(UQ.Rand(3))
:seealso: :meth:`UnitQuaternion.Rand`
"""
return cls([smb.qrand() for i in range(0, N)], check=False)
[docs] @classmethod
def Eul(cls, *angles: List[float], unit: Optional[str] = "rad") -> UnitQuaternion:
r"""
Construct a new unit quaternion from Euler angles
:param 𝚪: 3-vector of Euler angles
:type 𝚪: array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: unit-quaternion
:rtype: UnitQuaternion instance
- ``UnitQuaternion.Eul(𝚪)`` is a unit quaternion that describes the 3D
rotation defined by a 3-vector of Euler angles :math:`\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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Eul([0.1, 0.2, 0.3]))
:seealso: :meth:`UnitQuaternion.RPY` :meth:`SE3.eul` :meth:`SE3.Eul` :meth:`~spatialmath.base.transforms3d.eul2r`
"""
if len(angles) == 1:
angles = angles[0]
return cls(smb.r2q(smb.eul2r(angles, unit=unit)), check=False)
[docs] @classmethod
def RPY(
cls,
*angles: List[float],
order: Optional[str] = "zyx",
unit: Optional[str] = "rad",
) -> UnitQuaternion:
r"""
Construct a new unit quaternion from roll-pitch-yaw angles
:param 𝚪: 3-vector of roll-pitch-yaw angles
:type 𝚪: array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: unit-quaternion
:rtype: UnitQuaternion instance
- ``UnitQuaternion.RPY(𝚪)`` is a unit quaternion that describes the 3D
rotation defined by a 3-vector of roll, pitch, yaw angles
:math:`\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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.RPY([0.1, 0.2, 0.3]))
:seealso: :meth:`UnitQuaternion.Eul` :meth:`SE3.rpy` :meth:`SE3.RPY` :func:`~spatialmath.base.transforms3d.rpy2r`
"""
if len(angles) == 1:
angles = angles[0]
return cls(smb.r2q(smb.rpy2r(angles, unit=unit, order=order)), check=False)
[docs] @classmethod
def OA(cls, o: ArrayLike3, a: ArrayLike3) -> UnitQuaternion:
"""
Construct a new unit quaternion from two vectors
:param o: 3-vector parallel to Y- axis
:type o: array_like
:param a: 3-vector parallel to the Z-axis
:type a: array_like
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.OA([0,0,-1], [0,1,0]))
.. 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: :func:`~spatialmath.base.transforms3d.oa2r`
"""
return cls(smb.r2q(smb.oa2r(o, a)), check=False)
[docs] @classmethod
def AngVec(
cls, theta: float, v: ArrayLike3, *, unit: Optional[str] = "rad"
) -> UnitQuaternion:
r"""
Construct a new unit quaternion from rotation angle and axis
:param theta: rotation
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis, 3-vector
:type v: array_like
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.AngVec(0, [1,0,0]))
>>> print(UQ.AngVec(90, [1,0,0], unit='deg'))
.. note:: :math:`\theta = 0` the result in an identity quaternion, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :meth:`UnitQuaternion.angvec` :meth:`UnitQuaternion.exp` :func:`~spatialmath.base.transforms3d.angvec2r`
"""
v = smb.getvector(v, 3)
theta = smb.getunit(theta, unit, dim=0)
return cls(
s=math.cos(theta / 2), v=math.sin(theta / 2) * v, norm=False, check=False
)
[docs] @classmethod
def EulerVec(cls, w: ArrayLike3) -> UnitQuaternion:
r"""
Construct a new unit quaternion from an Euler rotation vector
:param ω: rotation axis
:type ω: 3-element array_like
:return: unit-quaternion
:rtype: UnitQuaternion instance
``UnitQuaternion.EulerVec(ω)`` is a unit quaternion that describes the 3D rotation
defined by a rotation of :math:`\theta = \lVert \omega \rVert` about the
unit 3-vector :math:`\omega / \lVert \omega \rVert`.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.EulerVec([0.5,0,0]))
.. note:: :math:`\theta \eq 0` the result in an identity matrix, otherwise
``V`` must have a finite length, ie. :math:`|V| > 0`.
:seealso: :meth:`SE3.angvec` :func:`~spatialmath.base.transforms3d.angvec2r`
"""
assert smb.isvector(w, 3), "w must be a 3-vector"
w = smb.getvector(w)
theta = smb.norm(w)
s = math.cos(theta / 2)
v = math.sin(theta / 2) * smb.unitvec(w)
return cls(s=s, v=v, check=False)
[docs] @classmethod
def Vec3(cls, vec: ArrayLike3) -> UnitQuaternion:
r"""
Construct a new unit quaternion from its vector part
:param vec: vector part of unit quaternion
:type vec: 3-element array_like
``UnitQuaternion.Vec(v)`` is a new unit quaternion with the specified vector part
and the scalar part is
.. math:: s = \sqrt{1 - v_x^2 - v_y^2 - v_z^2}
The unit quaternion will always have a positive scalar part.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rz(-4)
>>> print(q)
>>> q.vec3
>>> q2 = UQ.Vec3(q.vec3)
>>> print(q2)
>>> q == q2
:seealso: :meth:`UnitQuaternion.vec3`
"""
return cls(smb.v2q(vec))
[docs] def inv(self) -> UnitQuaternion:
"""
Inverse of unit quaternion
:return: unit-quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion
>>> print(UQ.Rx(0.3).inv())
>>> print(UQ.Rx(0.3).inv() * UQ.Rx(0.3))
>>> print(UQ.Rx([0.3, 0.6]).inv())
:seealso: :func:`~spatialmath.base.quaternions.qinv`
"""
return UnitQuaternion([smb.qconj(q._A) for q in self])
[docs] @staticmethod
def qvmul(qv1: ArrayLike3, qv2: ArrayLike3) -> R3:
"""
Multiply unit quaternions defined by unique vector parts
:param qv1: vector representation of first multiplicand
:type qv1: ndarray(3)
:param qv1: vector representation of second multiplicand
:type qv1: ndarray(3)
``UnitQuaternion(qv1, qv2)`` is the Hamilton product of two unit quaternions
represented in minimal vector form.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(-0.3)
>>> qv1 = q1.vec3
>>> qv1
>>> qv2 = q2.vec3
>>> qv = UQ.qvmul(qv1, qv2)
>>> qv
>>> print(UQ.Vec3(qv))
>>> print(UQ.Rx(0.3) * UQ.Ry(-0.3))
:seealso: :meth:`UnitQuaternion.vec3` :meth:`UnitQuaternion.Vec3`
"""
return smb.vvmul(qv1, qv2)
[docs] def dot(self, omega: ArrayLike3) -> R4:
"""
Rate of change of a unit quaternion in world frame
:param ω: angular velocity in world frame
:type ω: 3-element array_like
:return: rate of change of unit quaternion
:rtype: 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: :func:`~spatialmath.base.quaternions.qdot`
"""
return smb.qdot(self._A, omega)
[docs] def dotb(self, omega: ArrayLike3) -> R4:
"""
Rate of change of a unit quaternion in body frame
:param ω: angular velocity in body frame
:type ω: 3-element array_like
:return: rate of change of unit quaternion
:rtype: 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: :func:`~spatialmath.base.quaternions.qdotb`
"""
return smb.qdotb(self._A, omega)
[docs] def __mul__(
left, right: UnitQuaternion
) -> UnitQuaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Multiply unit quaternion
:return: product
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) * UQ.Rx(0.4))
>>> print(UQ.Rx(0.3) * 2)
>>> print(UQ.Rx(0.3) * [1, 2, 3])
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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) * UQ.Rx(0.4))
>>> q = UQ.Rx(0.3)
>>> q *= UQ.Rx(0.4))
>>> print(q)
>>> print(UQ.Rx(0.3) * UQ.Rx([0.4, 0.6]))
>>> print(UQ.Rx([0.3, 0.6]) * UQ.Rx(0.3))
>>> print(UQ.Rx([0.3, 0.6]) * UQ.Rx([0.3, 0.6]))
:seealso: :meth:`Quaternion.__mul__`
"""
if isinstance(left, right.__class__):
# quaternion * quaternion case (same class)
return right.__class__(left.binop(right, smb.qqmul))
elif smb.isscalar(right):
# quaternion * scalar case
# print('scalar * quat')
return Quaternion([right * q._A for q in left])
elif isinstance(right, (list, tuple, np.ndarray)):
# unit quaternion * vector
# print('*: pose x array')
if smb.isvector(right, 3):
v = smb.getvector(right)
if len(left) == 1:
# pose x vector
# print('*: pose x vector')
return smb.qvmul(left._A, smb.getvector(right, 3))
elif len(left) > 1 and smb.isvector(right, 3):
# pose array x vector
# print('*: pose array x vector')
return np.array([smb.qvmul(x, v) for x in left._A]).T
elif (
len(left) == 1 and isinstance(right, np.ndarray) and right.shape[0] == 3
):
# pose x stack of vectors
return np.array([smb.qvmul(left._A, x) for x in right.T]).T
else:
raise ValueError("bad operands")
else:
raise ValueError("UnitQuaternion: operands to * are of different types")
def __imul__(
left, right: UnitQuaternion
) -> UnitQuaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Multiply unit quaternion in place
:return: product
:rtype: UnitQuaternion, Quaternion
:raises: ValueError
Multiplies a quaternion in place. If the right operand is a list,
the result will be a list.
Example::
>>> q = UQ.Rx(0.3)
>>> q *= UQ.Rx(0.3)
>>> q
:seealso: :func:`__mul__`
"""
return left.__mul__(right)
[docs] def __truediv__(
left, right: UnitQuaternion
) -> UnitQuaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``/`` operator
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) / UQ.Rx(0.3))
>>> print(UQ.Rx(0.3) / 2)
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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> print(UQ.Rx(0.3) / UQ.Rx(0.3))
>>> print(UQ.Rx([0.3, 0.6]) / UQ.Rx(0.3))
>>> print(UQ.Rx(0.3) / UQ.Rx([0.3, 0.6]))
>>> print(UQ.Rx([0.3, 0.6]) / UQ.Rx([0.3, 0.6]))
"""
if isinstance(left, right.__class__):
return UnitQuaternion(
left.binop(right, lambda x, y: smb.qqmul(x, smb.qconj(y)))
)
elif smb.isscalar(right):
return Quaternion(left.binop(right, lambda x, y: x / y))
else:
raise ValueError("bad operands")
[docs] def __eq__(
left, right: UnitQuaternion
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``==`` operator
:rtype: bool
``q1 == q2`` is True if ``q1`` is elementwise equal to ``q2`` and accounts for the
double mapping. Supports broadcasting.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(0.3)
>>> q1 == q1
>>> q1 == (-q1)
>>> q1 == q2
>>> UQ([q1, q2]) == q1
>>> UQ([q1, q2]) == q2
>>> UQ([q1, q2]) == UQ([q1, q2])
:seealso: :func:`__ne__` :func:`~spatialmath.base.quaternions.qisequal`
"""
return left.binop(
right, lambda x, y: smb.qisequal(x, y, unitq=True), list1=False
)
[docs] def __ne__(
left, right: UnitQuaternion
) -> bool: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded ``!=`` operator
:rtype: bool
``q1 != q2`` is True if ``q1`` is elementwise not equal to ``q2`` and accounts for the
double mapping. Supports broadcasting.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3)
>>> q2 = UQ.Ry(0.3)
>>> q1 != q1
>>> q1 != (-q1)
>>> q1 != q2
>>> UQ([q1, q2]) == q1
>>> UQ([q1, q2]) == q2
>>> UQ([q1, q2]) == UQ([q1, q2])
:seealso: :func:`__eq__` :func:`~spatialmath.base.quaternions.qisequal`
"""
return left.binop(
right, lambda x, y: not smb.qisequal(x, y, unitq=True), list1=False
)
def __matmul__(
left, right: UnitQuaternion
) -> UnitQuaternion: # lgtm[py/not-named-self] pylint: disable=no-self-argument
"""
Overloaded @ operator
:return: product :rtype: UnitQuaternion
- ``q1 @ q2`` is the Hamilton product of ``q1`` and ``q2``, both unit
quaternions, followed by explicit normalization.
- `` q1 @= q2`` as above.
.. note:: This operator is functionally equivalent to ``*`` but is more
costly. It is useful for cases where a pose is incrementally update
over many cycles.
"""
return left.__class__(
left.binop(right, lambda x, y: smb.qunit(smb.qqmul(x, y)))
)
[docs] def interp(
self, end: UnitQuaternion, s: float = 0, shortest: Optional[bool] = False
) -> UnitQuaternion:
"""
Interpolate between two unit quaternions
:param end: final unit quaternion
:type end: UnitQuaternion
:param shortest: Take the shortest path along the great circle
:param s: interpolation coefficient, range 0 to 1, or number of steps
:type s: array_like or int
:return: interpolated unit quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q1 = UQ.Rx(0.3); q2 = UQ.Rz(-0.4)
>>> print(q1)
>>> print(q2)
>>> q1.interp(q2, 0) # this is q1
>>> q1.interp(q2, 1,) # this is q2
>>> q1.interp(q2, 0.5) # this is in between
>>> q = q1.interp(q2, 11) # in 11 steps
>>> len(q)
>>> q[0] # this is q1
>>> q[5] # this is in between
.. note:: values of ``s`` are silently clipped to the range [0, 1]
:seealso: :func:`~spatialmath.base.quaternions.qslerp`
"""
# TODO allow self to have len() > 1
if isinstance(s, int) and s > 1:
s = np.linspace(0, 1, s)
else:
s = smb.getvector(s)
s = np.clip(s, 0, 1) # enforce valid values
# 2 quaternion form
if not isinstance(end, UnitQuaternion):
raise TypeError("end argument must be a UnitQuaternion")
q1 = self.vec
q2 = end.vec
dot = smb.qinner(q1, q2)
# If the dot product is negative, the quaternions
# have opposite handed-ness and slerp won't take
# the shorter path. Fix by reversing one quaternion.
if shortest:
if dot < 0:
q1 = -q1
dot = -dot
# shouldn't be needed by handle numerical errors: -eps, 1+eps cases
dot = np.clip(dot, -1, 1) # Clip within domain of acos()
theta_0 = math.acos(dot) # theta_0 = angle between input vectors
qi = []
for sk in s:
theta = theta_0 * sk # theta = angle between v0 and result
s1 = float(math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0))
s2 = math.sin(theta) / math.sin(theta_0)
out = (q1 * s1) + (q2 * s2)
qi.append(out)
return UnitQuaternion(qi)
[docs] def interp1(self, s: float = 0, shortest: Optional[bool] = False) -> UnitQuaternion:
"""
Interpolate a unit quaternion
:param shortest: Take the shortest path along the great circle
:param s: interpolation coefficient, range 0 to 1, or number of steps
:type s: array_like or int
:return: interpolated unit quaternion
:rtype: 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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> q = UQ.Rx(0.3)
>>> print(q)
>>> q.interp1(0) # this is identity
>>> q.interp1(1) # this is q
>>> q.interp1(0.5) # this is in between
>>> qi = q.interp1(11) # in 11 steps
>>> len(qi)
>>> qi[0] # this is q1
>>> qi[5] # this is in between
.. note:: values of ``s`` are silently clipped to the range [0, 1]
:seealso: :func:`~spatialmath.base.quaternions.qslerp`
"""
# TODO allow self to have len() > 1
if isinstance(s, int) and s > 1:
s = np.linspace(0, 1, s)
else:
s = smb.getvector(s)
s = np.clip(s, 0, 1) # enforce valid values
q = self.vec
dot = q[0] # s
# If the dot product is negative, the quaternions
# have opposite handed-ness and slerp won't take
# the shorter path. Fix by reversing one quaternion.
if shortest:
if dot < 0:
q = -q
dot = -dot
# shouldn't be needed by handle numerical errors: -eps, 1+eps cases
dot = np.clip(dot, -1, 1) # Clip within domain of acos()
theta_0 = math.acos(dot) # theta_0 = angle between input vectors
qi = []
for sk in s:
theta = theta_0 * sk # theta = angle between v0 and result
s1 = float(math.cos(theta) - dot * math.sin(theta) / math.sin(theta_0))
s2 = math.sin(theta) / math.sin(theta_0)
out = np.r_[s1, 0, 0, 0] + (q * s2)
qi.append(out)
return UnitQuaternion(qi)
[docs] def increment(self, w: ArrayLike3, normalize: Optional[bool] = False) -> None:
"""
Quaternion incremental update
:param w: angular displacement, Euler vector
:type w: array_like(3)
:param normalize: normalize the result, defaults to False
:type normalize: bool, optional
.. note:: The object state is updated
"""
# is (v, theta) or None
try:
v, theta = smb.unitvec_norm(w)
except ValueError:
# zero update
return
ds = math.cos(theta / 2)
dv = math.sin(theta / 2) * v
updated = smb.qqmul(self.A, np.r_[ds, dv])
if normalize:
updated = smb.qunit(updated)
self.data = [updated]
[docs] def plot(self, *args: List, **kwargs):
"""
Plot unit quaternion as a coordinate frame
:param `**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: :func:`~spatialmath.base.transforms3d.trplot`
"""
smb.trplot(smb.q2r(self._A), *args, **kwargs)
[docs] def animate(self, *args: List, **kwargs):
"""
Plot unit quaternion as an animated coordinate frame
:param start: initial pose, defaults to null/identity
:type start: UnitQuaternion
:param `**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 :func:`~spatialmath.base.transforms3d.tranimate` :func:`~spatialmath.base.transforms3d.trplot`
"""
if len(self) > 1:
return smb.tranimate([smb.q2r(q) for q in self.data], *args, **kwargs)
else:
return smb.tranimate(smb.q2r(self._A), *args, **kwargs)
[docs] def rpy(
self, unit: Optional[str] = "rad", order: Optional[str] = "zyx"
) -> Union[R3, RNx3]:
"""
Unit quaternion as roll-pitch-yaw angles
:param order: angle sequence order, default to 'zyx'
:type order: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3-vector of roll-pitch-yaw angles
:rtype: 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 :math:`(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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rx(0.3).rpy()
>>> UQ.Rz([0.2, 0.3]).rpy()
:seealso: :meth:`SE3.RPY` :func:`~spatialmath.base.transforms3d.tr2rpy`
"""
if len(self) == 1:
return smb.tr2rpy(self.R, unit=unit, order=order)
else:
return np.array([smb.tr2rpy(q.R, unit=unit, order=order) for q in self])
[docs] def eul(self, unit: Optional[str] = "rad") -> Union[R3, RNx3]:
r"""
Unit quaternion as Euler angles
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3-vector of Euler angles
:rtype: ndarray(3)
``q.eul`` is the Euler angle representation of the rotation. Euler angles are
a 3-vector :math:`(\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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).eul()
>>> UQ.Ry([0.3, 0.4]).eul()
:seealso: :meth:`SE3.Eul` :func:`~spatialmath.base.transforms3d.tr2eul`
"""
if len(self) == 1:
return smb.tr2eul(self.R, unit=unit)
else:
return np.array([smb.tr2eul(q.R, unit=unit) for q in self])
[docs] def angvec(self, unit: Optional[str] = "rad") -> Tuple[float, R3]:
r"""
Unit quaternion as angle and rotation vector
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param check: check that rotation matrix is valid
:type check: bool
:return: :math:`(\theta, {\bf v})`
:rtype: float, ndarray(3)
``q.angvec()`` is a tuple :math:`(\theta, v)` containing the rotation
angle and a rotation axis which is equivalent to the rotation of
the unit quaternion ``q``.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).angvec()
:seealso: :meth:`Quaternion.AngVec` :meth:`UnitQuaternion.log` :func:`~spatialmath.base.transforms3d.angvec2r`
"""
return smb.tr2angvec(self.R, unit=unit)
# def log(self):
# r"""
# Logarithm of unit quaternion
# :rtype: Quaternion instance
# ``q.log()`` is the logarithm of the unit quaternion ``q``, ie.
# .. math::
# 0 \langle \frac{\mathb{v}}{\| \mathbf{v} \|} \acos s \rangle
# Example:
# .. runblock:: pycon
# >>> from spatialmath import UnitQuaternion
# >>> q = UnitQuaternion.Rx(0.3)
# >>> print(q.log())
# :reference: `Wikipedia <https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions>`_
# :seealso: :meth:`Quaternion.Quaternion.log`, `~spatialmath.quaternion.Quaternion.exp`
# """
# return Quaternion(s=0, v=math.acos(self.s) * smb.unitvec(self.v))
[docs] def angdist(self, other: UnitQuaternion, metric: Optional[int] = 3) -> float:
r"""
Angular distance metric between unit quaternions
:param other: second unit quaternion
:type other: UnitQuaternion instance
:param metric: metric, default is 3
:type metric: int
:raises TypeError: if other is not a UnitQuaternion
:return: angle in radians
:rtype: 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 :math:`1 - | \q_1 \bullet \q_2 | \in [0, 1]`
1 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
2 :math:`\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]`
3 :math:`2 \tan^{-1} \| \q_1 \pm \q_2\| / \|\q_1 \mp \q_2\| \in [0, \pi/2]`
4 :math:`\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:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion
>>> q1 = UnitQuaternion.Rx(0.3)
>>> q2 = UnitQuaternion.Ry(0.3)
>>> print(q1.angdist(q1))
>>> print(q1.angdist(q2))
.. 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
"""
if not isinstance(other, UnitQuaternion):
raise TypeError("bad operand")
if metric == 0:
measure = lambda p, q: 1 - abs(np.dot(p, q))
elif metric == 1:
measure = lambda p, q: math.acos(min(1.0, abs(np.dot(p, q))))
elif metric == 2:
measure = lambda p, q: math.acos(min(1.0, abs(np.dot(p, q))))
elif metric == 3:
def metric3(p, q):
x = smb.norm(p - q)
y = smb.norm(p + q)
if x >= y:
return 2 * math.atan(y / x)
else:
return 2 * math.atan(x / y)
measure = metric3
elif metric == 4:
measure = lambda p, q: math.acos(min(1.0, 2 * np.dot(p, q) ** 2 - 1))
ad = self.binop(other, measure)
if len(ad) == 1:
return ad[0]
else:
return np.array(ad)
[docs] def SO3(self) -> SO3:
"""
Unit quaternion as SO3 instance
:return: an SO(3) representation
:rtype: SO3 instance
``q.SO3()`` is an ``SO3`` instance representing the same rotation
as the unit quaternion ``q``.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).SO3()
"""
return SO3(self.R, check=False)
[docs] def SE3(self) -> SE3:
"""
Unit quaternion as SE3 instance
:return: an SE(3) representation
:rtype: SE3 instance
``q.SE3()`` is an ``SE3`` instance representing the same rotation
as the unit quaternion ``q`` and with zero translation.
Example:
.. runblock:: pycon
>>> from spatialmath import UnitQuaternion as UQ
>>> UQ.Rz(0.3).SE3()
"""
return SE3(smb.r2t(self.R), check=False)
if __name__ == "__main__": # pragma: no cover
import pathlib
a = UnitQuaternion([0, 1, 0, 0])
exec(
open(
pathlib.Path(__file__).parent.parent.absolute()
/ "tests"
/ "test_quaternion.py"
).read()
) # pylint: disable=exec-used