Source code for spatialmath.base.quaternions

# Part of Spatial Math Toolbox for Python
# Copyright (c) 2000 Peter Corke
# MIT Licence, see details in top-level file: LICENCE

"""
These functions create and manipulate quaternions or unit quaternions.
The quaternion is represented
by a 1D NumPy array with 4 elements: s, x, y, z.

"""
# pylint: disable=invalid-name

import sys
import math
import numpy as np
import spatialmath.base as smb
from spatialmath.base.types import *

_eps = np.finfo(np.float64).eps


[docs]def qeye() -> QuaternionArray: """ Create an identity quaternion :return: an identity quaternion :rtype: ndarray(4) Creates an identity quaternion, with the scalar part equal to one, and a zero vector value. .. runblock:: pycon >>> from spatialmath.base import qeye, qprint >>> q = qeye() >>> qprint(q) """ return np.r_[1, 0, 0, 0]
[docs]def qpure(v: ArrayLike3) -> QuaternionArray: """ Create a pure quaternion :arg v: 3D vector :type v: array_like(3) :return: pure quaternion :rtype: ndarray(4) Creates a pure quaternion, with a zero scalar value and the vector part equal to the passed vector value. .. runblock:: pycon >>> from spatialmath.base import pure, qprint >>> q = qpure([1, 2, 3]) >>> qprint(q) """ v = smb.getvector(v, 3) return np.r_[0, v]
[docs]def qpositive(q: ArrayLike4) -> QuaternionArray: """ Quaternion with positive scalar part :arg q: quaternion :type v: : ndarray(4) :return: pure quaternion :rtype: ndarray(4) If the scalar part is negative return -q. """ if q[0] < 0: return -q else: return q
[docs]def qnorm(q: ArrayLike4) -> float: r""" Norm of a quaternion :arg q: quaternion :type v: : array_like(4) :return: norm of the quaternion :rtype: float Returns the norm (length or magnitude) of the input quaternion which is .. math:: (s^2 + v_x^2 + v_y^2 + v_z^2)^{1/2} .. runblock:: pycon >>> from spatialmath.base import qnorm >>> q = qnorm([1, 2, 3, 4]) >>> print(q) :seealso: :func:`qunit` """ q = smb.getvector(q, 4) return np.linalg.norm(q)
[docs]def qunit(q: ArrayLike4, tol: float = 20) -> UnitQuaternionArray: """ Create a unit quaternion :arg v: quaterion :type v: array_like(4) :param tol: Tolerance in multiples of eps, defaults to 20 :type tol: float, optional :return: a pure quaternion :rtype: ndarray(4) :raises ValueError: quaternion has (near) zero norm Creates a unit quaternion, with unit norm, by scaling the input quaternion. .. runblock:: pycon >>> from spatialmath.base import qunit, qprint >>> q = qunit([1, 2, 3, 4]) >>> qprint(q) .. note:: Scalar part is always positive. .. note:: If the quaternion norm is less than ``tol * eps`` an exception is raised. :seealso: :func:`qnorm` """ q = smb.getvector(q, 4) nm = np.linalg.norm(q) if abs(nm) < tol * _eps: raise ValueError("cannot normalize (near) zero length quaternion") else: q /= nm if q[0] >= 0: return q else: return -q
[docs]def qisunit(q: ArrayLike4, tol: float = 20) -> bool: """ Test if quaternion has unit length :param v: quaternion :type v: array_like(4) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether quaternion has unit length :rtype: bool .. runblock:: pycon >>> from spatialmath.base import qeye, qpure, qisunit >>> q = qeye() >>> qisunit(q) >>> q = qpure([1, 2, 3]) >>> qisunit(q) :seealso: :func:`qunit` """ return smb.iszerovec(q, tol=tol)
@overload def qisequal( q1: ArrayLike4, q2: ArrayLike4, tol: float = 20, unitq: Optional[bool] = False, ) -> bool: ... @overload def qisequal( q1: ArrayLike4, q2: ArrayLike4, tol: float = 20, unitq: Optional[bool] = True, ) -> bool: ...
[docs]def qisequal(q1, q2, tol: float = 20, unitq: Optional[bool] = False): """ Test if quaternions are equal :param q1: quaternion :type q1: array_like(4) :param q2: quaternion :type q2: array_like(4) :param unitq: quaternions are unit quaternions :type unitq: bool :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether quaternions are equal :rtype: bool Tests if two quaternions are equal. For unit-quaternions ``unitq=True`` the double mapping is taken into account, that is ``q`` and ``-q`` represent the same orientation and ``isequal(q, -q, unitq=True)`` will return ``True``. .. runblock:: pycon >>> from spatialmath.base import qisequal >>> q1 = [1, 2, 3, 4] >>> q2 = [-1, -2, -3, -4] >>> qisequal(q1, q2) >>> qisequal(q1, q2, unitq=True) """ q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) if unitq: return (np.sum(np.abs(q1 - q2)) < tol * _eps) or ( np.sum(np.abs(q1 + q2)) < tol * _eps ) else: return np.sum(np.abs(q1 - q2)) < tol * _eps
[docs]def q2v(q: ArrayLike4) -> R3: """ Convert unit-quaternion to 3-vector :arg q: unit-quaternion :type v: array_like(4) :return: a unique 3-vector :rtype: ndarray(3) Returns a unique 3-vector representing the input unit-quaternion. The sign of the scalar part is made positive, if necessary by multiplying the entire quaternion by -1, then the vector part is taken. .. runblock:: pycon >>> from spatialmath.base import q2v >>> from math import sqrt >>> q = [1 / sqrt(2), 0, 1 / sqrt(2), 0] >>> print(q2v(q)) >>> q = [-1 / sqrt(2), 0, 1 / sqrt(2), 0] >>> print(q2v(q)) .. warning:: There is no check that the passed value is a unit-quaternion. :seealso: :func:`v2q` """ q = smb.getvector(q, 4) if q[0] >= 0: return q[1:4] else: return -q[1:4]
[docs]def v2q(v: ArrayLike3) -> UnitQuaternionArray: r""" Convert 3-vector to unit-quaternion :arg v: vector part of unit quaternion :type v: array_like(3) :return: a unit quaternion :rtype: ndarray(4) Returns a unit-quaternion reconsituted from just its vector part. Assumes that the scalar part was positive, so :math:`s = \sqrt{1-||v||}`. .. runblock:: pycon >>> from spatialmath.base import v2q, qprint >>> from math import sqrt >>> v = [0, 1 / sqrt(2), 0] >>> qprint(v2q(v)) >>> v = [0, -1 / sqrt(2), 0] >>> qprint(v2q(v)) .. warning:: There is no check that the value is the vector part of a unit-quaternion, and this can lead to a math domain error. :seealso: :func:`q2v` """ v = smb.getvector(v, 3) s = math.sqrt(1 - np.sum(v**2)) return np.r_[s, v]
[docs]def qqmul(q1: ArrayLike4, q2: ArrayLike4) -> QuaternionArray: """ Quaternion multiplication :arg q0: left-hand quaternion :type q0: : array_like(4) :arg q1: right-hand quaternion :type q1: array_like(4) :return: quaternion product :rtype: ndarray(4) This is the quaternion or Hamilton product. If both operands are unit-quaternions then the product will be a unit-quaternion. .. runblock:: pycon >>> from spatialmath.base import qqmul >>> q1 = [1, 2, 3, 4] >>> q2 = [5, 6, 7, 8] >>> qqmul(q1, q2) # conventional Hamilton product :seealso: qvmul, qinner, vvmul """ q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) s1 = q1[0] v1 = q1[1:4] s2 = q2[0] v2 = q2[1:4] return np.r_[s1 * s2 - np.dot(v1, v2), s1 * v2 + s2 * v1 + np.cross(v1, v2)]
[docs]def qinner(q1: ArrayLike4, q2: ArrayLike4) -> float: """ Quaternion inner product :arg q0: quaternion :type q0: : array_like(4) :arg q1: uaternion :type q1: array_like(4) :return: inner product :rtype: float This is the inner or dot product of two quaternions, it is the sum of the element-wise product. - The inner product ``inner(q, q)`` is the square of the norm of ``q``. - If ``q0`` and ``q1`` are unit quaternions then the inner product is the cosine of the angle between the two orientations. .. runblock:: pycon >>> from spatialmath.base import qinner >>> from math import sqrt, acos, pi >>> q1 = [1, 2, 3, 4] >>> qinner(q1, q1) # square of the norm >>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis >>> acos(qinner(q1, q2)) * 180 / pi # angle between q1 and q2 :seealso: qvmul """ q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) return np.dot(q1, q2)
[docs]def qvmul(q: ArrayLike4, v: ArrayLike3) -> R3: """ Vector rotation :arg q: unit-quaternion :type q: array_like(4) :arg v: 3-vector to be rotated :type v: array_like(3) :return: rotated 3-vector :rtype: ndarray(3) The vector `v` is rotated about the origin by the SO(3) equivalent of the unit quaternion. .. runblock:: pycon >>> from spatialmath.base import qvmul >>> from math import sqrt >>> q = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> qvmul(q, [1, 2, 3]) # rotated vector .. warning:: There is no check that the passed value is a unit-quaternions. :seealso: qvmul """ q = smb.getvector(q, 4) v = smb.getvector(v, 3) qv = qqmul(q, qqmul(qpure(v), qconj(q))) return qv[1:4]
[docs]def vvmul(qa: ArrayLike3, qb: ArrayLike3) -> R3: """ Quaternion multiplication :arg qa: left-hand quaternion :type qa: : array_like(3) :arg qb: right-hand quaternion :type qb: array_like(3) :return: quaternion product :rtype: ndarray(3) This is the quaternion or Hamilton product of unit-quaternions defined only by their vector components. The product will be a unit-quaternion, defined only by its vector component. .. runblock:: pycon >>> from spatialmath.base import vvmul, v2q, q2v, qqmul, qprint >>> from math import sqrt >>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis >>> qprint(qqmul(q1, q2)) # normal Hamilton product >>> v1 = q2v(q1); v2 = q2v(q2) >>> vp = vvmul(v1, v2) # product using 3-vectors >>> qprint(v2q(vp)) # same answer as Hamilton product :seealso: :func:`q2v` :func:`v2q` :func:`qvmul` """ t6 = math.sqrt(1.0 - np.sum(qa**2)) t11 = math.sqrt(1.0 - np.sum(qb**2)) return np.r_[ qa[1] * qb[2] - qb[1] * qa[2] + qb[0] * t6 + qa[0] * t11, -qa[0] * qb[2] + qb[0] * qa[2] + qb[1] * t6 + qa[1] * t11, qa[0] * qb[1] - qb[0] * qa[1] + qb[2] * t6 + qa[2] * t11, ]
[docs]def qpow(q: ArrayLike4, power: int) -> QuaternionArray: """ Raise quaternion to a power :arg q: quaternion :type v: array_like(4) :arg power: exponent :type power: int :return: input quaternion raised to the specified power :rtype: ndarray(4) :raises ValueError: if exponent is non integer Raises a quaternion to the specified power using repeated multiplication. .. runblock:: pycon >>> from spatialmath.base import qpow, qqmul, qprint >>> q = [1, 2, 3, 4] >>> qprint(qqmul(q, q)) >>> qprint(qpow(q, 2)) >>> qprint(qpow(q, -2)) # conjugate of above .. note: - Power must be an integer - Power can be negative, in which case the conjugate is taken :seealso: :func:`qqmul` :SymPy: supported for ``q`` but not ``power``. """ q = smb.getvector(q, 4) if not isinstance(power, int): raise ValueError("Power must be an integer") qr = qeye() for _ in range(0, abs(power)): qr = qqmul(qr, q) if power < 0: qr = qconj(qr) return qr
[docs]def qconj(q: ArrayLike4) -> QuaternionArray: """ Quaternion conjugate :arg q: quaternion :type v: array_like(4) :return: conjugate of input quaternion :rtype: ndarray(4) Conjugate of quaternion, the vector part is negated. .. runblock:: pycon >>> from spatialmath.base import qconj, qprint >>> q = [1, 2, 3, 4] >>> qprint(qconj(q)) :SymPy: supported """ q = smb.getvector(q, 4) return np.r_[q[0], -q[1:4]]
[docs]def q2r( q: Union[UnitQuaternionArray, ArrayLike4], order: Optional[str] = "sxyz" ) -> SO3Array: """ Convert unit-quaternion to SO(3) rotation matrix :arg q: unit-quaternion :type v: array_like(4) :param order: the order of the quaternion elements. Must be 'sxyz' or 'xyzs'. Defaults to 'sxyz'. :type order: str :return: corresponding SO(3) rotation matrix :rtype: ndarray(3,3) Returns an SO(3) rotation matrix corresponding to this unit-quaternion. .. runblock:: pycon >>> from spatialmath.base import q2r >>> q = [0, 0, 1, 0] # rotation of 180deg about y-axis >>> print(q2r(q)) .. warning:: There is no check that the passed value is a unit-quaternion. :seealso: :func:`r2q` """ q = smb.getvector(q, 4) if order == "sxyz": s, x, y, z = q elif order == "xyzs": x, y, z, s = q else: raise ValueError("order is invalid, must be 'sxyz' or 'xyzs'") return np.array( [ [1 - 2 * (y**2 + z**2), 2 * (x * y - s * z), 2 * (x * z + s * y)], [2 * (x * y + s * z), 1 - 2 * (x**2 + z**2), 2 * (y * z - s * x)], [2 * (x * z - s * y), 2 * (y * z + s * x), 1 - 2 * (x**2 + y**2)], ] )
[docs]def r2q( R: SO3Array, check: Optional[bool] = False, tol: float = 20, order: Optional[str] = "sxyz", shortest: bool = False, ) -> UnitQuaternionArray: """ Convert SO(3) rotation matrix to unit-quaternion :arg R: SO(3) rotation matrix :type R: ndarray(3,3) :param check: check validity of rotation matrix, default False :type check: bool :param tol: tolerance in units of eps, defaults to 20 :type tol: float :param order: the order of the returned quaternion elements. Must be 'sxyz' or 'xyzs'. Defaults to 'sxyz'. :type order: str :param shortest: ensures the quaternion has non-negative scalar part. :type shortest: bool, default to False :return: unit-quaternion as Euler parameters :rtype: ndarray(4) :raises ValueError: for non SO(3) argument Returns a unit-quaternion corresponding to the input SO(3) rotation matrix. .. runblock:: pycon >>> from spatialmath.base import r2q, qprint, rotx >>> R = rotx(90, 'deg') # rotation of 90deg about x-axis >>> print(R) >>> qprint(r2q(R)) .. warning:: There is no check that the passed matrix is a valid rotation matrix. .. note:: - Scalar part is always positive - implements Cayley's method :reference: - Sarabandi, S., and Thomas, F. (March 1, 2019). "A Survey on the Computation of Quaternions From Rotation Matrices." ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006. `doi.org/10.1115/1.4041889 <https://doi.org/10.1115/1.4041889>`_ :seealso: :func:`q2r` """ if not smb.isrot(R, check=check, tol=tol): raise ValueError("Argument must be a valid SO(3) matrix") t12p = (R[0, 1] + R[1, 0]) ** 2 t13p = (R[0, 2] + R[2, 0]) ** 2 t23p = (R[1, 2] + R[2, 1]) ** 2 t12m = (R[0, 1] - R[1, 0]) ** 2 t13m = (R[0, 2] - R[2, 0]) ** 2 t23m = (R[1, 2] - R[2, 1]) ** 2 d1 = (R[0, 0] + R[1, 1] + R[2, 2] + 1) ** 2 d2 = (R[0, 0] - R[1, 1] - R[2, 2] + 1) ** 2 d3 = (-R[0, 0] + R[1, 1] - R[2, 2] + 1) ** 2 d4 = (-R[0, 0] - R[1, 1] + R[2, 2] + 1) ** 2 e = np.array( [ math.sqrt(d1 + t23m + t13m + t12m) / 4.0, math.sqrt(t23m + d2 + t12p + t13p) / 4.0, math.sqrt(t13m + t12p + d3 + t23p) / 4.0, math.sqrt(t12m + t13p + t23p + d4) / 4.0, ] ) i = np.argmax(e) if i == 0: e[1] = math.copysign(e[1], R[2, 1] - R[1, 2]) e[2] = math.copysign(e[2], R[0, 2] - R[2, 0]) e[3] = math.copysign(e[3], R[1, 0] - R[0, 1]) elif i == 1: e[0] = math.copysign(e[0], R[2, 1] - R[1, 2]) e[2] = math.copysign(e[2], R[1, 0] + R[0, 1]) e[3] = math.copysign(e[3], R[0, 2] + R[2, 0]) elif i == 2: e[0] = math.copysign(e[0], R[0, 2] - R[2, 0]) e[1] = math.copysign(e[1], R[1, 0] + R[0, 1]) e[3] = math.copysign(e[3], R[2, 1] + R[1, 2]) else: e[0] = math.copysign(e[0], R[1, 0] - R[0, 1]) e[1] = math.copysign(e[1], R[0, 2] + R[2, 0]) e[2] = math.copysign(e[2], R[2, 1] + R[1, 2]) if shortest and e[0] < 0: e = -e if order == "sxyz": return e elif order == "xyzs": return e[[1, 2, 3, 0]] else: raise ValueError("order is invalid, must be 'sxyz' or 'xyzs'")
# def r2q_svd(R): # U = np.array( # [ # [ # R[0, 0] + R[1, 1] + R[2, 2] + 1, # R[2, 1] - R[1, 2], # -R[2, 0] + R[0, 2], # R[1, 0] - R[0, 1], # ], # [ # R[2, 1] - R[1, 2], # R[0, 0] - R[1, 1] - R[2, 2] + 1, # R[1, 0] + R[0, 1], # R[2, 0] + R[0, 2], # ], # [ # -R[2, 0] + R[0, 2], # R[1, 0] + R[0, 1], # -R[0, 0] + R[1, 1] - R[2, 2] + 1, # R[2, 1] + R[1, 2], # ], # [ # R[1, 0] - R[0, 1], # R[2, 0] + R[0, 2], # R[2, 1] + R[1, 2], # -R[0, 0] - R[1, 1] + R[2, 2] + 1, # ], # ] # ) # U, S, VT = np.linalg.svd(U) # e = U[:, 0] # # if e[0] < -10 * _eps: # # e = -e # return e # def r2q_old(R, check=False, tol=100): # """ # Convert SO(3) rotation matrix to unit-quaternion # :arg R: SO(3) rotation matrix # :type R: ndarray(3,3) # :param check: check validity of rotation matrix, default False # :type check: bool # :param tol: tolerance in units of eps # :type tol: float # :return: unit-quaternion # :rtype: ndarray(4) # :raises ValueError: for non SO(3) argument # Returns a unit-quaternion corresponding to the input SO(3) rotation matrix. # .. runblock:: pycon # >>> from spatialmath.base import r2q, qprint, rotx # >>> R = rotx(90, 'deg') # rotation of 90deg about x-axis # >>> print(R) # >>> qprint(r2q(R)) # .. warning:: There is no check that the passed matrix is a valid rotation matrix. # .. note:: Scalar part is always positive. # :reference: # - Funda, Taylor, IEEE Trans. Robotics and Automation, 6(3), # June 1990, pp.382-388. (coding reference) # - Sarabandi, S., and Thomas, F. (March 1, 2019). # "A Survey on the Computation of Quaternions From Rotation Matrices." # ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006. (according to this # paper the algorithm is Hughes' method) # :seealso: :func:`q2r` # """ # if not smb.isrot(R, check=check, tol=tol): # raise ValueError("Argument must be a valid SO(3) matrix") # qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part # kx = R[2, 1] - R[1, 2] # Oz - Ay # ky = R[0, 2] - R[2, 0] # Ax - Nz # kz = R[1, 0] - R[0, 1] # Ny - Ox # # equation (7) # if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]): # kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1 # ky1 = R[1, 0] + R[0, 1] # Ny + Ox # kz1 = R[2, 0] + R[0, 2] # Nz + Ax # add = kx >= 0 # elif R[1, 1] >= R[2, 2]: # kx1 = R[1, 0] + R[0, 1] # Ny + Ox # ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1 # kz1 = R[2, 1] + R[1, 2] # Oz + Ay # add = ky >= 0 # else: # kx1 = R[2, 0] + R[0, 2] # Nz + Ax # ky1 = R[2, 1] + R[1, 2] # Oz + Ay # kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1 # add = kz >= 0 # # equation (8) # if add: # kx = kx + kx1 # ky = ky + ky1 # kz = kz + kz1 # else: # kx = kx - kx1 # ky = ky - ky1 # kz = kz - kz1 # kv = np.r_[kx, ky, kz] # nm = np.linalg.norm(kv) # if abs(nm) < tol * _eps: # return qeye() # else: # return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
[docs]def qslerp( q0: ArrayLike4, q1: ArrayLike4, s: float, shortest: Optional[bool] = False, tol: float = 20, ) -> UnitQuaternionArray: """ Quaternion conjugate :arg q0: initial unit quaternion :type q0: array_like(4) :arg q1: final unit quaternion :type q1: array_like(4) :arg s: interpolation coefficient in the range [0,1] :type s: float :arg shortest: choose shortest distance [default False] :type shortest: bool :param tol: Tolerance when checking for identical quaternions, in multiples of eps, defaults to 20 :type tol: float, optional :return: interpolated unit-quaternion :rtype: ndarray(4) :raises ValueError: s is outside interval [0, 1] An interpolated quaternion between ``q0`` when ``s`` = 0 to ``q1`` when ``s`` = 1. Interpolation is performed on a great circle on a 4D hypersphere. This is a rotation about a single fixed axis in space which yields the straightest and shortest path between two points. For large rotations the path may be the *long way around* the circle, the option ``'shortest'`` ensures always the shortest path. .. runblock:: pycon >>> from spatialmath.base import qslerp, qprint >>> from math import sqrt >>> q0 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> q1 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis >>> qprint(qslerp(q0, q1, 0)) # this is q0 >>> qprint(qslerp(q0, q1, 1)) # this is q1 >>> qprint(qslerp(q0, q1, 0.5)) # this is in "half way" between .. warning:: There is no check that the passed values are unit-quaternions. """ if not 0 <= s <= 1: raise ValueError("s must be in the interval [0,1]") q0 = smb.getvector(q0, 4) q1 = smb.getvector(q1, 4) if s == 0: return q0 elif s == 1: return q1 dotprod = np.dot(q0, q1) # 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 dotprod < 0: q0 = -q0 # pylint: disable=invalid-unary-operand-type dotprod = -dotprod # pylint: disable=invalid-unary-operand-type dotprod = np.clip(dotprod, -1, 1) # Clip within domain of acos() theta = math.acos(dotprod) # theta is the angle between rotation vectors if abs(theta) > tol * _eps: s0 = math.sin((1 - s) * theta) s1 = math.sin(s * theta) return ((q0 * s0) + (q1 * s1)) / math.sin(theta) else: # quaternions are identical return q0
[docs]def qrand() -> UnitQuaternionArray: """ Random unit-quaternion :return: random unit-quaternion :rtype: ndarray(4) Computes a uniformly distributed random unit-quaternion which can be considered equivalent to a random SO(3) rotation. .. runblock:: pycon >>> from spatialmath.base import qrand, qprint >>> qprint(qrand()) """ u = np.random.uniform(low=0, high=1, size=3) # get 3 random numbers in [0,1] return np.r_[ math.sqrt(1 - u[0]) * math.sin(2 * math.pi * u[1]), math.sqrt(1 - u[0]) * math.cos(2 * math.pi * u[1]), math.sqrt(u[0]) * math.sin(2 * math.pi * u[2]), math.sqrt(u[0]) * math.cos(2 * math.pi * u[2]), ]
[docs]def qmatrix(q: ArrayLike4) -> R4x4: """ Convert quaternion to 4x4 matrix equivalent :arg q: quaternion :type v: array_like(4) :return: equivalent matrix :rtype: ndarray(4) Hamilton multiplication between two quaternions can be considered as a matrix-vector product, the left-hand quaternion is represented by an equivalent 4x4 matrix and the right-hand quaternion as 4x1 column vector. .. runblock:: pycon >>> from spatialmath.base import qmatrix, qqmul, qprint >>> q1 = [1, 2, 3, 4] >>> q2 = [5, 6, 7, 8] >>> qqmul(q1, q2) # conventional Hamilton product >>> m = qmatrix(q1) >>> print(m) >>> v = m @ np.array(q2) >>> print(v) :seealso: qqmul """ q = smb.getvector(q, 4) s = q[0] x = q[1] y = q[2] z = q[3] return np.array([[s, -x, -y, -z], [x, s, -z, y], [y, z, s, -x], [z, -y, x, s]])
[docs]def qdot(q: ArrayLike4, w: ArrayLike3) -> QuaternionArray: """ Rate of change of unit-quaternion :arg q0: unit-quaternion :type q0: array_like(4) :arg w: 3D angular velocity in world frame :type w: array_like(3) :return: rate of change of unit quaternion :rtype: ndarray(4) ``dot(q, w)`` is the rate of change of the elements of the unit quaternion ``q`` which represents the orientation of a body frame with angular velocity ``w`` in the world frame. .. runblock:: pycon >>> from spatialmath.base import qdot, qprint >>> from math import sqrt >>> q = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> qdot(q, [1, 2, 3]) .. warning:: There is no check that the passed values are unit-quaternions. """ q = smb.getvector(q, 4) w = smb.getvector(w, 3) E = q[0] * (np.eye(3, 3)) - smb.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
[docs]def qdotb(q: ArrayLike4, w: ArrayLike3) -> QuaternionArray: """ Rate of change of unit-quaternion :arg q0: unit-quaternion :type q0: array_like(4) :arg w: 3D angular velocity in body frame :type w: array_like(3) :return: rate of change of unit quaternion :rtype: ndarray(4) ``dotb(q, w)`` is the rate of change of the elements of the unit quaternion ``q`` which represents the orientation of a body frame with angular velocity ``w`` in the body frame. .. runblock:: pycon >>> from spatialmath.base import qdotb, qprint >>> from math import sqrt >>> q = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> qdotb(q, [1, 2, 3]) .. warning:: There is no check that the passed values are unit-quaternions. """ q = smb.getvector(q, 4) w = smb.getvector(w, 3) E = q[0] * (np.eye(3, 3)) + smb.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E @ w]
[docs]def qangle(q1: ArrayLike4, q2: ArrayLike4) -> float: """ Angle between two unit-quaternions :arg q0: unit-quaternion :type q0: array_like(4) :arg q1: unit-quaternion :type q1: array_like(4) :return: angle between the rotations [radians] :rtype: float If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second. .. runblock:: pycon >>> from spatialmath.base import qangle >>> from math import sqrt >>> q1 = [1/sqrt(2), 1/sqrt(2), 0, 0] # 90deg rotation about x-axis >>> q2 = [1/sqrt(2), 0, 1/sqrt(2), 0] # 90deg rotation about y-axis >>> qangle(q1, q2) :References: - Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2. .. warning:: There is no check that the passed values are unit-quaternions. """ # TODO different methods q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) return 4.0 * math.atan2(smb.norm(q1 - q2), smb.norm(q1 + q2))
[docs]def qprint( q: Union[ArrayLike4, ArrayLike4], delim: Optional[Tuple[str, str]] = ("<", ">"), fmt: Optional[str] = "{: .4f}", file: Optional[TextIO] = sys.stdout, ) -> str: """ Format a quaternion :arg q: unit-quaternion :type q: array_like(4) :arg delim: 2-list of delimeters [default ('<', '>')] :type delim: list or tuple of strings :arg fmt: printf-style format soecifier [default '{: .4f}'] :type fmt: str :arg file: destination for formatted string [default sys.stdout] :type file: file object :return: formatted string :rtype: str Format the quaternion in a human-readable form as:: S D1 VX VY VZ D2 where S, VX, VY, VZ are the quaternion elements, and D1 and D2 are a pair of delimeters given by `delim`. By default the string is written to `sys.stdout`. If `file=None` then a string is returned. .. runblock:: pycon >>> from spatialmath.base import qprint, qrand >>> q = [1, 2, 3, 4] >>> qprint(q) >>> q = qrand() # a unit quaternion >>> qprint(q, delim=('<<', '>>')) """ q = smb.getvector(q, 4) template = "# {} #, #, # {}".replace("#", fmt) s = template.format(q[0], delim[0], q[1], q[2], q[3], delim[1]) if file: file.write(s + "\n") else: return s
if __name__ == "__main__": # pragma: no cover import pathlib exec( open( pathlib.Path(__file__).parent.parent.parent.absolute() / "tests" / "base" / "test_quaternions.py" ).read() ) # pylint: disable=exec-used