Source code for spatialmath.base.vectors

"""
This modules contains functions to create and transform rotation matrices
and homogeneous tranformation matrices.

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.

"""

# 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.base import argcheck


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


[docs]def colvec(v): return np.array(v).reshape((len(v), 1))
# ---------------------------------------------------------------------------------------#
[docs]def unitvec(v): """ Create a unit vector :param v: n-dimensional vector :type v: array_like :return: a unit-vector parallel to V. :rtype: numpy.ndarray :raises ValueError: for zero length vector ``unitvec(v)`` is a vector parallel to `v` of unit length. :seealso: norm """ v = argcheck.getvector(v) n = np.linalg.norm(v) if n > 100 * _eps: # if greater than eps return v / n else: return None
[docs]def norm(v): """ Norm of vector :param v: n-vector as a list, dict, or a numpy array, row or column vector :return: norm of vector :rtype: float ``norm(v)`` is the 2-norm (length or magnitude) of the vector ``v``. :seealso: unit """ return np.linalg.norm(v)
[docs]def isunitvec(v, tol=10): """ Test if vector has unit length :param v: vector to test :type v: numpy.ndarray :param tol: tolerance in units of eps :type tol: float :return: whether vector has unit length :rtype: bool :seealso: unit, isunittwist """ return abs(np.linalg.norm(v) - 1) < tol * _eps
[docs]def iszerovec(v, tol=10): """ Test if vector has zero length :param v: vector to test :type v: numpy.ndarray :param tol: tolerance in units of eps :type tol: float :return: whether vector has zero length :rtype: bool :seealso: unit, isunittwist """ return np.linalg.norm(v) < tol * _eps
[docs]def isunittwist(v, tol=10): r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: vector to test :type v: array_like :param tol: tolerance in units of eps :type tol: float :return: whether vector has unit length :rtype: bool 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`. :seealso: unit, isunitvec """ v = argcheck.getvector(v) if len(v) == 6: # test for SE(3) twist return isunitvec(v[3:6], tol=tol) or (np.linalg.norm(v[3:6]) < tol * _eps and isunitvec(v[0:3], tol=tol)) elif len(v) == 3: return isunitvec(v[2], tol=tol) or (abs(v[2]) < tol * _eps and isunitvec(v[0:2], tol=tol)) else: raise ValueError
[docs]def isunittwist2(v, tol=10): r""" Test if vector represents a unit twist in SE(2) or SE(3) :param v: vector to test :type v: array_like :param tol: tolerance in units of eps :type tol: float :return: whether vector has unit length :rtype: bool 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`. :seealso: unit, isunitvec """ v = argcheck.getvector(v) if len(v) == 3: # test for SE(2) twist return isunitvec(v[2], tol=tol) or (np.abs(v[2]) < tol * _eps and isunitvec(v[0:2], tol=tol)) else: raise ValueError
[docs]def unittwist(S, tol=10): """ Convert twist to unit twist :param S: twist as a 6-vector :type S: array_like :param tol: tolerance in units of eps :type tol: float :return: unit twist and scalar motion :rtype: np.ndarray, shape=(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 Returns None if the twist has zero magnitude """ s = argcheck.getvector(S, 6) if iszerovec(s, tol=tol): return None v = S[0:3] w = S[3:6] if iszerovec(w): th = norm(v) else: th = norm(w) return S / th
[docs]def unittwist_norm(S, tol=10): """ Convert twist to unit twist and norm :param S: twist as a 6-vector :type S: array_like :param tol: tolerance in units of eps :type tol: float :return: unit twist and scalar motion :rtype: tuple (np.ndarray shape=(6,), theta) 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 Returns (None,None) if the twist has zero magnitude """ s = argcheck.getvector(S, 6) if iszerovec(s, tol=tol): return (None, None) v = S[0:3] w = S[3:6] if iszerovec(w): th = norm(v) else: th = norm(w) return (S / th, th)
[docs]def unittwist2(S): """ Convert twist to unit twist :param S: twist as a 3-vector :type S: array_like :return: unit twist and scalar motion :rtype: tuple (unit_twist, theta) 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 """ s = argcheck.getvector(S, 3) v = S[0:2] w = S[2] if iszerovec(w): th = norm(v) else: th = norm(w) return (S / th, th)
[docs]def angdiff(a, b): """ 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 - If ``a`` and ``b`` are both scalars, the result is scalar - If ``a`` is array_like, the result is a vector a[i]-b - If ``a`` is array_like, the result is a vector a-b[i] - If ``a`` and ``b`` are both vectors of the same length, the result is a vector a[i]-b[i] """ return np.mod(a - b + math.pi, 2 * math.pi) - math.pi
if __name__ == '__main__': # pragma: no cover import pathlib import os.path exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_transforms.py")).read())