Source code for spatialmath.super_pose

# 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