# Created by: Aditya Dua, 2017
# Peter Corke, 2020
# 13 June, 2017
import numpy as np
import sympy
from collections import UserList
import copy
from spatialmath.base import argcheck
import spatialmath.base as tr
_eps = np.finfo(np.float64).eps
# colored printing of matrices to the terminal
# colored package has much finer control than colorama, but the latter is available by default with anaconda
try:
from colored import fg, bg, attr
_color = True
print('using colored output')
except:
#print('colored not found')
_color = False
# try:
# import colorama
# colorama.init()
# print('using colored output')
# from colorama import Fore, Back, Style
# except:
# class color:
# def __init__(self):
# self.RED = ''
# self.BLUE = ''
# self.BLACK = ''
# self.DIM = ''
# print(Fore.RED + '1.00 2.00 ' + Fore.BLUE + '3.00')
# print(Fore.RED + '1.00 2.00 ' + Fore.BLUE + '3.00')
# print(Fore.BLACK + Style.DIM + '0 0 1')
class SMPose(UserList):
"""
Superclass for SO(N) and SE(N) objects
Subclasses are:
- ``SO2`` representing elements of SO(2) which describe rotations in 2D
- ``SE2`` representing elements of SE(2) which describe rigid-body motion in 2D
- ``SO3`` representing elements of SO(3) which describe rotations in 3D
- ``SE3`` representing elements of SE(3) which describe rigid-body motion in 3D
Arithmetic operators are overloaded but the operation they perform depend
on the types of the operands. For example:
- ``*`` will compose two instances of the same subclass, and the result will be
an instance of the same subclass, since this is a group operator.
- ``+`` will add two instances of the same subclass, and the result will be
a matrix, not an instance of the same subclass, since addition is not a group operator.
These classes all inherit from ``UserList`` which enables them to
represent a sequence of values, ie. an ``SE3`` instance can contain
a sequence of SE(3) values. Most of the Python ``list`` operators
are applicable::
>>> x = SE3() # new instance with identity matrix value
>>> len(x) # it is a sequence of one value
1
>>> x.append(x) # append to itself
>>> len(x) # it is a sequence of two values
2
>>> x[1] # the element has a 4x4 matrix value
SE3([
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) ])
>>> x[1] = SE3.Rx(0.3) # set an elements of the sequence
>>> x.reverse() # reverse the elements in the sequence
>>> del x[1] # delete an element
"""
def __new__(cls, *args, **kwargs):
"""
Create the subclass instance (superclass method)
Create a new instance and call the superclass initializer to enable the
``UserList`` capabilities.
"""
pose = super(SMPose, cls).__new__(cls) # create a new instance
super().__init__(pose) # initialize UserList
return pose
def _arghandler(self, arg, check=True):
"""
Assign value to pose subclasses (superclass method)
:param self: the pose object to be set
:type self: SO2, SE2, SO3, SE3 instance
:param arg: value of pose
:param check: check type of argument, defaults to True
:type check: TYPE, optional
:raises ValueError: bad type passed
The value ``arg`` can be any of:
# a numpy.ndarray of the appropriate shape and value which is valid for the subclass
# a list whose elements all meet the criteria above
# an instance of the subclass
# a list whose elements are all instances of the subclass
Examples::
SE3( np.identity(4))
SE3( [np.identity(4), np.identity(4)])
SE3( SE3() )
SE3( [SE3(), SE3()])
"""
if isinstance(arg, np.ndarray):
# it's a numpy array
assert arg.shape == self.shape, 'array must have valid shape for the class'
assert type(self).isvalid(arg), 'array must have valid value for the class'
self.data.append(arg)
elif isinstance(arg, list):
# construct from a list
if isinstance(arg[0], np.ndarray):
#print('list of numpys')
# possibly a list of numpy arrays
s = self.shape
if check:
checkfunc = type(self).isvalid # lambda function
assert all(map(lambda x: x.shape == s and checkfunc(x), arg)), 'all elements of list must have valid shape and value for the class'
else:
assert all(map(lambda x: x.shape == s, arg))
self.data = arg
elif type(arg[0]) == type(self):
# possibly a list of objects of same type
assert all(map(lambda x: type(x) == type(self), arg)), 'all elements of list must have same type'
self.data = [x.A for x in arg]
else:
raise ValueError('bad list argument to constructor')
elif type(self) == type(arg):
# it's an object of same type, do copy
self.data = arg.data.copy()
else:
raise ValueError('bad argument to constructor')
@classmethod
def Empty(cls):
"""
Construct a new pose object with zero items (superclass method)
:param cls: The pose subclass
:type cls: SO2, SE2, SO3, SE3
:return: a pose with zero values
:rtype: SO2, SE2, SO3, SE3 instance
This constructs an empty pose container which can be appended to. For example::
>>> x = SO2.Empty()
>>> len(x)
0
>>> x.append(SO2(20, 'deg'))
>>> len(x)
1
"""
X = cls()
X.data = []
return X
# ------------------------------------------------------------------------ #
@property
def A(self):
"""
Interal array representation (superclass property)
:param self: the pose object
:type self: SO2, SE2, SO3, SE3 instance
:return: The numeric array
:rtype: numpy.ndarray
Each pose subclass SO(N) or SE(N) are stored internally as a numpy array. This property returns
the array, shape depends on the particular subclass.
Examples::
>>> x = SE3()
>>> x.A
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]])
:seealso: `shape`, `N`
"""
# get the underlying numpy array
if len(self.data) == 1:
return self.data[0]
else:
return self.data
@property
def shape(self):
"""
Shape of the object's matrix representation (superclass property)
:return: matrix shape
:rtype: 2-tuple of ints
(2,2) for ``SO2``, (3,3) for ``SE2`` and ``SO3``, and (4,4) for ``SE3``.
Example::
>>> x = SE3()
>>> x.shape
(4, 4)
"""
if type(self).__name__ == 'SO2':
return (2, 2)
elif type(self).__name__ == 'SO3':
return (3, 3)
elif type(self).__name__ == 'SE2':
return (3, 3)
elif type(self).__name__ == 'SE3':
return (4, 4)
@property
def about(self):
"""
Succinct summary of object type and length (superclass property)
:return: succinct summary
:rtype: str
Displays the type and the number of elements in compact form, for
example::
>>> x = SE3([SE3() for i in range(20)])
>>> len(x)
20
>>> print(x.about)
SE3[20]
"""
return "{:s}[{:d}]".format(type(self).__name__, len(self))
@property
def N(self):
"""
Dimension of the object's group (superclass property)
:return: dimension
:rtype: int
Dimension of the group is 2 for ``SO2`` or ``SE2``, and 3 for ``SO3`` or ``SE3``.
This corresponds to the dimension of the space, 2D or 3D, to which these
rotations or rigid-body motions apply.
Example::
>>> x = SE3()
>>> x.N
3
"""
if type(self).__name__ == 'SO2' or type(self).__name__ == 'SE2':
return 2
else:
return 3
#----------------------- tests
@property
def isSO(self):
"""
Test if object belongs to SO(n) group (superclass property)
:param self: object to test
:type self: SO2, SE2, SO3, SE3 instance
:return: ``True`` if object is instance of SO2 or SO3
:rtype: bool
"""
return type(self).__name__ == 'SO2' or type(self).__name__ == 'SO3'
@property
def isSE(self):
"""
Test if object belongs to SE(n) group (superclass property)
:param self: object to test
:type self: SO2, SE2, SO3, SE3 instance
:return: ``True`` if object is instance of SE2 or SE3
:rtype: bool
"""
return type(self).__name__ == 'SE2' or type(self).__name__ == 'SE3'
# ------------------------------------------------------------------------ #
def __getitem__(self, i):
"""
Access value of a pose object (superclass method)
:param i: index of element to return
:type i: int
:return: the specific element of the pose
:rtype: SO2, SE2, SO3, SE3 instance
:raises IndexError: if the element is out of bounds
Note that only a single index is supported, slices are not.
Example::
>>> x = SE3.Rx([0, math.pi/2, math.pi])
>>> len(x)
3
>>> x[1]
1 0 0 0
0 0 -1 0
0 1 0 0
0 0 0 1
"""
if isinstance(i, slice):
return self.__class__([self.data[k] for k in range(i.start or 0, i.stop or len(self), i.step or 1)])
else:
return self.__class__(self.data[i])
def __setitem__(self, i, value):
"""
Assign a value to a pose object (superclass method)
:param i: index of element to assign to
:type i: int
:param value: the value to insert
:type value: SO2, SE2, SO3, SE3 instance
:raises ValueError: incorrect type of assigned value
Assign the argument to an element of the object's internal list of values.
This supports the assignement operator, for example::
>>> x = SE3([SE3() for i in range(10)]) # sequence of ten identity values
>>> len(x)
10
>>> x[3] = SE3.Rx(0.2) # assign to position 3 in the list
"""
if not type(self) == type(value):
raise ValueError("cant append different type of pose object")
if len(value) > 1:
raise ValueError("cant insert a pose sequence - must have len() == 1")
self.data[i] = value.A
def append(self, x):
"""
Append a value to a pose object (superclass method)
:param x: the value to append
:type x: SO2, SE2, SO3, SE3 instance
:raises ValueError: incorrect type of appended object
Appends the argument to the object's internal list of values.
Examples::
>>> x = SE3()
>>> len(x)
1
>>> x.append(SE3.Rx(0.1))
>>> len(x)
2
"""
#print('in append method')
if not type(self) == type(x):
raise ValueError("cant append different type of pose object")
if len(x) > 1:
raise ValueError("cant append a pose sequence - use extend")
super().append(x.A)
def extend(self, x):
"""
Extend sequence of values of a pose object (superclass method)
:param x: the value to extend
:type x: SO2, SE2, SO3, SE3 instance
:raises ValueError: incorrect type of appended object
Appends the argument to the object's internal list of values.
Examples::
>>> x = SE3()
>>> len(x)
1
>>> x.append(SE3.Rx(0.1))
>>> len(x)
2
"""
#print('in extend method')
if not type(self) == type(x):
raise ValueError("cant append different type of pose object")
if len(x) == 0:
raise ValueError("cant extend a singleton pose - use append")
super().extend(x.A)
def insert(self, i, value):
"""
Insert a value to a pose object (superclass method)
:param i: element to insert value before
:type i: int
:param value: the value to insert
:type value: SO2, SE2, SO3, SE3 instance
:raises ValueError: incorrect type of inserted value
Inserts the argument into the object's internal list of values.
Examples::
>>> x = SE3()
>>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list
>>> len(x)
2
"""
if not type(self) == type(value):
raise ValueError("cant append different type of pose object")
if len(value) > 1:
raise ValueError("cant insert a pose sequence - must have len() == 1")
super().insert(i, value.A)
def pop(self):
"""
Pop value of a pose object (superclass method)
:return: the specific element of the pose
:rtype: SO2, SE2, SO3, SE3 instance
:raises IndexError: if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example::
>>> x = SE3.Rx([0, math.pi/2, math.pi])
>>> len(x)
3
>>> y = x.pop()
>>> y
SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00],
[ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]]))
>>> len(x)
2
"""
return self.__class__(super().pop())
# ------------------------------------------------------------------------ #
# --------- compatibility methods
def isrot(self):
"""
Test if object belongs to SO(3) group (superclass method)
:return: ``True`` if object is instance of SO3
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SO3)``.
Example::
>>> x = SO3()
>>> x.isrot()
True
>>> x = SE3()
>>> x.isrot()
False
"""
return type(self).__name__ == 'SO3'
def isrot2(self):
"""
Test if object belongs to SO(2) group (superclass method)
:return: ``True`` if object is instance of SO2
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SO2)``.
Example::
>>> x = SO2()
>>> x.isrot()
True
>>> x = SE2()
>>> x.isrot()
False
"""
return type(self).__name__ == 'SO2'
def ishom(self):
"""
Test if object belongs to SE(3) group (superclass method)
:return: ``True`` if object is instance of SE3
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SE3)``.
Example::
>>> x = SO3()
>>> x.isrot()
False
>>> x = SE3()
>>> x.isrot()
True
"""
return type(self).__name__ == 'SE3'
def ishom2(self):
"""
Test if object belongs to SE(2) group (superclass method)
:return: ``True`` if object is instance of SE2
:rtype: bool
For compatibility with Spatial Math Toolbox for MATLAB.
In Python use ``isinstance(x, SE2)``.
Example::
>>> x = SO2()
>>> x.isrot()
False
>>> x = SE2()
>>> x.isrot()
True
"""
return type(self).__name__ == 'SE2'
#----------------------- functions
def log(self):
"""
Logarithm of pose (superclass method)
:return: logarithm
:rtype: numpy.ndarray
:raises: ValueError
An efficient closed-form solution of the matrix logarithm.
===== ====== ===============================
Input Output
----- ---------------------------------------
Pose Shape Structure
===== ====== ===============================
SO2 (2,2) skew-symmetric
SE2 (3,3) augmented skew-symmetric
SO3 (3,3) skew-symmetric
SE3 (4,4) augmented skew-symmetric
===== ====== ===============================
Example::
>>> x = SE3.Rx(0.3)
>>> y = x.log()
>>> y
array([[ 0. , -0. , 0. , 0. ],
[ 0. , 0. , -0.3, 0. ],
[-0. , 0.3, 0. , 0. ],
[ 0. , 0. , 0. , 0. ]])
:seealso: :func:`~spatialmath.base.transforms2d.trlog2`, :func:`~spatialmath.base.transforms3d.trlog`
"""
print('in log')
if self.N == 2:
log = [tr.trlog2(x) for x in self.data]
else:
log = [tr.trlog(x) for x in self.data]
if len(log) == 1:
return log[0]
else:
return log
def interp(self, s=None, T0=None):
"""
Interpolate pose (superclass method)
:param T0: initial pose
:type T0: SO2, SE2, SO3, SE3
:param s: interpolation coefficient, range 0 to 1
:type s: float or array_like
:return: interpolated pose
:rtype: SO2, SE2, SO3, SE3 instance
- ``X.interp(s)`` interpolates the pose X between identity when s=0
and X when s=1.
====== ====== =========== ===============================
len(X) len(s) len(result) Result
====== ====== =========== ===============================
1 1 1 Y = interp(identity, X, s)
M 1 M Y[i] = interp(T0, X[i], s)
1 M M Y[i] = interp(T0, X, s[i])
====== ====== =========== ===============================
Example::
>>> x = SE3.Rx(0.3)
>>> print(x.interp(0))
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
>>> print(x.interp(1))
SE3(array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.95533649, -0.29552021, 0. ],
[ 0. , 0.29552021, 0.95533649, 0. ],
[ 0. , 0. , 0. , 1. ]]))
>>> y = x.interp(x, np.linspace(0, 1, 10))
>>> len(y)
10
>>> y[5]
SE3(array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.98614323, -0.16589613, 0. ],
[ 0. , 0.16589613, 0.98614323, 0. ],
[ 0. , 0. , 0. , 1. ]]))
Notes:
#. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
:seealso: :func:`~spatialmath.base.transforms3d.trinterp`, :func:`spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
"""
s = argcheck.getvector(s)
if T0 is not None:
assert len(T0) == 1, 'len(X0) must == 1'
T0 = T0.A
if self.N == 2:
if len(s) > 1:
assert len(self) == 1, 'if len(s) > 1, len(X) must == 1'
return self.__class__([tr.trinterp2(self.A, T0, _s) for _s in s])
else:
assert len(s) == 1, 'if len(X) > 1, len(s) must == 1'
return self.__class__([tr.trinterp2(x, T0, s) for x in self.data])
elif self.N == 3:
if len(s) > 1:
assert len(self) == 1, 'if len(s) > 1, len(X) must == 1'
return self.__class__([tr.trinterp(self.A, T1=T0, s=_s) for _s in s])
else:
assert len(s) == 1, 'if len(X) > 1, len(s) must == 1'
return self.__class__([tr.trinterp(x, T1=T0, s=s) for x in self.data])
def norm(self):
"""
Normalize pose (superclass method)
:return: pose
:rtype: SO2, SE2, SO3, SE3 instance
- ``X.norm()`` is an equivalent pose object but the rotational matrix
part of all values has been adjusted to ensure it is a proper orthogonal
matrix rotation.
Example::
>>> x = SE3()
>>> y = x.norm()
>>> y
SE3(array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]))
Notes:
#. Only the direction of A vector (the z-axis) is unchanged.
#. Used to prevent finite word length arithmetic causing transforms to
become 'unnormalized'.
:seealso: :func:`~spatialmath.base.transforms3d.trnorm`, :func:`~spatialmath.base.transforms2d.trnorm2`
"""
if self.N == 2:
return self.__class__([tr.trnorm2(x) for x in self.data])
else:
return self.__class__([tr.trnorm(x) for x in self.data])
# ----------------------- i/o stuff
def printline(self, **kwargs):
"""
Print pose as a single line (superclass method)
:param label: text label to put at start of line
:type label: str
:param file: file to write formatted string to. [default, stdout]
:type file: str
:param fmt: conversion format for each number as used by ``format()``
:type fmt: str
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: optional formatted string
:rtype: str
For SO(3) or SE(3) also:
:param orient: 3-angle convention to use
:type orient: str
- ``X.printline()`` print ``X`` in single-line format to ``stdout``, followed
by a newline
- ``X.printline(file=None)`` return a string containing ``X`` in
single-line format
Example::
>>> x=SE3.Rx(0.3)
>>> x.printline()
t = 0, 0, 0; rpy/zyx = 17, 0, 0 deg
"""
if self.N == 2:
tr.trprint2(self.A, **kwargs)
else:
tr.trprint(self.A, **kwargs)
def __repr__(self):
"""
Readable representation of pose (superclass method)
:return: readable representation of the pose as a list of arrays
:rtype: str
Example::
>>> x = SE3.Rx(0.3)
>>> x
SE3(array([[ 1. , 0. , 0. , 0. ],
[ 0. , 0.95533649, -0.29552021, 0. ],
[ 0. , 0.29552021, 0.95533649, 0. ],
[ 0. , 0. , 0. , 1. ]]))
"""
name = type(self).__name__
if len(self) == 0:
return name + '([])'
elif len(self) == 1:
# need to indent subsequent lines of the native repr string by 4 spaces
return name + '(' + self.A.__repr__().replace('\n', '\n ') + ')'
else:
# format this as a list of ndarrays
return name + '([\n' + ',\n'.join([v.__repr__() for v in self.data]) + ' ])'
def __str__(self):
"""
Pretty string representation of pose (superclass method)
:return: readable representation of the pose
:rtype: str
Convert the pose's matrix value to a simple grid of numbers.
Example::
>>> x = SE3.Rx(0.3)
>>> print(x)
1 0 0 0
0 0.955336 -0.29552 0
0 0.29552 0.955336 0
0 0 0 1
Notes:
- By default, the output is colorised for an ANSI terminal console:
* red: rotational elements
* blue: translational elements
* white: constant elements
"""
return self._string(color=True)
def _string(self, color=False, tol=10):
"""
Pretty print the matrix value
:param color: colorise the output, defaults to False
:type color: bool, optional
:param tol: zero values smaller than tol*eps, defaults to 10
:type tol: float, optional
:return: multiline matrix representation
:rtype: str
Convert a matrix to a simple grid of numbers with optional
colorization for an ANSI terminal console:
* red: rotational elements
* blue: translational elements
* white: constant elements
Example::
>>> x = SE3.Rx(0.3)
>>> print(str(x))
1 0 0 0
0 0.955336 -0.29552 0
0 0.29552 0.955336 0
0 0 0 1
"""
#print('in __str__')
FG = lambda c: fg(c) if _color else ''
BG = lambda c: bg(c) if _color else ''
ATTR = lambda c: attr(c) if _color else ''
def mformat(self, X):
# X is an ndarray value to be display
# self provides set type for formatting
out = ''
n = self.N # dimension of rotation submatrix
for rownum, row in enumerate(X):
rowstr = ' '
# format the columns
for colnum, element in enumerate(row):
if isinstance(element, sympy.Expr):
s = '{:<12s}'.format(str(element))
else:
if tol > 0 and abs(element) < tol * _eps:
element = 0
s = '{:< 12g}'.format(element)
if rownum < n:
if colnum < n:
# rotation part
s = FG('red') + BG('grey_93') + s + ATTR(0)
else:
# translation part
s = FG('blue') + BG('grey_93') + s + ATTR(0)
else:
# bottom row
s = FG('grey_50') + BG('grey_93') + s + ATTR(0)
rowstr += s
out += rowstr + BG('grey_93') + ' ' + ATTR(0) + '\n'
return out
output_str = ''
if len(self.data) == 0:
output_str = '[]'
elif len(self.data) == 1:
# single matrix case
output_str = mformat(self, self.A)
else:
# sequence case
for count, X in enumerate(self.data):
# add separator lines and the index
output_str += fg('green') + '[{:d}] =\n'.format(count) + attr(0) + mformat(self, X)
return output_str
# ----------------------- graphics
def plot(self, *args, **kwargs):
"""
Plot pose object as a coordinate frame (superclass method)
:param `**kwargs`: plotting options
- ``X.plot()`` displays the pose ``X`` as a coordinate frame in either
2D or 3D axes. There are many options, see the links below.
Example::
>>> X = SE3.Rx(0.3)
>>> X.plot(frame='A', color='green')
:seealso: :func:`~spatialmath.base.transforms3d.trplot`, :func:`~spatialmath.base.transforms2d.trplot2`
"""
if self.N == 2:
tr.trplot2(self.A, *args, **kwargs)
else:
tr.trplot(self.A, *args, **kwargs)
def animate(self, *args, T0=None, **kwargs):
"""
Plot pose object as an animated coordinate frame (superclass method)
:param `**kwargs`: plotting options
- ``X.plot()`` displays the pose ``X`` as a coordinate frame moving
from the origin, or ``T0``, in either 2D or 3D axes. There are
many options, see the links below.
Example::
>>> X = SE3.Rx(0.3)
>>> X.animate(frame='A', color='green')
:seealso: :func:`~spatialmath.base.transforms3d.tranimate`, :func:`~spatialmath.base.transforms2d.tranimate2`
"""
if T0 is not None:
T0 = T0.A
if self.N == 2:
tr.tranimate2(self.A, T0=T0, *args, **kwargs)
else:
tr.tranimate(self.A, T0=T0, *args, **kwargs)
# ------------------------------------------------------------------------ #
#----------------------- arithmetic
def __mul__(left, right):
"""
Overloaded ``*`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
Pose composition, scaling or vector transformation:
- ``X * Y`` compounds the poses ``X`` and ``Y``
- ``X * s`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
- ``X * v`` linear transform of the vector ``v``
============== ============== =========== ======================
Multiplicands Product
------------------------------- -----------------------------------
left right type operation
============== ============== =========== ======================
Pose Pose Pose matrix product
Pose scalar NxN matrix element-wise product
scalar Pose NxN matrix element-wise product
Pose N-vector N-vector vector transform
Pose NxM matrix NxM matrix transform each column
============== ============== =========== ======================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar x Pose is handled by ``__rmul__``
#. scalar multiplication is commutative but the result is not a group
operation so the result will be a matrix
#. Any other input combinations result in a ValueError.
For pose composition the ``left`` and ``right`` operands may be a sequence
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right``
1 M M ``prod[i] = left * right[i]``
N 1 M ``prod[i] = left[i] * right``
M M M ``prod[i] = left[i] * right[i]``
========= ========== ==== ================================
For vector transformation there are three cases
========= =========== ===== ==========================
Multiplicands Product
---------------------- ---------------------------------
len(left) right.shape shape operation
========= =========== ===== ==========================
1 (N,) (N,) vector transformation
M (N,) (N,M) vector transformations
1 (N,M) (N,M) column transformation
========= =========== ===== ==========================
Notes:
#. for the ``SE2`` and ``SE3`` case the vectors are converted to homogeneous
form, transformed, then converted back to Euclidean form.
"""
if isinstance(left, right.__class__):
#print('*: pose x pose')
return left.__class__(left._op2(right, lambda x, y: x @ y))
elif isinstance(right, (list, tuple, np.ndarray)):
#print('*: pose x array')
if len(left) == 1 and argcheck.isvector(right, left.N):
# pose x vector
#print('*: pose x vector')
v = argcheck.getvector(right, out='col')
if left.isSE:
# SE(n) x vector
return tr.h2e(left.A @ tr.e2h(v))
else:
# SO(n) x vector
return left.A @ v
elif len(left) > 1 and argcheck.isvector(right, left.N):
# pose array x vector
#print('*: pose array x vector')
v = argcheck.getvector(right)
if left.isSE:
# SE(n) x vector
v = tr.e2h(v)
return np.array([tr.h2e(x @ v).flatten() for x in left.A]).T
else:
# SO(n) x vector
return np.array([(x @ v).flatten() for x in left.A]).T
elif len(left) == 1 and isinstance(right, np.ndarray) and left.isSO and right.shape[0] == left.N:
# SO(n) x matrix
return left.A @ right
elif len(left) == 1 and isinstance(right, np.ndarray) and left.isSE and right.shape[0] == left.N:
# SE(n) x matrix
return tr.h2e(left.A @ tr.e2h(right))
elif isinstance(right, np.ndarray) and left.isSO and right.shape[0] == left.N and len(left) == right.shape[1]:
# SO(n) x matrix
return np.c_[[x.A @ y for x,y in zip(right, left.T)]].T
elif isinstance(right, np.ndarray) and left.isSE and right.shape[0] == left.N and len(left) == right.shape[1]:
# SE(n) x matrix
return np.c_[[tr.h2e(x.A @ tr.e2h(y)) for x,y in zip(right, left.T)]].T
else:
raise ValueError('bad operands')
elif isinstance(right, (int, np.int64, float, np.float64)):
return left._op2(right, lambda x, y: x * y)
else:
return NotImplemented
def __rmul__(right, left):
"""
Overloaded ``*`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: NotImplemented
Left-multiplication by a scalar
- ``s * X`` performs elementwise multiplication of the elements of ``X`` by ``s``
Notes:
#. For other left-operands return ``NotImplemented``. Other classes
such as ``Plucker`` and ``Twist`` implement left-multiplication by
an ``SE33`` using their own ``__rmul__`` methods.
"""
if isinstance(left, (int, np.int64, float, np.float64)):
return right.__mul__(left)
else:
return NotImplemented
def __imul__(left, right):
"""
Overloaded ``*=`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises: ValueError
- ``X *= Y`` compounds the poses ``X`` and ``Y`` and places the result in ``X``
- ``X *= s`` performs elementwise multiplication of the elements of ``X``
and ``s`` and places the result in ``X``
:seealso: ``__mul__``
"""
return left.__mul__(right)
def __pow__(self, n):
"""
Overloaded ``**`` operator (superclass method)
:param n: pose
:return: pose to the power n
:type self: SO2, SE2, SO3, SE3
Raise all elements of pose to the specified power.
- ``X**n`` raise all values in ``X`` to the power ``n``
"""
assert type(n) is int, 'exponent must be an int'
return self.__class__([np.linalg.matrix_power(x, n) for x in self.data])
# def __ipow__(self, n):
# return self.__pow__(n)
def __truediv__(left, right):
"""
Overloaded ``/`` operator (superclass method)
:arg left: left multiplicand
:arg right: right multiplicand
:return: product
:raises ValueError: for incompatible arguments
:return: matrix
:rtype: numpy ndarray
Pose composition or scaling:
- ``X / Y`` compounds the poses ``X`` and ``Y.inv()``
- ``X / s`` performs elementwise multiplication of the elements of ``X`` by ``s``
============== ============== =========== =========================
Multiplicands Quotient
------------------------------- --------------------------------------
left right type operation
============== ============== =========== =========================
Pose Pose Pose matrix product by inverse
Pose scalar NxN matrix element-wise division
============== ============== =========== =========================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar multiplication is not a group operation so the result will
be a matrix
#. Any other input combinations result in a ValueError.
For pose composition the ``left`` and ``right`` operands may be a sequence
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left * right.inv()``
1 M M ``prod[i] = left * right[i].inv()``
N 1 M ``prod[i] = left[i] * right.inv()``
M M M ``prod[i] = left[i] * right[i].inv()``
========= ========== ==== ================================
"""
if isinstance(left, right.__class__):
return left.__class__(left._op2(right.inv(), lambda x, y: x @ y))
elif isinstance(right, (int, np.int64, float, np.float64)):
return left._op2(right, lambda x, y: x / y)
else:
raise ValueError('bad operands')
# def __itruediv__(left, right):
# """
# Overloaded ``/=`` operator (superclass method)
# :arg left: left dividend
# :arg right: right divisor
# :return: quotient
# :raises: ValueError
# - ``X /= Y`` compounds the poses ``X`` and ``Y.inv()`` and places the result in ``X``
# - ``X /= s`` performs elementwise division of the elements of ``X`` by ``s``
# :seealso: ``__truediv__``
# """
# return left.__truediv__(right)
def __add__(left, right):
"""
Overloaded ``+`` operator (superclass method)
:arg left: left addend
:arg right: right addend
:return: sum
:raises ValueError: for incompatible arguments
:return: matrix
:rtype: numpy ndarray, shape=(N,N)
Add elements of two poses. This is not a group operation so the
result is a matrix not a pose class.
- ``X + Y`` is the element-wise sum of the matrix value of ``X`` and ``Y``
- ``X + s`` is the element-wise sum of the matrix value of ``X`` and ``s``
- ``s + X`` is the element-wise sum of the matrix value of ``s`` and ``X``
============== ============== =========== ========================
Operands Sum
------------------------------- -------------------------------------
left right type operation
============== ============== =========== ========================
Pose Pose NxN matrix element-wise matrix sum
Pose scalar NxN matrix element-wise sum
scalar Pose NxN matrix element-wise sum
============== ============== =========== ========================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar + Pose is handled by ``__radd__``
#. scalar addition is commutative
#. Any other input combinations result in a ValueError.
For pose addition the ``left`` and ``right`` operands may be a sequence which
results in the result being a sequence:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left + right``
1 M M ``prod[i] = left + right[i]``
N 1 M ``prod[i] = left[i] + right``
M M M ``prod[i] = left[i] + right[i]``
========= ========== ==== ================================
"""
# results is not in the group, return an array, not a class
return left._op2(right, lambda x, y: x + y)
def __radd__(left, right):
"""
Overloaded ``+`` operator (superclass method)
:arg left: left addend
:arg right: right addend
:return: sum
:raises ValueError: for incompatible arguments
Left-addition by a scalar
- ``s + X`` performs elementwise addition of the elements of ``X`` and ``s``
"""
return left.__add__(right)
# def __iadd__(left, right):
# return left.__add__(right)
def __sub__(left, right):
"""
Overloaded ``-`` operator (superclass method)
:arg left: left minuend
:arg right: right subtrahend
:return: difference
:raises ValueError: for incompatible arguments
:return: matrix
:rtype: numpy ndarray, shape=(N,N)
Subtract elements of two poses. This is not a group operation so the
result is a matrix not a pose class.
- ``X - Y`` is the element-wise difference of the matrix value of ``X`` and ``Y``
- ``X - s`` is the element-wise difference of the matrix value of ``X`` and ``s``
- ``s - X`` is the element-wise difference of ``s`` and the matrix value of ``X``
============== ============== =========== ==============================
Operands Sum
------------------------------- -------------------------------------------
left right type operation
============== ============== =========== ==============================
Pose Pose NxN matrix element-wise matrix difference
Pose scalar NxN matrix element-wise sum
scalar Pose NxN matrix element-wise sum
============== ============== =========== ==============================
Notes:
#. Pose is ``SO2``, ``SE2``, ``SO3`` or ``SE3`` instance
#. N is 2 for ``SO2``, ``SE2``; 3 for ``SO3`` or ``SE3``
#. scalar - Pose is handled by ``__rsub__``
#. Any other input combinations result in a ValueError.
For pose addition the ``left`` and ``right`` operands may be a sequence which
results in the result being a sequence:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``prod = left - right``
1 M M ``prod[i] = left - right[i]``
N 1 M ``prod[i] = left[i] - right``
M M M ``prod[i] = left[i] right[i]``
========= ========== ==== ================================
"""
# results is not in the group, return an array, not a class
# TODO allow class +/- a conformant array
return left._op2(right, lambda x, y: x - y)
def __rsub__(left, right):
"""
Overloaded ``-`` operator (superclass method)
:arg left: left minuend
:arg right: right subtrahend
:return: difference
:raises ValueError: for incompatible arguments
Left-addition by a scalar
- ``s + X`` performs elementwise addition of the elements of ``X`` and ``s``
"""
return -left.__sub__(right)
# def __isub__(left, right):
# return left.__sub__(right)
def __eq__(left, right):
"""
Overloaded ``==`` operator (superclass method)
:param left: left side of comparison
:type self: SO2, SE2, SO3, SE3
:param right: right side of comparison
:type self: SO2, SE2, SO3, SE3
:return: poses are equal
:rtype: bool
Test two poses for equality
- ``X == Y`` is true of the poses are of the same type and numerically
equal.
If either operand contains a sequence the results is a sequence
according to:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``ret = left == right``
1 M M ``ret[i] = left == right[i]``
N 1 M ``ret[i] = left[i] == right``
M M M ``ret[i] = left[i] == right[i]``
========= ========== ==== ================================
"""
assert type(left) == type(right), 'operands to == are of different types'
return left._op2(right, lambda x, y: np.allclose(x, y))
def __ne__(left, right):
"""
Overloaded ``!=`` operator
:param left: left side of comparison
:type self: SO2, SE2, SO3, SE3
:param right: right side of comparison
:type self: SO2, SE2, SO3, SE3
:return: poses are not equal
:rtype: bool
Test two poses for inequality
- ``X == Y`` is true of the poses are of the same type but not numerically
equal.
If either operand contains a sequence the results is a sequence
according to:
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``ret = left != right``
1 M M ``ret[i] = left != right[i]``
N 1 M ``ret[i] = left[i] != right``
M M M ``ret[i] = left[i] != right[i]``
========= ========== ==== ================================
"""
return [not x for x in self == right]
def _op2(left, right, op):
"""
Perform binary operation
:param left: left side of comparison
:type self: SO2, SE2, SO3, SE3
:param right: right side of comparison
:type self: SO2, SE2, SO3, SE3
:param op: binary operation
:type op: callable
:raises ValueError: arguments are not compatible
:return: list of matrices
:rtype: list
Peform a binary operation on a pair of operands. If either operand
contains a sequence the results is a sequence accordinging to this
truth table.
========= ========== ==== ================================
len(left) len(right) len operation
========= ========== ==== ================================
1 1 1 ``ret = op(left, right)``
1 M M ``ret[i] = op(left, right[i])``
N 1 M ``ret[i] = op(left[i], right)``
M M M ``ret[i] = op(left[i], right[i])``
========= ========== ==== ================================
"""
if isinstance(right, left.__class__):
# class by class
if len(left) == 1:
if len(right) == 1:
#print('== 1x1')
return op(left.A, right.A)
else:
#print('== 1xN')
return [op(left.A, x) for x in right.A]
else:
if len(right) == 1:
#print('== Nx1')
return [op(x, right.A) for x in left.A]
elif len(left) == len(right):
#print('== NxN')
return [op(x, y) for (x, y) in zip(left.A, right.A)]
else:
raise ValueError('length of lists to == must be same length')
elif isinstance(right, (float, int, np.float64, np.int64)) or (isinstance(right, np.ndarray) and right.shape == left.shape):
# class by matrix
if len(left) == 1:
return op(left.A, right)
else:
return [op(x, right) for x in left.A]
class SMTwist(UserList):
"""
Superclass for 2D and 3D twist objects
Subclasses are:
- ``Twist2`` representing rigid-body motion in 2D as a 3-vector
- ``Twist`` representing rigid-body motion in 3D as a 6-vector
A twist is the unique elements of the logarithm of the corresponding SE(N)
matrix.
Arithmetic operators are overloaded but the operation they perform depend
on the types of the operands. For example:
- ``*`` will compose two instances of the same subclass, and the result will be
an instance of the same subclass, since this is a group operator.
These classes all inherit from ``UserList`` which enables them to
represent a sequence of values, ie. an ``Twist`` instance can contain
a sequence of twists. Most of the Python ``list`` operators
are applicable::
>>> x = Twist() # new instance with zero value
>>> len(x) # it is a sequence of one value
1
>>> x.append(x) # append to itself
>>> len(x) # it is a sequence of two values
2
>>> x[1] # the element has a 4x4 matrix value
SE3([
array([[1., 0., 0., 0.],
[0., 1., 0., 0.],
[0., 0., 1., 0.],
[0., 0., 0., 1.]]) ])
>>> x[1] = SE3.Rx(0.3) # set an elements of the sequence
>>> x.reverse() # reverse the elements in the sequence
>>> del x[1] # delete an element
"""
# ------------------------- list support -------------------------------#
def __init__(self):
# handle common cases
# deep copy
# numpy array
# list of numpy array
# validity checking??
# TODO should this be done by __new__?
super().__init__() # enable UserList superpowers
@classmethod
def Empty(cls):
"""
Construct an empty twist object (superclass method)
:param cls: The twist subclass
:type cls: type
:return: a twist object with zero values
:rtype: Twist or Twist2 instance
Example::
>>> x = Twist.Empty()
>>> len(x)
0
"""
X = cls()
X.data = []
return X
@property
def S(self):
"""
Twist as a vector (superclass property)
:return: Twist vector
:rtype: numpy.ndarray, shape=(N,)
- ``X.S`` is a 3-vector if X is a ``Twist2`` instance, and a 6-vector if
X is a ``Twist`` instance.
Notes::
- the vector is the unique elements of the se(N) representation
- the vector is sometimes referred to as the twist coordinate vector.
- if ``len(X)`` > 1 then return a list of vectors.
"""
# get the underlying numpy array
if len(self.data) == 1:
return self.data[0]
else:
return self.data
@property
def isprismatic(self):
"""
Test for prismatic twist (superclass property)
:return: If twist is purely prismatic
:rtype: book
Example::
>>> x = Twist.R([1,2,3], [4,5,6])
>>> x.isprismatic
False
"""
if len(self) == 1:
return tr.iszerovec(self.w)
else:
return [tr.iszerovec(x.w) for x in self.data]
@property
def unit(self):
"""
Unit twist
TW.unit() is a Twist object representing a unit aligned with the Twist
TW.
"""
if tr.iszerovec(self.w):
# rotational twist
return Twist(self.S / tr.norm(S.w))
else:
# prismatic twist
return Twist(tr.unitvec(self.v), [0, 0, 0])
@property
def isunit(self):
"""
Test for unit twist (superclass property)
:return: If twist is a unit-twist
:rtype: bool
"""
if len(self) == 1:
return tr.isunittwist(self.S)
else:
return [tr.isunittwist(x) for x in self.data]
def __getitem__(self, i):
"""
Access value of a twist object (superclass method)
:param i: index of element to return
:type i: int
:return: the specific element of the twist
:rtype: Twist or Twist2 instance
:raises IndexError: if the element is out of bounds
Note that only a single index is supported, slices are not.
Example::
>>> x = SE3.Rx([0, math.pi/2, math.pi])
>>> len(x)
3
>>> x[1]
1 0 0 0
0 0 -1 0
0 1 0 0
0 0 0 1
"""
# print('getitem', i, 'class', self.__class__)
if isinstance(i, slice):
return self.__class__([self.data[k] for k in range(i.start or 0, i.stop or len(self), i.step or 1)], check=False)
else:
return self.__class__(self.data[i], check=False)
def __setitem__(self, i, value):
"""
Assign a value to a twist object (superclass method)
:param i: index of element to assign to
:type i: int
:param value: the value to insert
:type value: Twist or Twist2 instance
:raises ValueError: incorrect type of assigned value
Assign the argument to an element of the object's internal list of values.
This supports the assignement operator, for example::
>>> x = SE3([SE3() for i in range(10)]) # sequence of ten identity values
>>> len(x)
10
>>> x[3] = SE3.Rx(0.2) # assign to position 3 in the list
"""
if not type(self) == type(value):
raise ValueError("cant append different type of pose object")
if len(value) > 1:
raise ValueError("cant insert a pose sequence - must have len() == 1")
self.data[i] = value.A
def append(self, x):
"""
Append a twist object
:param x: A twist subclass
:type x: subclass
:raises ValueError: incorrect type of appended object
Appends the argument to the object's internal list.
Examples::
>>> x = Twist()
>>> len(x)
1
>>> x.append(Twist())
>>> len(x)
2
"""
#print('in append method')
if not type(self) == type(x):
raise ValueError("cant append different type of pose object")
if len(x) > 1:
raise ValueError("cant append a pose sequence - use extend")
super().append(x.S)
def pop(self):
"""
Pop value of a pose object (superclass method)
:return: the specific element of the pose
:rtype: SO2, SE2, SO3, SE3 instance
:raises IndexError: if there are no values to pop
Removes the first pose value from the sequence in the pose object.
Example::
>>> x = SE3.Rx([0, math.pi/2, math.pi])
>>> len(x)
3
>>> y = x.pop()
>>> y
SE3(array([[ 1.0000000e+00, 0.0000000e+00, 0.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, -1.0000000e+00, -1.2246468e-16, 0.0000000e+00],
[ 0.0000000e+00, 1.2246468e-16, -1.0000000e+00, 0.0000000e+00],
[ 0.0000000e+00, 0.0000000e+00, 0.0000000e+00, 1.0000000e+00]]))
>>> len(x)
2
"""
return self.__class__(super().pop())
def insert(self, i, value):
"""
Insert a value to a pose object (superclass method)
:param i: element to insert value before
:type i: int
:param value: the value to insert
:type value: SO2, SE2, SO3, SE3 instance
:raises ValueError: incorrect type of inserted value
Inserts the argument into the object's internal list of values.
Examples::
>>> x = SE3()
>>> x.inert(0, SE3.Rx(0.1)) # insert at position 0 in the list
>>> len(x)
2
"""
if not type(self) == type(value):
raise ValueError("cant append different type of pose object")
if len(value) > 1:
raise ValueError("cant insert a pose sequence - must have len() == 1")
super().insert(i, value.A)
def prod(self):
"""
%Twist.prod Compound array of twists
%
TW.prod is a twist representing the product (composition) of the
successive elements of TW (1xN), an array of Twists.
%
%
See also RTBPose.prod, Twist.mtimes.
"""
out = self[0]
for t in self[1:]:
out *= t
return out