Source code for spatialmath.base.vectors

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

"""
Functions to manipulate vectors

Vector arguments are what numpy refers to as ``array_like`` and can be a list,
tuple, numpy array, numpy row vector or numpy column vector.
"""

# pylint: disable=invalid-name

import math
import numpy as np
from spatialmath.base.argcheck import getvector
from spatialmath.base.types import *

try:  # pragma: no cover
    # print('Using SymPy')
    import sympy

    _symbolics = True

except ImportError:  # pragma: no cover
    _symbolics = False

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


[docs]def norm(v: ArrayLikePure) -> float: """ Norm of vector :param v: any vector :type v: array_like(n) :return: norm of vector :rtype: float ``norm(v)`` is the 2-norm (length or magnitude) of the vector ``v``. .. runblock:: pycon >>> from spatialmath.base import * >>> norm([3, 4]) .. note:: This function does not use NumPy, it is ~2x faster than `numpy.linalg.norm()` for a 3-vector :seealso: :func:`~spatialmath.base.unit` :SymPy: supported """ sum = 0 for x in v: sum += x * x if _symbolics and isinstance(sum, sympy.Expr): return sympy.sqrt(sum) else: return math.sqrt(sum)
[docs]def normsq(v: ArrayLikePure) -> float: """ Squared norm of vector :param v: any vector :type v: array_like(n) :return: norm of vector :rtype: float ``norm(sq)`` is the sum of squared elements of the vector ``v`` or :math:`|v|^2`. .. runblock:: pycon >>> from spatialmath.base import * >>> normsq([2, 3]) .. note:: This function does not use NumPy, it is ~2x faster than `numpy.linalg.norm() ** 2` for a 3-vector :seealso: :func:`~spatialmath.base.unit` :SymPy: supported """ sum = 0 for x in v: sum += x * x return sum
[docs]def cross(u: ArrayLike3, v: ArrayLike3) -> R3: """ Cross product of vectors :param u: any vector :type u: array_like(3) :param v: any vector :type v: array_like(3) :return: cross product :rtype: nd.array(3) ``cross(u, v)`` is the cross product of the vectors ``u`` and ``v``. .. runblock:: pycon >>> from spatialmath.base import * >>> cross([1, 0, 0], [0, 1, 0]) .. note:: This function does not use NumPy, it is ~1.5x faster than `numpy.cross()` :seealso: :func:`~spatialmath.base.unit` :SymPy: supported """ return np.r_[ u[1] * v[2] - u[2] * v[1], u[2] * v[0] - u[0] * v[2], u[0] * v[1] - u[1] * v[0] ]
[docs]def colvec(v: ArrayLike) -> NDArray: """ Create a column vector :param v: any vector :type v: array_like(n) :return: a column vector :rtype: ndarray(n,1) Convert input to a column vector. .. runblock:: pycon >>> from spatialmath.base import * >>> colvec([1, 2, 3]) """ v = getvector(v) return np.array(v).reshape((len(v), 1))
[docs]def unitvec(v: ArrayLike, tol: float = 20) -> NDArray: """ Create a unit vector :param v: any vector :type v: array_like(n) :param tol: Tolerance in units of eps for zero-norm case, defaults to 20 :type: float :return: a unit-vector parallel to ``v``. :rtype: ndarray(n) :raises ValueError: for zero length vector ``unitvec(v)`` is a vector parallel to `v` of unit length. .. runblock:: pycon >>> from spatialmath.base import * >>> unitvec([3, 4]) :seealso: :func:`~numpy.linalg.norm` """ v = getvector(v) n = norm(v) if n > tol * _eps: # if greater than eps return v / n else: raise ValueError("zero norm vector")
[docs]def unitvec_norm(v: ArrayLike, tol: float = 20) -> Tuple[NDArray, float]: """ Create a unit vector :param v: any vector :type v: array_like(n) :param tol: Tolerance in units of eps for zero-norm case, defaults to 20 :type: float :return: a unit-vector parallel to ``v`` and the norm :rtype: (ndarray(n), float) :raises ValueError: for zero length vector ``unitvec(v)`` is a vector parallel to `v` of unit length. .. runblock:: pycon >>> from spatialmath.base import * >>> unitvec([3, 4]) :seealso: :func:`~numpy.linalg.norm` """ v = getvector(v) nm = norm(v) if nm > tol * _eps: # if greater than eps return (v / nm, nm) else: raise ValueError("zero norm vector")
[docs]def isunitvec(v: ArrayLike, tol: float = 20) -> bool: """ Test if vector has unit length :param v: vector to test :type v: ndarray(n) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has unit length :rtype: bool .. runblock:: pycon >>> from spatialmath.base import * >>> isunitvec([1, 0]) >>> isunitvec([1, 2]) :seealso: unit, iszerovec, isunittwist """ return bool(abs(np.linalg.norm(v) - 1) < tol * _eps)
[docs]def iszerovec(v: ArrayLike, tol: float = 20) -> bool: """ Test if vector has zero length :param v: vector to test :type v: ndarray(n) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has zero length :rtype: bool .. runblock:: pycon >>> from spatialmath.base import * >>> iszerovec([0, 0]) >>> iszerovec([1, 2]) :seealso: unit, isunitvec, isunittwist """ return bool(np.linalg.norm(v) < tol * _eps)
[docs]def iszero(v: float, tol: float = 20) -> bool: """ Test if scalar is zero :param v: value to test :type v: float :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether value is zero :rtype: bool .. runblock:: pycon >>> from spatialmath.base import * >>> iszero(0) >>> iszero(1) :seealso: unit, iszerovec, isunittwist """ return bool(abs(v) < tol * _eps)
[docs]def isunittwist(v: ArrayLike6, tol: float = 20) -> bool: r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: twist vector to test :type v: array_like(6) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether twist has unit length :rtype: bool :raises ValueError: for incorrect vector length Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and :math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3). A unit twist can be a: - unit rotational twist where :math:`|| \omega || = 1`, or - unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`. .. runblock:: pycon >>> from spatialmath.base import * >>> isunittwist([1, 2, 3, 1, 0, 0]) >>> isunittwist([0, 0, 0, 2, 0, 0]) :seealso: unit, isunitvec """ v = getvector(v) if len(v) == 6: # test for SE(3) twist return isunitvec(v[3:6], tol=tol) or ( iszerovec(v[3:6], tol=tol) and isunitvec(v[0:3], tol=tol) ) else: raise ValueError
[docs]def isunittwist2(v: ArrayLike3, tol: float = 20) -> bool: r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: twist vector to test :type v: array_like(3) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: whether vector has unit length :rtype: bool :raises ValueError: for incorrect vector length Vector is is intepretted as :math:`[v, \omega]` where :math:`v \in \mathbb{R}^n` and :math:`\omega \in \mathbb{R}^1` for SE(2) and :math:`\omega \in \mathbb{R}^3` for SE(3). A unit twist can be a: - unit rotational twist where :math:`|| \omega || = 1`, or - unit translational twist where :math:`|| \omega || = 0` and :math:`|| v || = 1`. .. runblock:: pycon >>> from spatialmath.base import * >>> isunittwist2([1, 2, 1]) >>> isunittwist2([0, 0, 2]) :seealso: unit, isunitvec """ v = getvector(v) if len(v) == 3: # test for SE(2) twist return isunitvec(v[2], tol=tol) or ( iszero(v[2], tol=tol) and isunitvec(v[0:2], tol=tol) ) else: raise ValueError
[docs]def unittwist(S: ArrayLike6, tol: float = 20) -> Union[R6, None]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(6) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist :rtype: ndarray(6) A unit twist is a twist where: - the rotation part has unit magnitude - if the rotational part is zero, then the translational part has unit magnitude .. runblock:: pycon >>> from spatialmath.base import * >>> unittwist([2, 4, 6, 2, 0, 0]) >>> unittwist([2, 0, 0, 0, 0, 0]) Returns None if the twist has zero magnitude """ S = getvector(S, 6) if iszerovec(S, tol=tol): return None v = S[0:3] w = S[3:6] if iszerovec(w, tol=tol): th = norm(v) else: th = norm(w) return S / th
[docs]def unittwist_norm( S: Union[R6, ArrayLike6], tol: float = 20 ) -> Tuple[Union[R6, None], Union[float, None]]: """ Convert twist to unit twist and norm :param S: twist vector :type S: array_like(6) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist and scalar motion :rtype: tuple (ndarray(6), float) A unit twist is a twist where: - the rotation part has unit magnitude - if the rotational part is zero, then the translational part has unit magnitude .. runblock:: pycon >>> from spatialmath.base import * >>> S, n = unittwist_norm([1, 2, 3, 1, 0, 0]) >>> print(S, n) >>> S, n = unittwist_norm([0, 0, 0, 2, 0, 0]) >>> print(S, n) >>> S, n = unittwist_norm([0, 0, 0, 0, 0, 0]) >>> print(S, n) .. note:: Returns (None,None) if the twist has zero magnitude """ S = getvector(S, 6) if iszerovec(S, tol=tol): return (None, None) # according to "note" in docstring. v = S[0:3] w = S[3:6] if iszerovec(w, tol=tol): th = norm(v) else: th = norm(w) return (S / th, th)
[docs]def unittwist2(S: ArrayLike3, tol: float = 20) -> Union[R3, None]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(3) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist :rtype: ndarray(3) A unit twist is a twist where: - the rotation part has unit magnitude - if the rotational part is zero, then the translational part has unit magnitude .. runblock:: pycon >>> from spatialmath.base import * >>> unittwist2([2, 4, 2) >>> unittwist2([2, 0, 0]) .. note:: Returns None if the twist has zero magnitude """ S = getvector(S, 3) if iszerovec(S, tol=tol): return None v = S[0:2] w = S[2] if iszero(w, tol=tol): th = norm(v) else: th = abs(w) return S / th
[docs]def unittwist2_norm( S: ArrayLike3, tol: float = 20 ) -> Tuple[Union[R3, None], Union[float, None]]: """ Convert twist to unit twist :param S: twist vector :type S: array_like(3) :param tol: tolerance in units of eps, defaults to 20 :type tol: float :return: unit twist and scalar motion :rtype: tuple (ndarray(3), float) A unit twist is a twist where: - the rotation part has unit magnitude - if the rotational part is zero, then the translational part has unit magnitude .. runblock:: pycon >>> from spatialmath.base import * >>> unittwist2([2, 4, 2) >>> unittwist2([2, 0, 0]) .. note:: Returns (None, None) if the twist has zero magnitude """ S = getvector(S, 3) if iszerovec(S, tol=tol): return (None, None) v = S[0:2] w = S[2] if iszero(w, tol=tol): th = norm(v) else: th = abs(w) return (S / th, th)
[docs]def wrap_0_pi(theta: ArrayLike) -> Union[float, NDArray]: r""" Wrap angle to range :math:`[0, \pi]` :param theta: input angle :type theta: scalar or ndarray :return: angle wrapped into range :math:`[0, \pi)` This is used to fold angles of colatitude. If zero is the angle of the north pole, colatitude increases to :math:`\pi` at the south pole then decreases to :math:`0` as we head back to the north pole. :seealso: :func:`wrap_mpi2_pi2` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`angle_wrap` """ theta = np.abs(theta) n = theta / np.pi if isinstance(n, np.ndarray): n = n.astype(int) else: n = np.fix(n).astype(int) y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, (n + 1) * np.pi - theta) if isinstance(y, np.ndarray) and y.size == 1: return float(y) else: return y
[docs]def wrap_mpi2_pi2(theta: ArrayLike) -> Union[float, NDArray]: r""" Wrap angle to range :math:`[-\pi/2, \pi/2]` :param theta: input angle :type theta: scalar or ndarray :return: angle wrapped into range :math:`[-\pi/2, \pi/2]` This is used to fold angles of latitude. :seealso: :func:`wrap_0_pi` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`angle_wrap` """ theta = getvector(theta) n = theta / np.pi * 2 if isinstance(n, np.ndarray): n = n.astype(int) else: n = np.fix(n).astype(int) y = np.where(np.bitwise_and(n, 1) == 0, theta - n * np.pi, n * np.pi - theta) if isinstance(y, np.ndarray) and len(y) == 1: return float(y) else: return y
[docs]def wrap_0_2pi(theta: ArrayLike) -> Union[float, NDArray]: r""" Wrap angle to range :math:`[0, 2\pi)` :param theta: input angle :type theta: scalar or ndarray :return: angle wrapped into range :math:`[0, 2\pi)` :seealso: :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` :func:`angle_wrap` """ theta = getvector(theta) y = theta - 2.0 * math.pi * np.floor(theta / 2.0 / np.pi) if isinstance(y, np.ndarray) and len(y) == 1: return float(y) else: return y
[docs]def wrap_mpi_pi(theta: ArrayLike) -> Union[float, NDArray]: r""" Wrap angle to range :math:`[-\pi, \pi)` :param theta: input angle :type theta: scalar or ndarray :return: angle wrapped into range :math:`[-\pi, \pi)` :seealso: :func:`wrap_0_2pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` :func:`angle_wrap` """ theta = getvector(theta) y = np.mod(theta + math.pi, 2 * math.pi) - np.pi if isinstance(y, np.ndarray) and len(y) == 1: return float(y) else: return y
# @overload # def angdiff(a:ArrayLike): # ... @overload def angdiff(a: ArrayLike, b: ArrayLike) -> NDArray: ... @overload def angdiff(a: ArrayLike) -> NDArray: ...
[docs]def angdiff(a, b=None): r""" Angular difference :param a: angle in radians :type a: scalar or array_like :param b: angle in radians :type b: scalar or array_like :return: angular difference a-b :rtype: scalar or array_like - ``angdiff(a, b)`` is the difference ``a - b`` wrapped to the range :math:`[-\pi, \pi)`. This is the operator :math:`a \circleddash b` used in the RVC book - If ``a`` and ``b`` are both scalars, the result is scalar - If ``a`` is array_like, the result is a NumPy array ``a[i]-b`` - If ``a`` is array_like, the result is a NumPy array ``a-b[i]`` - If ``a`` and ``b`` are both vectors of the same length, the result is a NumPy array ``a[i]-b[i]`` - ``angdiff(a)`` is the angle or vector of angles ``a`` wrapped to the range :math:`[-\pi, \pi)`. - If ``a`` is a scalar, the result is scalar - If ``a`` is array_like, the result is a NumPy array .. runblock:: pycon >>> from spatialmath.base import * >>> from math import pi >>> angdiff(0, 2 * pi) >>> angdiff(0.9 * pi, -0.9 * pi) / pi >>> angdiff(3 * pi) :seealso: :func:`vector_diff` :func:`wrap_mpi_pi` """ a = getvector(a) if b is not None: b = getvector(b) a = a - b # cannot use -= here, numpy wont broadcast y = np.mod(a + math.pi, 2 * math.pi) - math.pi if isinstance(y, np.ndarray) and len(y) == 1: return float(y) else: return y
[docs]def angle_std(theta: ArrayLike) -> float: r""" Standard deviation of angular values :param theta: angular values :type theta: array_like :return: circular standard deviation :rtype: float .. math:: \sigma_{\theta} = \sqrt{-2 \log \| \left[ \frac{\sum \sin \theta_i}{N}, \frac{\sum \sin \theta_i}{N} \right] \|} \in [0, \infty) :seealso: :func:`angle_mean` """ X = np.cos(theta).mean() Y = np.sin(theta).mean() R = np.sqrt(X**2 + Y**2) return np.sqrt(-2 * np.log(R))
[docs]def angle_mean(theta: ArrayLike) -> float: r""" Mean of angular values :param theta: angular values :type v: array_like :return: circular mean :rtype: float The circular mean is given by .. math:: \bar{\theta} = \tan^{-1} \frac{\sum \sin \theta_i}{\sum \cos \theta_i} \in [-\pi, \pi)] :seealso: :func:`angle_std` """ X = np.cos(theta).sum() Y = np.sin(theta).sum() return np.arctan2(Y, X)
[docs]def angle_wrap(theta: ArrayLike, mode: str = "-pi:pi") -> Union[float, NDArray]: """ Generalized angle-wrapping :param v: angles to wrap :type v: array_like :param mode: wrapping mode, one of: ``"0:2pi"``, ``"0:pi"``, ``"-pi/2:pi/2"`` or ``"-pi:pi"`` [default] :type mode: str, optional :return: wrapped angles :rtype: ndarray .. note:: The modes ``"0:pi"`` and ``"-pi/2:pi/2"`` are used to wrap angles of colatitude and latitude respectively. :seealso: :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` """ if mode == "0:2pi": return wrap_0_2pi(theta) elif mode == "-pi:pi": return wrap_mpi_pi(theta) elif mode == "0:pi": return wrap_0_pi(theta) elif mode == "-pi/2:pi/2": return wrap_mpi2_pi2(theta) else: raise ValueError("bad method specified")
[docs]def vector_diff(v1: ArrayLike, v2: ArrayLike, mode: str) -> NDArray: """ Generalized vector differnce :param v1: first vector :type v1: array_like(n) :param v2: second vector :type v2: array_like(n) :param mode: subtraction mode :type mode: str of length n ============== ==================================== mode character purpose ============== ==================================== r real number, don't wrap c angle on circle, wrap to [-π, π) C angle on circle, wrap to [0, 2π) l latitude angle, wrap to [-π/2, π/2] L colatitude angle, wrap to [0, π] ============== ==================================== :seealso: :func:`angdiff` :func:`wrap_0_2pi` :func:`wrap_mpi_pi` :func:`wrap_0_pi` :func:`wrap_mpi2_pi2` """ v = getvector(v1) - getvector(v2) for i, m in enumerate(mode): if m == "r": pass elif m == "c": v[i] = wrap_mpi_pi(v[i]) elif m == "C": v[i] = wrap_0_2pi(v[i]) elif m == "l": v[i] = wrap_mpi2_pi2(v[i]) elif m == "L": v[i] = wrap_0_pi(v[i]) else: raise ValueError("bad mode character") return v
[docs]def removesmall(v: ArrayLike, tol: float = 20) -> NDArray: """ Set small values to zero :param v: any vector :type v: array_like(n) or ndarray(n,m) :param tol: Tolerance in units of eps, defaults to 20 :type tol: int, optional :return: vector with small values set to zero :rtype: ndarray(n) or ndarray(n,m) Values with absolute value less than ``tol`` will be set to zero. .. runblock:: pycon >>> from spatialmath.base import * >>> a = np.r_[1, 2, 3, 1e-16] >>> print(a) >>> a = removesmall(a) >>> print(a) >>> print(a[3]) """ return np.where(np.abs(v) < tol * _eps, 0, v)
[docs]def project(v1: ArrayLike3, v2: ArrayLike3) -> ArrayLike3: """ Projects vector v1 onto v2. Returns a vector parallel to v2. :param v1: vector to be projected :type v1: array_like(n) :param v2: vector to be projected onto :type v2: array_like(n) :return: vector projection of v1 onto v2 (parrallel to v2) :rtype: ndarray(n) """ return np.dot(v1, v2) * v2
[docs]def orthogonalize(v1: ArrayLike3, v2: ArrayLike3, normalize: bool = True) -> ArrayLike3: """ Orthoginalizes vector v1 with respect to v2 with minimum rotation. Returns a the nearest vector to v1 that is orthoginal to v2. :param v1: vector to be orthoginalized :type v1: array_like(n) :param v2: vector that returned vector will be orthoginal to :type v2: array_like(n) :param normalize: whether to normalize the output vector :type normalize: bool :return: nearest vector to v1 that is orthoginal to v2 :rtype: ndarray(n) """ v_orth = v1 - project(v1, v2) if normalize: v_orth = v_orth / np.linalg.norm(v_orth) return v_orth
if __name__ == "__main__": # pragma: no cover import pathlib exec( open( pathlib.Path(__file__).parent.parent.parent.absolute() / "tests" / "base" / "test_vectors.py" ).read() ) # pylint: disable=exec-used