Source code for spatialmath.base.transforms2d

"""
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
from spatialmath.base import vectors as vec
from spatialmath.base import transformsNd as trn
import scipy.linalg

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

    def issymbol(x):
        return isinstance(x, sym.Symbol)
except BaseException:
[docs] def issymbol(x): return False
_eps = np.finfo(np.float64).eps
[docs]def colvec(v): return np.array(v).reshape((len(v), 1))
# ---------------------------------------------------------------------------------------# def _cos(theta): if issymbol(theta): return sym.cos(theta) else: return math.cos(theta) def _sin(theta): if issymbol(theta): return sym.sin(theta) else: return math.sin(theta) # ---------------------------------------------------------------------------------------#
[docs]def rot2(theta, unit='rad'): """ Create SO(2) rotation :param theta: rotation angle :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: 2x2 rotation matrix :rtype: numpy.ndarray, shape=(2,2) - ``ROT2(THETA)`` is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians. - ``ROT2(THETA, 'deg')`` as above but THETA is in degrees. """ theta = argcheck.getunit(theta, unit) ct = _cos(theta) st = _sin(theta) R = np.array([ [ct, -st], [st, ct]]) if not isinstance(theta, sym.Symbol): R = R.round(15) return R
# ---------------------------------------------------------------------------------------#
[docs]def trot2(theta, unit='rad', t=None): """ Create SE(2) pure rotation :param theta: rotation angle about X-axis :type theta: float :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :param t: translation 2-vector, defaults to [0,0] :type t: array_like :return: 3x3 homogeneous transformation matrix :rtype: numpy.ndarray, shape=(3,3) - ``TROT2(THETA)`` is a homogeneous transformation (3x3) representing a rotation of THETA radians. - ``TROT2(THETA, 'deg')`` as above but THETA is in degrees. Notes: - Translational component is zero. """ T = np.pad(rot2(theta, unit), (0, 1), mode='constant') if t is not None: T[:2, 2] = argcheck.getvector(t, 2, 'array') T[2, 2] = 1.0 return T
# ---------------------------------------------------------------------------------------#
[docs]def transl2(x, y=None): """ Create SE(2) pure translation, or extract translation from SE(2) matrix :param x: translation along X-axis :type x: float :param y: translation along Y-axis :type y: float :return: homogeneous transform matrix or the translation elements of a homogeneous transform :rtype: numpy.ndarray, shape=(3,3) Create a translational SE(2) matrix: - ``T = transl2([X, Y])`` is an SE(2) homogeneous transform (3x3) representing a pure translation. - ``T = transl2( V )`` as above but the translation is given by a 2-element list, dict, or a numpy array, row or column vector. Extract the translational part of an SE(2) matrix: P = TRANSL2(T) is the translational part of a homogeneous transform as a 2-element numpy array. """ if np.isscalar(x): T = np.identity(3) T[:2, 2] = [x, y] return T elif argcheck.isvector(x, 2): T = np.identity(3) T[:2, 2] = argcheck.getvector(x, 2) return T elif argcheck.ismatrix(x, (3, 3)): return x[:2, 2] else: ValueError('bad argument')
[docs]def ishom2(T, check=False): """ Test if matrix belongs to SE(2) :param T: matrix to test :type T: numpy.ndarray :param check: check validity of rotation submatrix :type check: bool :return: whether matrix is an SE(2) homogeneous transformation matrix :rtype: bool - ``ISHOM2(T)`` is True if the argument ``T`` is of dimension 3x3 - ``ISHOM2(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and validitity of the bottom row. :seealso: isR, isrot2, ishom, isvec """ return isinstance(T, np.ndarray) and T.shape == (3, 3) and (not check or (trn.isR(T[:2, :2]) and np.all(T[2, :] == np.array([0, 0, 1]))))
[docs]def isrot2(R, check=False): """ Test if matrix belongs to SO(2) :param R: matrix to test :type R: numpy.ndarray :param check: check validity of rotation submatrix :type check: bool :return: whether matrix is an SO(2) rotation matrix :rtype: bool - ``ISROT(R)`` is True if the argument ``R`` is of dimension 2x2 - ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix. :seealso: isR, ishom2, isrot """ return isinstance(R, np.ndarray) and R.shape == (2, 2) and (not check or trn.isR(R))
# ---------------------------------------------------------------------------------------#
[docs]def trlog2(T, check=True): """ Logarithm of SO(2) or SE(2) matrix :param T: SO(2) or SE(2) matrix :type T: numpy.ndarray, shape=(2,2) or (3,3) :return: logarithm :rtype: numpy.ndarray, shape=(2,2) or (3,3) :raises: ValueError An efficient closed-form solution of the matrix logarithm for arguments that are SO(2) or SE(2). - ``trlog2(R)`` is the logarithm of the passed rotation matrix ``R`` which will be 2x2 skew-symmetric matrix. The equivalent vector from ``vex()`` is parallel to rotation axis and its norm is the amount of rotation about that axis. - ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be 3x3 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist vector (6x1) comprising [v w]. :seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa` """ if ishom2(T, check=check): # SE(2) matrix if trn.iseye(T): # is identity matrix return np.zeros((3,3)) else: return scipy.linalg.logm(T) elif isrot2(T, check=check): # SO(2) rotation matrix return scipy.linalg.logm(T) else: raise ValueError("Expect SO(2) or SE(2) matrix")
# ---------------------------------------------------------------------------------------#
[docs]def trexp2(S, theta=None): """ Exponential of so(2) or se(2) matrix :param S: so(2), se(2) matrix or equivalent velctor :type T: numpy.ndarray, shape=(2,2) or (3,3); array_like :param theta: motion :type theta: float :return: 2x2 or 3x3 matrix exponential in SO(2) or SE(2) :rtype: numpy.ndarray, shape=(2,2) or (3,3) An efficient closed-form solution of the matrix exponential for arguments that are so(2) or se(2). For so(2) the results is an SO(2) rotation matrix: - ``trexp2(S)`` is the matrix exponential of the so(3) element ``S`` which is a 2x2 skew-symmetric matrix. - ``trexp2(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given by ``THETA``. - ``trexp2(W)`` is the matrix exponential of the so(2) element ``W`` expressed as a 1-vector (array_like). - ``trexp2(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a unit-norm vector representing a rotation axis and a rotation magnitude given by ``THETA``. ``W`` is expressed as a 1-vector (array_like). For se(2) the results is an SE(2) homogeneous transformation matrix: - ``trexp2(SIGMA)`` is the matrix exponential of the se(2) element ``SIGMA`` which is a 3x3 augmented skew-symmetric matrix. - ``trexp2(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. - ``trexp2(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as a 3-vector which can be considered a screw motion. - ``trexp2(TW, THETA)`` as above but for an se(2) motion of TW*THETA, where ``TW`` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix. :seealso: trlog, trexp2 """ if argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3): # se(2) case if argcheck.ismatrix(S, (3, 3)): # augmentented skew matrix tw = trn.vexa(S) else: # 3 vector tw = argcheck.getvector(S) if vec.iszerovec(tw): return np.eye(3) if theta is None: (tw, theta) = vec.unittwist2(tw) else: assert vec.isunittwist2(tw), 'If theta is specified S must be a unit twist' t = tw[0:2] w = tw[2] R = trn._rodrigues(w, theta) skw = trn.skew(w) V = np.eye(2) * theta + (1.0 - math.cos(theta)) * skw + (theta - math.sin(theta)) * skw @ skw return trn.rt2tr(R, V@t) elif argcheck.ismatrix(S, (2, 2)) or argcheck.isvector(S, 1): # so(2) case if argcheck.ismatrix(S, (2, 2)): # skew symmetric matrix w = trn.vex(S) else: # 1 vector w = argcheck.getvector(S) if theta is not None: assert vec.isunitvec(w), 'If theta is specified S must be a unit twist' # do Rodrigues' formula for rotation return trn._rodrigues(w, theta) else: raise ValueError(" First argument must be SO(2), 1-vector, SE(2) or 3-vector")
[docs]def trinterp2(T0, T1=None, s=None): """ Interpolate SE(2) matrices :param T0: first SE(2) matrix :type T0: np.ndarray, shape=(3,3) :param T1: second SE(2) matrix :type T1: np.ndarray, shape=(3,3) :param s: interpolation coefficient, range 0 to 1 :type s: float :return: SE(2) matrix :rtype: np.ndarray, shape=(3,3) - ``trinterp2(T0, T1, S)`` is a homogeneous transform (3x3) interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (3x3). - ``trinterp2(T1, S)`` as above but interpolated between the identity matrix when S=0 to T1 when S=1. Notes: - Rotation angle is linearly interpolated. :seealso: :func:`~spatialmath.base.transforms3d.trinterp` %## 2d homogeneous trajectory """ if argcheck.ismatrix(T0, (2,2)): # SO(2) case if T1 is None: # TRINTERP2(T, s) th0 = math.atan2(T0[1,0], T0[0,0]) th = s * th0 else: # TRINTERP2(T0, T1, s) assert T0.shape == T1.shape, 'both matrices must be same shape' th0 = math.atan2(T0[1,0], T0[0,0]) th1 = math.atan2(T1[1,0], T1[0,0]) th = th0 * (1 - s) + s * th1 return rot2(th) elif argcheck.ismatrix(T0, (3,3)): if T1 is None: # TRINTERP2(T, s) th0 = math.atan2(T0[1,0], T0[0,0]) p0 = transl2(T0) th = s * th0 pr = s * p0 else: # TRINTERP2(T0, T1, s) assert T0.shape == T1.shape, 'both matrices must be same shape' th0 = math.atan2(T0[1,0], T0[0,0]) th1 = math.atan2(T1[1,0], T1[0,0]) p0 = transl2(T0) p1 = transl2(T1) pr = p0 * (1 - s) + s * p1; th = th0 * (1 - s) + s * th1 return trn.rt2tr(rot2(th), pr) else: return ValueError('Argument must be SO(2) or SE(2)')
[docs]def trprint2(T, label=None, file=sys.stdout, fmt='{:8.2g}', unit='deg'): """ Compact display of SO(2) or SE(2) matrices :param T: matrix to format :type T: numpy.ndarray, shape=(2,2) or (3,3) :param label: text label to put at start of line :type label: str :param file: file to write formatted string to :type file: str :param fmt: conversion format for each number :type fmt: str :param unit: angular units: 'rad' [default], or 'deg' :type unit: str :return: optional formatted string :rtype: str The matrix is formatted and written to ``file`` or if ``file=None`` then the string is returned. - ``trprint2(R)`` displays the SO(2) rotation matrix in a compact single-line format:: [LABEL:] THETA UNIT - ``trprint2(T)`` displays the SE(2) homogoneous transform in a compact single-line format:: [LABEL:] [t=X, Y;] THETA UNIT Example:: >>> T = transl2(1,2)@trot2(0.3) >>> trprint2(a, file=None, label='T') 'T: t = 1, 2; 17 deg' :seealso: trprint """ s = '' if label is not None: s += '{:s}: '.format(label) # print the translational part if it exists s += 't = {};'.format(_vec2s(fmt, transl2(T))) angle = math.atan2(T[1, 0], T[0, 0]) if unit == 'deg': angle *= 180.0 / math.pi s += ' {} {}'.format(_vec2s(fmt, [angle]), unit) if file: print(s, file=file) else: return s
def _vec2s(fmt, v): v = [x if np.abs(x) > 100 * _eps else 0.0 for x in v] return ', '.join([fmt.format(x) for x in v]) try: import matplotlib.pyplot as plt from mpl_toolkits.mplot3d import Axes3D _matplotlib_exists = True except BaseException: # pragma: no cover def trplot(*args, **kwargs): print('** trplot: no plot produced -- matplotlib not installed') _matplotlib_exists = False if _matplotlib_exists:
[docs] def trplot2(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y'], length=1, arrow=True, rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs): """ Plot a 2D coordinate frame :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame :type: numpy.ndarray, shape=(2,2) or (3,3) :param axes: the axes to plot into, defaults to current axes :type axes: Axes3D reference :param dims: dimension of plot volume as [xmin, xmax, ymin, ymax] :type dims: array_like :param color: color of the lines defining the frame :type color: str :param textcolor: color of text labels for the frame, default color of lines above :type textcolor: str :param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels :type frame: str :param labels: labels for the axes, defaults to X, Y and Z :type labels: 3-tuple of strings :param length: length of coordinate frame axes, default 1 :type length: float :param arrow: show arrow heads, default True :type arrow: bool :param wtl: width-to-length ratio for arrows, default 0.2 :type wtl: float :param rviz: show Rviz style arrows, default False :type rviz: bool :param projection: 3D projection: ortho [default] or persp :type projection: str :param width: width of lines, default 1 :type width: float :param d1: distance of frame axis label text from origin, default 1.15 :type d2: distance of frame label text from origin, default 0.05 Adds a 2D coordinate frame represented by the SO(2) or SE(2) matrix to the current axes. - If no current figure, one is created - If current figure, but no axes, a 3d Axes is created Examples: trplot2(T, frame='A') trplot2(T, frame='A', color='green') trplot2(T1, 'labels', 'AB'); """ # TODO # animation # style='line', 'arrow', 'rviz' # check input types if isrot2(T, check=True): T = trn.r2t(T) else: assert ishom2(T, check=True) if axes is None: # create an axes fig = plt.gcf() if fig.axes == []: # no axes in the figure, create a 3D axes ax = plt.gca() if dims is None: ax.autoscale(enable=True, axis='both') else: if len(dims) == 2: dims = dims * 2 ax.set_xlim(dims[0:2]) ax.set_ylim(dims[2:4]) ax.set_aspect('equal') ax.set_xlabel(labels[0]) ax.set_ylabel(labels[1]) else: # reuse an existing axis ax = plt.gca() else: ax = axes # create unit vectors in homogeneous form o = T @ np.array([0, 0, 1]) x = T @ np.array([1, 0, 1]) * length y = T @ np.array([0, 1, 1]) * length # draw the axes if rviz: ax.plot([o[0], x[0]], [o[1], x[1]], color='red', linewidth=5 * width) ax.plot([o[0], y[0]], [o[1], y[1]], color='lime', linewidth=5 * width) elif arrow: ax.quiver(o[0], o[1], x[0] - o[0], x[1] - o[1], angles='xy', scale_units='xy', scale=1, linewidth=width, facecolor=color, edgecolor=color) ax.quiver(o[0], o[1], y[0] - o[0], y[1] - o[1], angles='xy', scale_units='xy', scale=1, linewidth=width, facecolor=color, edgecolor=color) # plot an invisible point at the end of each arrow to allow auto-scaling to work ax.scatter(x=[o[0], x[0], y[0]], y=[o[1], x[1], y[1]], s=[20, 0, 0]) else: ax.plot([o[0], x[0]], [o[1], x[1]], color=color, linewidth=width) ax.plot([o[0], y[0]], [o[1], y[1]], color=color, linewidth=width) # label the frame if frame: if textcolor is not None: color = textcolor o1 = T @ np.array([-d1, -d1, 1]) ax.text(o1[0], o1[1], r'$\{' + frame + r'\}$', color=color, verticalalignment='top', horizontalalignment='center') # add the labels to each axis x = (x - o) * d2 + o y = (y - o) * d2 + o ax.text(x[0], x[1], "$%c_{%s}$" % (labels[0], frame), color=color, horizontalalignment='center', verticalalignment='center') ax.text(y[0], y[1], "$%c_{%s}$" % (labels[1], frame), color=color, horizontalalignment='center', verticalalignment='center')
from spatialmath.base import animate as animate
[docs] def tranimate2(T, **kwargs): """ Animate a 2D coordinate frame :param T: an SO(2) or SE(2) pose to be displayed as coordinate frame :type: numpy.ndarray, shape=(2,2) or (3,3) :param nframes: number of steps in the animation [defaault 100] :type nframes: int :param repeat: animate in endless loop [default False] :type repeat: bool :param interval: number of milliseconds between frames [default 50] :type interval: int :param movie: name of file to write MP4 movie into :type movie: str Animates a 2D coordinate frame moving from the world frame to a frame represented by the SO(2) or SE(2) matrix to the current axes. - If no current figure, one is created - If current figure, but no axes, a 3d Axes is created Examples: tranimate2(transl(1,2)@trot2(1), frame='A', arrow=False, dims=[0, 5]) tranimate2(transl(1,2)@trot2(1), frame='A', arrow=False, dims=[0, 5], movie='spin.mp4') """ anim = animate.Animate2(**kwargs) anim.trplot2(T, **kwargs) anim.run(**kwargs)
if __name__ == '__main__': # pragma: no cover import pathlib import os.path # trplot2( transl2(1,2), frame='A', rviz=True, width=1) # trplot2( transl2(3,1), color='red', arrow=True, width=3, frame='B') # trplot2( transl2(4, 3)@trot2(math.pi/3), color='green', frame='c') # plt.grid(True) exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_transforms.py")).read())