"""

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

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

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

"""

def __init__(self, s: Any = None, v=None, check: Optional[bool] = True):

r"""

Construct a new quaternion

:param s: scalar part

:type s: float or ndarray(N)

:param v: vector part

:type v: ndarray(3), ndarray(Nx3)

- ``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)]]

elif (

smb.isvector(s) and smb.ismatrix(v, (None, 3)) and s.shape[0] == v.shape[0]

):

# Quaternion(s, v) where s and v are arrays

self.data = [np.r_[_s, _v] for _s, _v in zip(s, v)]

else:

raise ValueError("bad argument to Quaternion constructor")

@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,)

@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)

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])

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])

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)

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 = np.log(norm)

if len(self) == 1:

if smb.iszerovec(self._A[1:4]):

v = np.zeros((3,))

else:

v = math.acos(np.clip(self._A[0] / norm, -1, 1)) * smb.unitvec(

self._A[1:4]

)

return Quaternion(s=s, v=v)

else:

v = [

np.zeros((3,))

if smb.iszerovec(A[1:4])

else math.acos(np.clip(A[0] / n, -1, 1)) * smb.unitvec(A[1:4])

for A, n in zip(self._A, norm)

]

return Quaternion(s=s, v=np.array(v))

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)

if smb.iszerovec(self.v, tol * _eps):

# result will be a unit quaternion

v = np.zeros((3,))

else:

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)

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

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)

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)

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)

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)

def __truediv__(self, other: Quaternion):

return NotImplemented # Quaternion division not supported

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))

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.q2str(q, delim=delim) for q in self.data])

# ========================================================================= #

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

"""

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