Source code for spatialmath.base.quaternions

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Fri Apr 10 14:12:56 2020

@author: Peter Corke
"""

# This file is part of the SpatialMath toolbox for Python
# https://github.com/petercorke/spatialmath-python
# 
# MIT License
# 
# Copyright (c) 1993-2020 Peter Corke
# 
# Permission is hereby granted, free of charge, to any person obtaining a copy
# of this software and associated documentation files (the "Software"), to deal
# in the Software without restriction, including without limitation the rights
# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
# copies of the Software, and to permit persons to whom the Software is
# furnished to do so, subject to the following conditions:
# 
# The above copyright notice and this permission notice shall be included in all
# copies or substantial portions of the Software.
# 
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
# SOFTWARE.

# Contributors:
# 
#     1. Luis Fernando Lara Tobar and Peter Corke, 2008
#     2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)
#     3. Peter Corke, 2020

import sys
import math
import numpy as np
from spatialmath import base as tr
from spatialmath.base import argcheck

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


[docs]def eye(): """ Create an identity quaternion :return: an identity quaternion :rtype: numpy.ndarray, shape=(4,) Creates an identity quaternion, with the scalar part equal to one, and a zero vector value. """ return np.r_[1, 0, 0, 0]
[docs]def pure(v): """ Create a pure quaternion :arg v: vector from a 3-vector :type v: array_like :return: pure quaternion :rtype: numpy.ndarray, shape=(4,) Creates a pure quaternion, with a zero scalar value and the vector part equal to the passed vector value. """ v = argcheck.getvector(v, 3) return np.r_[0, v]
[docs]def qnorm(q): r""" Norm of a quaternion :arg q: input quaternion as a 4-vector :type v: : array_like :return: norm of the quaternion :rtype: float Returns the norm, length or magnitude of the input quaternion which is :math:`\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}` :seealso: unit """ q = argcheck.getvector(q, 4) return np.linalg.norm(q)
[docs]def unit(q, tol=10): """ Create a unit quaternion :arg v: quaterion as a 4-vector :type v: array_like :return: a pure quaternion :rtype: numpy.ndarray, shape=(4,) Creates a unit quaternion, with unit norm, by scaling the input quaternion. .. seealso:: norm """ q = argcheck.getvector(q, 4) nm = np.linalg.norm(q) assert abs(nm) > tol * _eps, 'cannot normalize (near) zero length quaternion' return q / nm
[docs]def isunit(q, tol=10): """ Test if quaternion has unit length :param v: quaternion as a 4-vector :type v: array_like :param tol: tolerance in units of eps :type tol: float :return: whether quaternion has unit length :rtype: bool :seealso: unit """ return tr.iszerovec(q)
[docs]def isequal(q1, q2, tol=100, unitq=False): """ Test if quaternions are equal :param q1: quaternion as a 4-vector :type q1: array_like :param q2: quaternion as a 4-vector :type q2: array_like :param unitq: quaternions are unit quaternions :type unitq: bool :param tol: tolerance in units of eps :type tol: float :return: whether quaternion has unit length :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``. """ q1 = argcheck.getvector(q1, 4) q2 = argcheck.getvector(q2, 4) if unit: 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): """ Convert unit-quaternion to 3-vector :arg q: unit-quaternion as a 4-vector :type v: array_like :return: a unique 3-vector :rtype: numpy.ndarray, shape=(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. .. warning:: There is no check that the passed value is a unit-quaternion. .. seealso:: v2q """ q = argcheck.getvector(q, 4) if q[0] >= 0: return q[1:4] else: return -q[1:4]
[docs]def v2q(v): r""" Convert 3-vector to unit-quaternion :arg v: vector part of unit quaternion, a 3-vector :type v: array_like :return: a unit quaternion :rtype: numpy.ndarray, shape=(4,) Returns a unit-quaternion reconsituted from just its vector part. Assumes that the scalar part was positive, so :math:`s = \sqrt{1-||v||}`. .. seealso:: q2v """ v = argcheck.getvector(v, 3) s = math.sqrt(1 - np.sum(v**2)) return np.r_[s, v]
[docs]def qqmul(q1, q2): """ Quaternion multiplication :arg q0: left-hand quaternion as a 4-vector :type q0: : array_like :arg q1: right-hand quaternion as a 4-vector :type q1: array_like :return: quaternion product :rtype: numpy.ndarray, shape=(4,) This is the quaternion or Hamilton product. If both operands are unit-quaternions then the product will be a unit-quaternion. :seealso: qvmul, inner, vvmul """ q1 = argcheck.getvector(q1, 4) q2 = argcheck.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 inner(q1, q2): """ Quaternion innert product :arg q0: quaternion as a 4-vector :type q0: : array_like :arg q1: uaternion as a 4-vector :type q1: array_like :return: inner product :rtype: numpy.ndarray, shape=(4,) This is the inner or dot product of two quaternions, it is the sum of the element-wise product. :seealso: qvmul """ q1 = argcheck.getvector(q1, 4) q2 = argcheck.getvector(q2, 4) return np.dot(q1, q2)
[docs]def qvmul(q, v): """ Vector rotation :arg q: unit-quaternion as a 4-vector :type q: array_like :arg v: 3-vector to be rotated :type v: list, tuple, numpy.ndarray :return: rotated 3-vector :rtype: numpy.ndarray, shape=(3,) The vector `v` is rotated about the origin by the SO(3) equivalent of the unit quaternion. .. warning:: There is no check that the passed value is a unit-quaternions. :seealso: qvmul """ q = argcheck.getvector(q, 4) v = argcheck.getvector(v, 3) qv = qqmul(q, qqmul(pure(v), conj(q))) return qv[1:4]
[docs]def vvmul(qa, qb): """ Quaternion multiplication :arg qa: left-hand quaternion as a 3-vector :type qa: : array_like :arg qb: right-hand quaternion as a 3-vector :type qb: array_like :return: quaternion product :rtype: numpy.ndarray, shape=(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. :seealso: qvmul, inner """ 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 pow(q, power): """ Raise quaternion to a power :arg q: quaternion as a 4-vector :type v: array_like :arg power: exponent :type power: int :return: input quaternion raised to the specified power :rtype: numpy.ndarray, shape=(4,) Raises a quaternion to the specified power using repeated multiplication. Notes: - power must be an integer - power can be negative, in which case the conjugate is taken """ q = argcheck.getvector(q, 4) assert isinstance(power, int), "Power must be an integer" qr = eye() for i in range(0, abs(power)): qr = qqmul(qr, q) if power < 0: qr = conj(qr) return qr
[docs]def conj(q): """ Quaternion conjugate :arg q: quaternion as a 4-vector :type v: array_like :return: conjugate of input quaternion :rtype: numpy.ndarray, shape=(4,) Conjugate of quaternion, the vector part is negated. """ q = argcheck.getvector(q, 4) return np.r_[q[0], -q[1:4]]
[docs]def q2r(q): """ Convert unit-quaternion to SO(3) rotation matrix :arg q: unit-quaternion as a 4-vector :type v: array_like :return: corresponding SO(3) rotation matrix :rtype: numpy.ndarray, shape=(3,3) Returns an SO(3) rotation matrix corresponding to this unit-quaternion. .. warning:: There is no check that the passed value is a unit-quaternion. :seealso: r2q """ q = argcheck.getvector(q, 4) s = q[0] x = q[1] y = q[2] z = q[3] 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, check=True): """ Convert SO(3) rotation matrix to unit-quaternion :arg R: rotation matrix :type R: numpy.ndarray, shape=(3,3) :return: unit-quaternion :rtype: numpy.ndarray, shape=(3,) Returns a unit-quaternion corresponding to the input SO(3) rotation matrix. .. warning:: There is no check that the passed matrix is a valid rotation matrix. :seealso: q2r """ assert R.shape == (3, 3) and tr.isR(R), "Argument must be 3x3 rotation matrix" qs = math.sqrt(np.trace(R) + 1) / 2.0 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 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) 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) < 100 * _eps: return eye() else: return np.r_[qs, (math.sqrt(1.0 - qs ** 2) / nm) * kv]
[docs]def slerp(q0, q1, s, shortest=False): """ Quaternion conjugate :arg q0: initial unit quaternion as a 4-vector :type q0: array_like :arg q1: final unit quaternion as a 4-vector :type q1: array_like :arg s: interpolation coefficient in the range [0,1] :type s: float :arg shortest: choose shortest distance [default False] :type shortest: bool :return: interpolated unit-quaternion :rtype: numpy.ndarray, shape=(4,) 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. .. warning:: There is no check that the passed values are unit-quaternions. """ assert 0 <= s <= 1, 's must be in the interval [0,1]' q0 = argcheck.getvector(q0, 4) q1 = argcheck.getvector(q1, 4) if s == 0: return q0 elif s == 1: return q1 dot = 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 dot < 0: q0 = - q0 dot = -dot dot = np.clip(dot, -1, 1) # Clip within domain of acos() theta = math.acos(dot) # theta is the angle between rotation vectors if abs(theta) > 10*_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 rand(): """ Random unit-quaternion :return: random unit-quaternion :rtype: numpy.ndarray, shape=(4,) Computes a uniformly distributed random unit-quaternion which can be considered equivalent to a random SO(3) rotation. """ 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 matrix(q): """ Convert to 4x4 matrix equivalent :arg q: quaternion as a 4-vector :type v: array_like :return: equivalent matrix :rtype: numpy.ndarray, shape=(4,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. :seealso: qqmul """ q = argcheck.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 dot(q, w): """ Rate of change of unit-quaternion :arg q0: unit-quaternion as a 4-vector :type q0: array_like :arg w: angular velocity in world frame as a 3-vector :type w: array_like :return: rate of change of unit quaternion :rtype: numpy.ndarray, shape=(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. .. warning:: There is no check that the passed values are unit-quaternions. """ q = argcheck.getvector(q, 4) w = argcheck.getvector(w, 3) E = q[0] * (np.eye(3, 3)) - tr.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E@w]
[docs]def dotb(q, w): """ Rate of change of unit-quaternion :arg q0: unit-quaternion as a 4-vector :type q0: array_like :arg w: angular velocity in body frame as a 3-vector :type w: array_like :return: rate of change of unit quaternion :rtype: numpy.ndarray, shape=(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 body frame. .. warning:: There is no check that the passed values are unit-quaternions. """ q = argcheck.getvector(q, 4) w = argcheck.getvector(w, 3) E = q[0] * (np.eye(3, 3)) + tr.skew(q[1:4]) return 0.5 * np.r_[-np.dot(q[1:4], w), E@w]
[docs]def angle(q1, q2): """ Angle between two unit-quaternions :arg q0: unit-quaternion as a 4-vector :type q0: array_like :arg q1: unit-quaternion as a 4-vector :type q1: array_like :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. 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 = argcheck.getvector(q1, 4) q2 = argcheck.getvector(q2, 4) return 2.0 * math.atan2(norm(q1 - q2), norm(q1 + q2))
[docs]def qprint(q, delim=('<', '>'), fmt='%f', file=sys.stdout): """ Format a quaternion :arg q: unit-quaternion as a 4-vector :type q: array_like :arg delim: 2-list of delimeters [default ('<', '>')] :type delim: list or tuple of strings :arg fmt: printf-style format soecifier [default '%f'] :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. """ q = argcheck.getvector(q, 4) template = "# %s #, #, # %s".replace('#', fmt) s = template % (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 import os.path exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_quaternions.py")).read())