# Source code for spatialmath.base.transforms3d

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

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

    TODO:

        - trinterp
        - trjac, trjac2
        - tranimate, tranimate2
    """

    # This file is part of the SpatialMath toolbox for Python
    # https://github.com/petercorke/spatialmath-python
    #
    # MIT License
    #
    # Copyright (c) 1993-2020 Peter Corke
    #
    # Permission is hereby granted, free of charge, to any person obtaining a copy
    # of this software and associated documentation files (the "Software"), to deal
    # in the Software without restriction, including without limitation the rights
    # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
    # copies of the Software, and to permit persons to whom the Software is
    # furnished to do so, subject to the following conditions:
    #
    # The above copyright notice and this permission notice shall be included in all
    # copies or substantial portions of the Software.
    #
    # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
    # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
    # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
    # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
    # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
    # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
    # SOFTWARE.

    # Contributors:
    #
    #     1. Luis Fernando Lara Tobar and Peter Corke, 2008
    #     2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017 (robopy)
    #     3. Peter Corke, 2020

    import sys
    import math
    import numpy as np
    from spatialmath.base import argcheck
    from spatialmath.base import vectors as vec
    from spatialmath.base import transformsNd as trn
    from spatialmath.base import quaternions as quat

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

        def issymbol(x):
            return isinstance(x, sym.Symbol)
    except BaseException:
        def issymbol(x):
            return False

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

    def colvec(v):
        return np.array(v).reshape((len(v), 1))

    # ---------------------------------------------------------------------------------------#

    def _cos(theta):
        if issymbol(theta):
            return sym.cos(theta)
        else:
            return math.cos(theta)

    def _sin(theta):
        if issymbol(theta):
            return sym.sin(theta)
        else:
            return math.sin(theta)

    def rotx(theta, unit="rad"):
        """
        Create SO(3) rotation about X-axis

        :param theta: rotation angle about X-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpy.ndarray, shape=(3,3)

        - ``rotx(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
          of THETA radians about the x-axis
        - ``rotx(THETA, "deg")`` as above but THETA is in degrees

        :seealso: :func:`~trotx`
        """

        theta = argcheck.getunit(theta, unit)
        ct = _cos(theta)
        st = _sin(theta)
        R = np.array([
            [1, 0, 0],
            [0, ct, -st],
            [0, st, ct]])
        return R

    # ---------------------------------------------------------------------------------------#
    def roty(theta, unit="rad"):
        """
        Create SO(3) rotation about Y-axis

        :param theta: rotation angle about Y-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpy.ndarray, shape=(3,3)

        - ``roty(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
          of THETA radians about the y-axis
        - ``roty(THETA, "deg")`` as above but THETA is in degrees

        :seealso: :func:`~troty`
        """

        theta = argcheck.getunit(theta, unit)
        ct = _cos(theta)
        st = _sin(theta)
        R = np.array([
            [ct, 0, st],
            [0, 1, 0],
            [-st, 0, ct]])
        return R

    # ---------------------------------------------------------------------------------------#
    def rotz(theta, unit="rad"):
        """
        Create SO(3) rotation about Z-axis

        :param theta: rotation angle about Z-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpy.ndarray, shape=(3,3)

        - ``rotz(THETA)`` is an SO(3) rotation matrix (3x3) representing a rotation
          of THETA radians about the z-axis
        - ``rotz(THETA, "deg")`` as above but THETA is in degrees

        :seealso: :func:`~yrotz`
        """
        theta = argcheck.getunit(theta, unit)
        ct = _cos(theta)
        st = _sin(theta)
        R = np.array([
            [ct, -st, 0],
            [st, ct, 0],
            [0, 0, 1]])
        return R

    # ---------------------------------------------------------------------------------------#
    def trotx(theta, unit="rad", t=None):
        """
        Create SE(3) pure rotation about X-axis

        :param theta: rotation angle about X-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param t: translation 3-vector, defaults to [0,0,0]
        :type t: array_like    :return: 4x4 homogeneous transformation matrix
        :rtype: numpy.ndarray, shape=(4,4)

        - ``trotx(THETA)`` is a homogeneous transformation (4x4) representing a rotation
          of THETA radians about the x-axis.
        - ``trotx(THETA, 'deg')`` as above but THETA is in degrees
        - ``trotx(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]

        :seealso: :func:`~rotx`
        """
        T = np.pad(rotx(theta, unit), (0, 1), mode='constant')
        if t is not None:
            T[:3, 3] = argcheck.getvector(t, 3, 'array')
        T[3, 3] = 1.0
        return T

    # ---------------------------------------------------------------------------------------#
    def troty(theta, unit="rad", t=None):
        """
        Create SE(3) pure rotation about Y-axis

        :param theta: rotation angle about Y-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param t: translation 3-vector, defaults to [0,0,0]
        :type t: array_like
        :return: 4x4 homogeneous transformation matrix as a numpy array
        :rtype: numpy.ndarray, shape=(4,4)

        - ``troty(THETA)`` is a homogeneous transformation (4x4) representing a rotation
          of THETA radians about the y-axis.
        - ``troty(THETA, 'deg')`` as above but THETA is in degrees
        - ``troty(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]

        :seealso: :func:`~roty`
        """
        T = np.pad(roty(theta, unit), (0, 1), mode='constant')
        if t is not None:
            T[:3, 3] = argcheck.getvector(t, 3, 'array')
        T[3, 3] = 1.0
        return T

    # ---------------------------------------------------------------------------------------#
    def trotz(theta, unit="rad", t=None):
        """
        Create SE(3) pure rotation about Z-axis

        :param theta: rotation angle about Z-axis
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param t: translation 3-vector, defaults to [0,0,0]
        :type t: array_like
        :return: 4x4 homogeneous transformation matrix
        :rtype: numpy.ndarray, shape=(4,4)

        - ``trotz(THETA)`` is a homogeneous transformation (4x4) representing a rotation
          of THETA radians about the z-axis.
        - ``trotz(THETA, 'deg')`` as above but THETA is in degrees
        - ``trotz(THETA, 'rad', t=[x,y,z])`` as above with translation of [x,y,z]

        :seealso: :func:`~rotz`
        """
        T = np.pad(rotz(theta, unit), (0, 1), mode='constant')
        if t is not None:
            T[:3, 3] = argcheck.getvector(t, 3, 'array')
        T[3, 3] = 1.0
        return T

    # ---------------------------------------------------------------------------------------#

    def transl(x, y=None, z=None):
        """
        Create SE(3) pure translation, or extract translation from SE(3) matrix

        :param x: translation along X-axis
        :type x: float
        :param y: translation along Y-axis
        :type y: float
        :param z: translation along Z-axis
        :type z: float
        :return: 4x4 homogeneous transformation matrix
        :rtype: numpy.ndarray, shape=(4,4)

        Create a translational SE(3) matrix:

        - ``T = transl( X, Y, Z )`` is an SE(3) homogeneous transform (4x4) representing a
          pure translation of X, Y and Z.
        - ``T = transl( V )`` as above but the translation is given by a 3-element
          list, dict, or a numpy array, row or column vector.

        Extract the translational part of an SE(3) matrix:

        - ``P = TRANSL(T)`` is the translational part of a homogeneous transform T as a
          3-element numpy array.

        :seealso: :func:`~spatialmath.base.transforms2d.transl2`
       """

        if np.isscalar(x):
            T = np.identity(4)
            T[:3, 3] = [x, y, z]
            return T
        elif argcheck.isvector(x, 3):
            T = np.identity(4)
            T[:3, 3] = argcheck.getvector(x, 3, out='array')
            return T
        elif argcheck.ismatrix(x, (4, 4)):
            return x[:3, 3]
        else:
            ValueError('bad argument')

    def ishom(T, check=False, tol=10):
        """
        Test if matrix belongs to SE(3)

        :param T: matrix to test
        :type T: numpy.ndarray
        :param check: check validity of rotation submatrix
        :type check: bool
        :return: whether matrix is an SE(3) homogeneous transformation matrix
        :rtype: bool

        - ``ISHOM(T)`` is True if the argument ``T`` is of dimension 4x4
        - ``ISHOM(T, check=True)`` as above, but also checks orthogonality of the rotation sub-matrix and
          validitity of the bottom row.

        :seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~isrot`, :func:`~spatialmath.base.transforms2d.ishom2`
        """
        return isinstance(T, np.ndarray) and T.shape == (4, 4) and (not check or (trn.isR(T[:3, :3], tol=tol) and np.all(T[3, :] == np.array([0, 0, 0, 1]))))

    def isrot(R, check=False, tol=10):
        """
        Test if matrix belongs to SO(3)

        :param R: matrix to test
        :type R: numpy.ndarray
        :param check: check validity of rotation submatrix
        :type check: bool
        :return: whether matrix is an SO(3) rotation matrix
        :rtype: bool

        - ``ISROT(R)`` is True if the argument ``R`` is of dimension 3x3
        - ``ISROT(R, check=True)`` as above, but also checks orthogonality of the rotation matrix.

        :seealso: :func:`~spatialmath.base.transformsNd.isR`, :func:`~spatialmath.base.transforms2d.isrot2`,  :func:`~ishom`
        """
        return isinstance(R, np.ndarray) and R.shape == (3, 3) and (not check or trn.isR(R, tol=tol))

    # ---------------------------------------------------------------------------------------#
    def rpy2r(roll, pitch=None, yaw=None, *, unit='rad', order='zyx'):
        """
        Create an SO(3) rotation matrix from roll-pitch-yaw angles

        :param roll: roll angle
        :type roll: float
        :param pitch: pitch angle
        :type pitch: float
        :param yaw: yaw angle
        :type yaw: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpdy.ndarray, shape=(3,3)

        - ``rpy2r(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
          (3x3) equivalent to the specified roll, pitch, yaw angles angles.
          These correspond to successive rotations about the axes specified by ``order``:

            - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
              then by roll about the new x-axis.  Convention for a mobile robot with x-axis forward
              and y-axis sideways.
            - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
              then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
              and y-axis between the gripper fingers.
            - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
              then by roll about the new z-axis. Convention for a camera with z-axis parallel
              to the optic axis and x-axis parallel to the pixel rows.

        - ``rpy2r(RPY)`` as above but the roll, pitch, yaw angles are taken
          from ``RPY`` which is a 3-vector (array_like) with values
          (ROLL, PITCH, YAW).

        :seealso: :func:`~eul2r`, :func:`~rpy2tr`, :func:`~tr2rpy`
        """

        if np.isscalar(roll):
            angles = [roll, pitch, yaw]
        else:
            angles = argcheck.getvector(roll, 3)

        angles = argcheck.getunit(angles, unit)

        if order == 'xyz' or order == 'arm':
            R = rotx(angles[2]) @ roty(angles[1]) @ rotz(angles[0])
        elif order == 'zyx' or order == 'vehicle':
            R = rotz(angles[2]) @ roty(angles[1]) @ rotx(angles[0])
        elif order == 'yxz' or order == 'camera':
            R = roty(angles[2]) @ rotx(angles[1]) @ rotz(angles[0])
        else:
            raise ValueError('Invalid angle order')

        return R

    # ---------------------------------------------------------------------------------------#
    def rpy2tr(roll, pitch=None, yaw=None, unit='rad', order='zyx'):
        """
        Create an SE(3) rotation matrix from roll-pitch-yaw angles

        :param roll: roll angle
        :type roll: float
        :param pitch: pitch angle
        :type pitch: float
        :param yaw: yaw angle
        :type yaw: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpdy.ndarray, shape=(3,3)

        - ``rpy2tr(ROLL, PITCH, YAW)`` is an SO(3) orthonormal rotation matrix
          (3x3) equivalent to the specified roll, pitch, yaw angles angles.
          These correspond to successive rotations about the axes specified by ``order``:

            - 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
              then by roll about the new x-axis.  Convention for a mobile robot with x-axis forward
              and y-axis sideways.
            - 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
              then by roll about the new z-axis. Convention for a robot gripper with z-axis forward
              and y-axis between the gripper fingers.
            - 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
              then by roll about the new z-axis. Convention for a camera with z-axis parallel
              to the optic axis and x-axis parallel to the pixel rows.

        - ``rpy2tr(RPY)`` as above but the roll, pitch, yaw angles are taken
          from ``RPY`` which is a 3-vector (array_like) with values
          (ROLL, PITCH, YAW).

        Notes:

        - The translational part is zero.

        :seealso: :func:`~eul2tr`, :func:`~rpy2r`, :func:`~tr2rpy`
        """

        R = rpy2r(roll, pitch, yaw, order=order, unit=unit)
        return trn.r2t(R)

    # ---------------------------------------------------------------------------------------#

    def eul2r(phi, theta=None, psi=None, unit='rad'):
        """
        Create an SO(3) rotation matrix from Euler angles

        :param phi: Z-axis rotation
        :type phi: float
        :param theta: Y-axis rotation
        :type theta: float
        :param psi: Z-axis rotation
        :type psi: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: 3x3 rotation matrix
        :rtype: numpdy.ndarray, shape=(3,3)

        - ``R = eul2r(PHI, THETA, PSI)`` is an SO(3) orthonornal rotation
          matrix equivalent to the specified Euler angles.  These correspond
          to rotations about the Z, Y, Z axes respectively.
        - ``R = eul2r(EUL)`` as above but the Euler angles are taken from
          ``EUL`` which is a 3-vector (array_like) with values
          (PHI THETA PSI).

        :seealso: :func:`~rpy2r`, :func:`~eul2tr`, :func:`~tr2eul`
        """

        if np.isscalar(phi):
            angles = [phi, theta, psi]
        else:
            angles = argcheck.getvector(phi, 3)

        angles = argcheck.getunit(angles, unit)

        return rotz(angles[0]) @ roty(angles[1]) @ rotz(angles[2])

    # ---------------------------------------------------------------------------------------#
    def eul2tr(phi, theta=None, psi=None, unit='rad'):
        """
        Create an SE(3) pure rotation matrix from Euler angles

        :param phi: Z-axis rotation
        :type phi: float
        :param theta: Y-axis rotation
        :type theta: float
        :param psi: Z-axis rotation
        :type psi: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: 4x4 homogeneous transformation matrix
        :rtype: numpdy.ndarray, shape=(4,4)

        - ``R = eul2tr(PHI, THETA, PSI)`` is an SE(3) homogeneous transformation
          matrix equivalent to the specified Euler angles.  These correspond
          to rotations about the Z, Y, Z axes respectively.
        - ``R = eul2tr(EUL)`` as above but the Euler angles are taken from
          ``EUL`` which is a 3-vector (array_like) with values
          (PHI THETA PSI).

        Notes:

        - The translational part is zero.

        :seealso: :func:`~rpy2tr`, :func:`~eul2r`, :func:`~tr2eul`
        """

        R = eul2r(phi, theta, psi, unit=unit)
        return trn.r2t(R)

    # ---------------------------------------------------------------------------------------#

    def angvec2r(theta, v, unit='rad'):
        """
        Create an SO(3) rotation matrix from rotation angle and axis

        :param theta: rotation
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param v: rotation axis, 3-vector
        :type v: array_like
        :return: 3x3 rotation matrix
        :rtype: numpdy.ndarray, shape=(3,3)

        ``angvec2r(THETA, V)`` is an SO(3) orthonormal rotation matrix
        equivalent to a rotation of ``THETA`` about the vector ``V``.

        Notes:

        - If ``THETA == 0`` then return identity matrix.
        - If ``THETA ~= 0`` then ``V`` must have a finite length.

        :seealso: :func:`~angvec2tr`, :func:`~tr2angvec`
        """
        assert np.isscalar(theta) and argcheck.isvector(v, 3), "Arguments must be theta and vector"

        if np.linalg.norm(v) < 10 * _eps:
            return np.eye(3)

        theta = argcheck.getunit(theta, unit)

        # Rodrigue's equation

        sk = trn.skew(vec.unitvec(v))
        R = np.eye(3) + math.sin(theta) * sk + (1.0 - math.cos(theta)) * sk @ sk
        return R

    # ---------------------------------------------------------------------------------------#
    def angvec2tr(theta, v, unit='rad'):
        """
        Create an SE(3) pure rotation from rotation angle and axis

        :param theta: rotation
        :type theta: float
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :param v: rotation axis, 3-vector
        :type v: : array_like
        :return: 4x4 homogeneous transformation matrix
        :rtype: numpdy.ndarray, shape=(4,4)

        ``angvec2tr(THETA, V)`` is an SE(3) homogeneous transformation matrix
        equivalent to a rotation of ``THETA`` about the vector ``V``.

        Notes:

        - If ``THETA == 0`` then return identity matrix.
        - If ``THETA ~= 0`` then ``V`` must have a finite length.
        - The translational part is zero.

        :seealso: :func:`~angvec2r`, :func:`~tr2angvec`
        """
        return trn.r2t(angvec2r(theta, v, unit=unit))

    # ---------------------------------------------------------------------------------------#
    def oa2r(o, a=None):
        """
        Create SO(3) rotation matrix from two vectors

        :param o: 3-vector parallel to Y- axis
        :type o: array_like
        :param a: 3-vector parallel to the Z-axis
        :type o: array_like
        :return: 3x3 rotation matrix
        :rtype: numpy.ndarray, shape=(3,3)

        ``T = oa2tr(O, A)`` is an SO(3) orthonormal rotation matrix for a frame defined in terms of
        vectors parallel to its Y- and Z-axes with respect to a reference frame.  In robotics these axes are
        respectively called the orientation and approach vectors defined such that
        R = [N O A] and N = O x A.

        Steps:

            1. N' = O x A
            2. O' = A x N
            3. normalize N', O', A
            4. stack horizontally into rotation matrix

        Notes:

        - The A vector is the only guaranteed to have the same direction in the resulting
          rotation matrix
        - O and A do not have to be unit-length, they are normalized
        - O and A do not have to be orthogonal, so long as they are not parallel
        - The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.

        :seealso: :func:`~oa2tr`
        """
        o = argcheck.getvector(o, 3, out='array')
        a = argcheck.getvector(a, 3, out='array')
        n = np.cross(o, a)
        o = np.cross(a, n)
        R = np.stack((vec.unitvec(n), vec.unitvec(o), vec.unitvec(a)), axis=1)
        return R

    # ---------------------------------------------------------------------------------------#
    def oa2tr(o, a=None):
        """
        Create SE(3) pure rotation from two vectors

        :param o: 3-vector parallel to Y- axis
        :type o: array_like
        :param a: 3-vector parallel to the Z-axis
        :type o: array_like
        :return: 4x4 homogeneous transformation matrix
        :rtype: numpy.ndarray, shape=(4,4)

        ``T = oa2tr(O, A)`` is an SE(3) homogeneous transformation matrix for a frame defined in terms of
        vectors parallel to its Y- and Z-axes with respect to a reference frame.  In robotics these axes are
        respectively called the orientation and approach vectors defined such that
        R = [N O A] and N = O x A.

        Steps:

            1. N' = O x A
            2. O' = A x N
            3. normalize N', O', A
            4. stack horizontally into rotation matrix

        Notes:

        - The A vector is the only guaranteed to have the same direction in the resulting
          rotation matrix
        - O and A do not have to be unit-length, they are normalized
        - O and A do not have to be orthogonal, so long as they are not parallel
        - The translational part is zero.
        - The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.

        :seealso: :func:`~oa2r`
        """
        return trn.r2t(oa2r(o, a))

    # ------------------------------------------------------------------------------------------------------------------- #
    def tr2angvec(T, unit='rad', check=False):
        r"""
        Convert SO(3) or SE(3) to angle and rotation vector

        :param R: SO(3) or SE(3) matrix
        :type R: numpy.ndarray, shape=(3,3) or (4,4)
        :param unit: 'rad' or 'deg'
        :type unit: str
        :param check: check that rotation matrix is valid
        :type check: bool
        :return: :math:`(\theta, {\bf v})`
        :rtype: float, numpy.ndarray, shape=(3,)

        ``tr2angvec(R)`` is a rotation angle and a vector about which the rotation
        acts that corresponds to the rotation part of ``R``.

        By default the angle is in radians but can be changed setting `unit='deg'`.

        Notes:

        - If the input is SE(3) the translation component is ignored.

        :seealso: :func:`~angvec2r`, :func:`~angvec2tr`, :func:`~tr2rpy`, :func:`~tr2eul`
        """

        if argcheck.ismatrix(T, (4, 4)):
            R = trn.t2r(T)
        else:
            R = T
        assert isrot(R, check=check)

        v = trn.vex(trlog(R))

        if vec.iszerovec(v):
            theta = 0
            v = np.r_[0, 0, 0]
        else:
            theta = vec.norm(v)
            v = vec.unitvec(v)

        if unit == 'deg':
            theta *= 180 / math.pi

        return (theta, v)

    # ------------------------------------------------------------------------------------------------------------------- #
    def tr2eul(T, unit='rad', flip=False, check=False):
        r"""
        Convert SO(3) or SE(3) to ZYX Euler angles

        :param R: SO(3) or SE(3) matrix
        :type R: numpy.ndarray, shape=(3,3) or (4,4)
        :param unit: 'rad' or 'deg'
        :type unit: str
        :param flip: choose first Euler angle to be in quadrant 2 or 3
        :type flip: bool
        :param check: check that rotation matrix is valid
        :type check: bool
        :return: ZYZ Euler angles
        :rtype: numpy.ndarray, shape=(3,)

        ``tr2eul(R)`` are the Euler angles corresponding to
        the rotation part of ``R``.

        The 3 angles :math:`[\phi, \theta, \psi` correspond to sequential rotations about the
        Z, Y and Z axes respectively.

        By default the angles are in radians but can be changed setting `unit='deg'`.

        Notes:

        - There is a singularity for the case where :math:`\theta=0` in which case :math:`\phi` is arbitrarily set to zero and :math:`\phi` is set to :math:`\phi+\psi`.
        - If the input is SE(3) the translation component is ignored.

        :seealso: :func:`~eul2r`, :func:`~eul2tr`, :func:`~tr2rpy`, :func:`~tr2angvec`
        """

        if argcheck.ismatrix(T, (4, 4)):
            R = trn.t2r(T)
        else:
            R = T
        assert isrot(R, check=check)

        eul = np.zeros((3,))
        if abs(R[0, 2]) < 10 * _eps and abs(R[1, 2]) < 10 * _eps:
            eul[0] = 0
            sp = 0
            cp = 1
            eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
            eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0], -sp * R[0, 1] + cp * R[1, 1])
        else:
            if flip:
                eul[0] = math.atan2(-R[1, 2], -R[0, 2])
            else:
                eul[0] = math.atan2(R[1, 2], R[0, 2])
            sp = math.sin(eul[0])
            cp = math.cos(eul[0])
            eul[1] = math.atan2(cp * R[0, 2] + sp * R[1, 2], R[2, 2])
            eul[2] = math.atan2(-sp * R[0, 0] + cp * R[1, 0], -sp * R[0, 1] + cp * R[1, 1])

        if unit == 'deg':
            eul *= 180 / math.pi

        return eul

    # ------------------------------------------------------------------------------------------------------------------- #

    def tr2rpy(T, unit='rad', order='zyx', check=False):
        """
        Convert SO(3) or SE(3) to roll-pitch-yaw angles

        :param R: SO(3) or SE(3) matrix
        :type R: numpy.ndarray, shape=(3,3) or (4,4)
        :param unit: 'rad' or 'deg'
        :type unit: str
        :param order: 'xyz', 'zyx' or 'yxz' [default 'zyx']
        :type unit: str
        :param check: check that rotation matrix is valid
        :type check: bool
        :return: Roll-pitch-yaw angles
        :rtype: numpy.ndarray, shape=(3,)

        ``tr2rpy(R)`` are the roll-pitch-yaw angles corresponding to
        the rotation part of ``R``.

        The 3 angles RPY=[R,P,Y] correspond to sequential rotations about the
        Z, Y and X axes respectively.  The axis order sequence can be changed by
        setting:

        - `order='xyz'`  for sequential rotations about X, Y, Z axes
        - `order='yxz'`  for sequential rotations about Y, X, Z axes

        By default the angles are in radians but can be changed setting `unit='deg'`.

        Notes:

        - There is a singularity for the case where P=:math:`\pi/2` in which case R is arbitrarily set to zero and Y is the sum (R+Y).
        - If the input is SE(3) the translation component is ignored.

        :seealso: :func:`~rpy2r`, :func:`~rpy2tr`, :func:`~tr2eul`, :func:`~tr2angvec`
        """

        if argcheck.ismatrix(T, (4, 4)):
            R = trn.t2r(T)
        else:
            R = T
        assert isrot(R, check=check)

        rpy = np.zeros((3,))
        if order == 'xyz' or order == 'arm':

            # XYZ order
            if abs(abs(R[0, 2]) - 1) < 10 * _eps:  # when |R13| == 1
                # singularity
                rpy[0] = 0  # roll is zero
                if R[0, 2] > 0:
                    rpy[2] = math.atan2(R[2, 1], R[1, 1])   # R+Y
                else:
                    rpy[2] = -math.atan2(R[1, 0], R[2, 0])   # R-Y
                rpy[1] = math.asin(R[0, 2])
            else:
                rpy[0] = -math.atan2(R[0, 1], R[0, 0])
                rpy[2] = -math.atan2(R[1, 2], R[2, 2])

                k = np.argmax(np.abs([R[0, 0], R[0, 1], R[1, 2], R[2, 2]]))
                if k == 0:
                    rpy[1] = math.atan(R[0, 2] * math.cos(rpy[0]) / R[0, 0])
                elif k == 1:
                    rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[0]) / R[0, 1])
                elif k == 2:
                    rpy[1] = -math.atan(R[0, 2] * math.sin(rpy[2]) / R[1, 2])
                elif k == 3:
                    rpy[1] = math.atan(R[0, 2] * math.cos(rpy[2]) / R[2, 2])

        elif order == 'zyx' or order == 'vehicle':

            # old ZYX order (as per Paul book)
            if abs(abs(R[2, 0]) - 1) < 10 * _eps:  # when |R31| == 1
                # singularity
                rpy[0] = 0     # roll is zero
                if R[2, 0] < 0:
                    rpy[2] = -math.atan2(R[0, 1], R[0, 2])  # R-Y
                else:
                    rpy[2] = math.atan2(-R[0, 1], -R[0, 2])  # R+Y
                rpy[1] = -math.asin(R[2, 0])
            else:
                rpy[0] = math.atan2(R[2, 1], R[2, 2])  # R
                rpy[2] = math.atan2(R[1, 0], R[0, 0])  # Y

                k = np.argmax(np.abs([R[0, 0], R[1, 0], R[2, 1], R[2, 2]]))
                if k == 0:
                    rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[2]) / R[0, 0])
                elif k == 1:
                    rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[2]) / R[1, 0])
                elif k == 2:
                    rpy[1] = -math.atan(R[2, 0] * math.sin(rpy[0]) / R[2, 1])
                elif k == 3:
                    rpy[1] = -math.atan(R[2, 0] * math.cos(rpy[0]) / R[2, 2])

        elif order == 'yxz' or order == 'camera':

            if abs(abs(R[1, 2]) - 1) < 10 * _eps:  # when |R23| == 1
                    # singularity
                rpy[0] = 0
                if R[1, 2] < 0:
                    rpy[2] = -math.atan2(R[2, 0], R[0, 0])   # R-Y
                else:
                    rpy[2] = math.atan2(-R[2, 0], -R[2, 1])   # R+Y
                rpy[1] = -math.asin(R[1, 2])    # P
            else:
                rpy[0] = math.atan2(R[1, 0], R[1, 1])
                rpy[2] = math.atan2(R[0, 2], R[2, 2])

                k = np.argmax(np.abs([R[1, 0], R[1, 1], R[0, 2], R[2, 2]]))
                if k == 0:
                    rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[0]) / R[1, 0])
                elif k == 1:
                    rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[0]) / R[1, 1])
                elif k == 2:
                    rpy[1] = -math.atan(R[1, 2] * math.sin(rpy[2]) / R[0, 2])
                elif k == 3:
                    rpy[1] = -math.atan(R[1, 2] * math.cos(rpy[2]) / R[2, 2])

        else:
            raise ValueError('Invalid order')

        if unit == 'deg':
            rpy *= 180 / math.pi

        return rpy

    # ---------------------------------------------------------------------------------------#
    def trlog(T, check=True):
        """
        Logarithm of SO(3) or SE(3) matrix

        :param T: SO(3) or SE(3) matrix
        :type T: numpy.ndarray, shape=(3,3) or (4,4)
        :return: logarithm
        :rtype: numpy.ndarray, shape=(3,3) or (4,4)
        :raises: ValueError

        An efficient closed-form solution of the matrix logarithm for arguments that are SO(3) or SE(3).

        - ``trlog(R)`` is the logarithm of the passed rotation matrix ``R`` which will be
          3x3 skew-symmetric matrix.  The equivalent vector from ``vex()`` is parallel to rotation axis
          and its norm is the amount of rotation about that axis.
        - ``trlog(T)`` is the logarithm of the passed homogeneous transformation matrix ``T`` which will be
          4x4 augumented skew-symmetric matrix. The equivalent vector from ``vexa()`` is the twist
          vector (6x1) comprising [v w].

        :seealso: :func:`~trexp`, :func:`~spatialmath.base.transformsNd.vex`, :func:`~spatialmath.base.transformsNd.vexa`
        """

        if ishom(T, check=check):
            # SE(3) matrix

            if trn.iseye(T):
                # is identity matrix
                return np.zeros((4, 4))
            else:
                [R, t] = trn.tr2rt(T)

                if trn.iseye(R):
                    # rotation matrix is identity
                    skw = np.zeros((3, 3))
                    v = t
                    theta = 1
                else:
                    S = trlog(R, check=False)  # recurse
                    w = trn.vex(S)
                    theta = vec.norm(w)
                    skw = trn.skew(w / theta)
                    Ginv = np.eye(3) / theta - skw / 2 + (1 / theta - 1 / np.tan(theta / 2) / 2) * skw @ skw
                    v = Ginv @ t
                return trn.rt2m(skw, v) * theta

        elif isrot(T, check=check):
            # deal with rotation matrix
            R = T
            if trn.iseye(R):
                # matrix is identity
                return np.zeros((3, 3))
            elif abs(np.trace(R) + 1) < 100 * _eps:
                # check for trace = -1
                #   rotation by +/- pi, +/- 3pi etc.
                diagonal = R.diagonal()
                k = diagonal.argmax()
                mx = diagonal[k]
                I = np.eye(3)
                col = R[:, k] + I[:, k]
                w = col / np.sqrt(2 * (1 + mx))
                theta = math.pi
                return trn.skew(w * theta)
            else:
                # general case
                theta = np.arccos((np.trace(R) - 1) / 2)
                skw = (R - R.T) / 2 / np.sin(theta)
                return skw * theta
        else:
            raise ValueError("Expect SO(3) or SE(3) matrix")

    # ---------------------------------------------------------------------------------------#

    def trexp(S, theta=None):
        """
        Exponential of so(3) or se(3) matrix

        :param S: so(3), se(3) matrix or equivalent velctor
        :type T: numpy.ndarray, shape=(3,3), (3,), (4,4), or (6,)
        :param theta: motion
        :type theta: float
        :return: 3x3 or 4x4 matrix exponential in SO(3) or SE(3)
        :rtype: numpy.ndarray, shape=(3,3) or (4,4)

        An efficient closed-form solution of the matrix exponential for arguments
        that are so(3) or se(3).

        For so(3) the results is an SO(3) rotation matrix:

        - ``trexp(S)`` is the matrix exponential of the so(3) element ``S`` which is a 3x3
           skew-symmetric matrix.
        - ``trexp(S, THETA)`` as above but for an so(3) motion of S*THETA, where ``S`` is
          unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude
          given by ``THETA``.
        - ``trexp(W)`` is the matrix exponential of the so(3) element ``W`` expressed as
          a 3-vector (array_like).
        - ``trexp(W, THETA)`` as above but for an so(3) motion of W*THETA where ``W`` is a
          unit-norm vector representing a rotation axis and a rotation magnitude
          given by ``THETA``. ``W`` is expressed as a 3-vector (array_like).

        For se(3) the results is an SE(3) homogeneous transformation matrix:

        - ``trexp(SIGMA)`` is the matrix exponential of the se(3) element ``SIGMA`` which is
          a 4x4 augmented skew-symmetric matrix.
        - ``trexp(SIGMA, THETA)`` as above but for an se(3) motion of SIGMA*THETA, where ``SIGMA``
          must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
          matrix.
        - ``trexp(TW)`` is the matrix exponential of the se(3) element ``TW`` represented as
          a 6-vector which can be considered a screw motion.
        - ``trexp(TW, THETA)`` as above but for an se(3) motion of TW*THETA, where ``TW``
          must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric
          matrix.

         :seealso: :func:`~trlog, :func:`~spatialmath.base.transforms2d.trexp2`
        """

        if argcheck.ismatrix(S, (4, 4)) or argcheck.isvector(S, 6):
            # se(3) case
            if argcheck.ismatrix(S, (4, 4)):
                # augmentented skew matrix
                tw = trn.vexa(S)
            else:
                # 6 vector
                tw = argcheck.getvector(S)

            if vec.iszerovec(tw):
                return np.eye(4)

            if theta is None:
                (tw, theta) = vec.unittwist_norm(tw)
            else:
                if theta == 0:
                    return np.eye(4)
                else:
                    assert vec.isunittwist(tw), 'If theta is specified S must be a unit twist'

            t = tw[0:3]
            w = tw[3:6]

            R = trn._rodrigues(w, theta)

            skw = trn.skew(w)
            V = np.eye(3) * theta + (1.0 - math.cos(theta)) * skw + (theta - math.sin(theta)) * skw @ skw

            return trn.rt2tr(R, V@t)

        elif argcheck.ismatrix(S, (3, 3)) or argcheck.isvector(S, 3):
            # so(3) case
            if argcheck.ismatrix(S, (3, 3)):
                # skew symmetric matrix
                w = trn.vex(S)
            else:
                # 3 vector
                w = argcheck.getvector(S)

            if theta is not None:
                assert vec.isunitvec(w), 'If theta is specified S must be a unit twist'

            # do Rodrigues' formula for rotation
            return trn._rodrigues(w, theta)
        else:
            raise ValueError(" First argument must be SO(3), 3-vector, SE(3) or 6-vector")

    def trnorm(T):
        """
        Normalize an SO(3) or SE(3) matrix

        :param T: SO(3) or SE(3) matrix
        :type T1: np.ndarray, shape=(3,3) or (4,4)
        :param T1: second SE(3) matrix
        :return: SO(3) or SE(3) matrix
        :rtype: np.ndarray, shape=(3,3) or (4,4)

        - ``trnorm(R)`` is guaranteed to be a proper orthogonal matrix rotation
          matrix (3x3) which is "close" to the input matrix R (3x3). If R
          = [N,O,A] the O and A vectors are made unit length and the normal vector
          is formed from N = O x A, and then we ensure that O and A are orthogonal
          by O = A x N.

        - ``trnorm(T)`` as above but the rotational submatrix of the homogeneous
          transformation T (4x4) is normalised while the translational part is
          unchanged.

        Notes:

        - Only the direction of A (the z-axis) is unchanged.
        - Used to prevent finite word length arithmetic causing transforms to
          become 'unnormalized'.
        """

        assert ishom(T) or isrot(T), 'expecting 3x3 or 4x4 hom xform'

        o = T[:3,1]
        a = T[:3,2]

        n = np.cross(o, a)        # N = O x A
        o = np.cross(a, n)        # (a)];
        R = np.stack((vec.unitvec(n), vec.unitvec(o), vec.unitvec(a)), axis=1)

        if ishom(T):
            return trn.rt2tr( R, T[:3,3] )
        else:
            return R

    def trinterp(T0, T1=None, s=None):
        """
        Interpolate SE(3) matrices

        :param T0: first SE(3) matrix
        :type T0: np.ndarray, shape=(4,4)
        :param T1: second SE(3) matrix
        :type T1: np.ndarray, shape=(4,4)
        :param s: interpolation coefficient, range 0 to 1
        :type s: float
        :return: SE(3) matrix
        :rtype: np.ndarray, shape=(4,4)

        - ``trinterp(T0, T1, S)`` is a homogeneous transform (4x4) interpolated
          between T0 when S=0 and T1 when S=1.  T0 and T1 are both homogeneous
          transforms (4x4).

        - ``trinterp(T1, S)`` as above but interpolated between the identity matrix
          when S=0 to T1 when S=1.

        Notes:

        - Rotation is interpolated using quaternion spherical linear interpolation (slerp).

        :seealso: :func:`spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms3d.trinterp2`
        """

        assert 0 <= s <= 1, 's outside interval [0,1]'

        if T1 is None:
            #	TRINTERP(T, s)

            q0 = quat.r2q(trn.t2r(T0))
            p0 = transl(T0)

            qr = quat.slerp(quat.eye(), q0, s)
            pr = s * p0
        else:
            #	TRINTERP(T0, T1, s)

            q0 = quat.r2q(trn.t2r(T0))
            q1 = quat.r2q(trn.t2r(T1))

            p0 = transl(T0)
            p1 = transl(T1)

            qr = quat.slerp(q0, q1, s)
            pr = p0 * (1 - s) + s * p1;

        return trn.rt2tr(quat.q2r(qr), pr)

    def delta2tr(d):
        r"""
        Convert differential motion to SE(3)

        :param d: differential motion as a 6-vector
        :type d: array_like
        :return: SE(3) matrix
        :rtype: np.ndarray, shape=(4,4)

        ``T = delta2tr(d)`` is an SE(3) matrix representing differential
        motion :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`.

        Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.

        :seealso: :func:`~tr2delta`
        """

        return np.eye(4,4) + trn.skewa(d)

    def trinv(T):
        r"""
        Invert an SE(3) matrix

        :param T: an SE(3) matrix
        :type T: np.ndarray, shape=(4,4)
        :return: SE(3) matrix
        :rtype: np.ndarray, shape=(4,4)

        Computes an efficient inverse of an SE(3) matrix:

        :math:`\begin{pmatrix} {\bf R} & t \\ 0\,0\,0 & 1 \end{pmatrix}^{-1} =  \begin{pmatrix} {\bf R}^T & -{\bf R}^T t \\ 0\,0\, 0 & 1 \end{pmatrix}`

        """
        assert ishom(T), 'expecting SE(3) matrix'
        (R, t) = trn.tr2rt(T)
        return trn.rt2tr(R.T, -R.T@t)

    def tr2delta(T0, T1=None):
        """
        Difference of SE(3) matrices as differential motion

        :param T0: first SE(3) matrix
        :type T0: np.ndarray, shape=(4,4)
        :param T1: second SE(3) matrix
        :type T1: np.ndarray, shape=(4,4)
        :return: Sdifferential motion as a 6-vector
        :rtype: np.ndarray, shape=(6,)

        - ``tr2delta(T0, T1)`` is the differential motion (6x1) corresponding to
          infinitessimal motion (in the T0 frame) from pose T0 to T1 which are SE(3) matrices.

        - ``tr2delta(T)`` as above but the motion is from the world frame to the pose represented by T.

        The vector :math:`d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z`
        represents infinitessimal translation and rotation, and is an approximation to the
        instantaneous spatial velocity multiplied by time step.

        Notes:

        - D is only an approximation to the motion T, and assumes
          that T0 ~ T1 or T ~ eye(4,4).
        - Can be considered as an approximation to the effect of spatial velocity over a
          a time interval, average spatial velocity multiplied by time.

        Reference: Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p67.

        :seealso: :func:`~delta2tr`
        """

        if T1 is None:
            # tr2delta(T)

            assert ishom(T0), 'expecting SE(3) matrix'
            Td = T0

        else:
            #  incremental transformation from T0 to T1 in the T0 frame
            Td = trinv(T0) @ T1

        return np.r_[transl(Td), trn.vex(trn.t2r(Td) - np.eye(3))]

    def tr2jac(T, samebody=False):
        """
        SE(3) adjoint

        :param T: an SE(3) matrix
        :type T: np.ndarray, shape=(4,4)
        :return: adjoint matrix
        :rtype: np.ndarray, shape=(6,6)

        Computes an adjoint matrix that maps spatial velocity between two frames defined by
        an SE(3) matrix.  It acts like a Jacobian matrix.

        - ``tr2jac(T)`` is a Jacobian matrix (6x6) that maps spatial velocity or
          differential motion from frame {A} to frame {B} where the pose of {B}
          relative to {A} is represented by the homogeneous transform T = :math:`{}^A {\bf T}_B`.

        - ``tr2jac(T, True)`` as above but for the case when frame {A} to frame {B} are both
          attached to the same moving body.
        """

        assert ishom(T), 'expecting an SE(3) matrix'
        Z = np.zeros((3,3))

        if samebody:
            (R,t) = trn.tr2rt(T)
            return np.block([[R.T, (trn.skew(t)@R).T], [Z, R.T]])
        else:
            R = trn.t2r(T);
            return np.block([[R.T, Z], [Z, R.T]])

    def trprint(T, orient='rpy/zyx', label=None, file=sys.stdout, fmt='{:8.2g}', unit='deg'):
        """
        Compact display of SO(3) or SE(3) matrices

        :param T: matrix to format
        :type T: numpy.ndarray, shape=(3,3) or (4,4)
        :param label: text label to put at start of line
        :type label: str
        :param orient: 3-angle convention to use
        :type orient: str
        :param file: file to write formatted string to. [default, stdout]
        :type file: str
        :param fmt: conversion format for each number
        :type fmt: str
        :param unit: angular units: 'rad' [default], or 'deg'
        :type unit: str
        :return: optional formatted string
        :rtype: str

        The matrix is formatted and written to ``file`` or if ``file=None`` then the
        string is returned.

       - ``trprint(R)`` displays the SO(3) rotation matrix in a compact
          single-line format:

            [LABEL:] ORIENTATION UNIT

        - ``trprint(T)`` displays the SE(3) homogoneous transform in a compact
          single-line format:

            [LABEL:] [t=X, Y, Z;] ORIENTATION UNIT

        Orientation is expressed in one of several formats:

        - 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order [default]
        - 'rpy/yxz' roll-pitch-yaw angles in YXZ axis order
        - 'rpy/zyx' roll-pitch-yaw angles in ZYX axis order
        - 'eul' Euler angles in ZYZ axis order
        - 'angvec' angle and axis

        Example:

        >>> T = transl(1,2,3) @ rpy2tr(10, 20, 30, 'deg')
        >>> trprint(T, file=None, label='T')
        'T: t =        1,        2,        3; rpy/zyx =       10,       20,       30 deg'
        >>> trprint(T, file=None, label='T', orient='angvec')
        'T: t =        1,        2,        3; angvec = (      56 deg |     0.12,     0.62,     0.78)'
        >>> trprint(T, file=None, label='T', orient='angvec', fmt='{:8.4g}')
        'T: t =        1,        2,        3; angvec = (   56.04 deg |    0.124,   0.6156,   0.7782)'

        Notes:

         - If the 'rpy' option is selected, then the particular angle sequence can be
           specified with the options 'xyz' or 'yxz' which are passed through to ``tr2rpy``.
           'zyx' is the default.
         - Default formatting is for readable columns of data

        :seealso: :func:`~spatialmath.base.transforms2d.trprint2`, :func:`~tr2eul`, :func:`~tr2rpy`, :func:`~tr2angvec`
        """

        s = ''

        if label is not None:
            s += '{:s}: '.format(label)

        # print the translational part if it exists
        if ishom(T):
            s += 't = {};'.format(_vec2s(fmt, transl(T)))

        # print the angular part in various representations

        a = orient.split('/')
        if a[0] == 'rpy':
            if len(a) == 2:
                seq = a[1]
            else:
                seq = None
            angles = tr2rpy(T, order=seq, unit=unit)
            s += ' {} = {} {}'.format(orient, _vec2s(fmt, angles), unit)

        elif a[0].startswith('eul'):
            angles = tr2eul(T, unit)
            s += ' eul = {} {}'.format(_vec2s(fmt, angles), unit)

        elif a[0] == 'angvec':
            pass
            # as a vector and angle
            (theta, v) = tr2angvec(T, unit)
            if theta == 0:
                s += ' R = nil'
            else:
                s += ' angvec = ({} {} | {})'.format(fmt.format(theta), unit, _vec2s(fmt, v))
        else:
            raise ValueError('bad orientation format')

        if file:
            print(s, file=file)
        else:
            return s

    def _vec2s(fmt, v):
        v = [x if np.abs(x) > 100 * _eps else 0.0 for x in v]
        return ', '.join([fmt.format(x) for x in v])

    try:
        import matplotlib.pyplot as plt
        from mpl_toolkits.mplot3d import Axes3D
        _matplotlib_exists = True

    except BaseException:  # pragma: no cover
        def trplot(*args, **kwargs):
            print('** trplot: no plot produced -- matplotlib not installed')
        _matplotlib_exists = False

    if _matplotlib_exists:
        def trplot(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y', 'Z'], length=1, arrow=True, projection='ortho', rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs):
            """
            Plot a 3D coordinate frame

            :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
            :type: numpy.ndarray, shape=(3,3) or (4,4)
            :param axes: the axes to plot into, defaults to current axes
            :type axes: Axes3D reference
            :param dims: dimension of plot volume as [xmin, xmax, ymin, ymax,zmin, zmax].
                         If dims is [min, max] those limits are applied to the x-, y- and z-axes.
            :type dims: array_like
            :param color: color of the lines defining the frame
            :type color: str
            :param textcolor: color of text labels for the frame, default color of lines above
            :type textcolor: str
            :param frame: label the frame, name is shown below the frame and as subscripts on the frame axis labels
            :type frame: str
            :param labels: labels for the axes, defaults to X, Y and Z
            :type labels: 3-tuple of strings
            :param length: length of coordinate frame axes, default 1
            :type length: float
            :param arrow: show arrow heads, default True
            :type arrow: bool
            :param wtl: width-to-length ratio for arrows, default 0.2
            :type wtl: float
            :param rviz: show Rviz style arrows, default False
            :type rviz: bool
            :param projection: 3D projection: ortho [default] or persp
            :type projection: str
            :param width: width of lines, default 1
            :type width: float
            :param d1: distance of frame axis label text from origin, default 1.15
            :type d2: distance of frame label text from origin, default 0.05

            Adds a 3D coordinate frame represented by the SO(3) or SE(3) matrix to the current axes.

            - If no current figure, one is created
            - If current figure, but no axes, a 3d Axes is created

            Examples:

                 trplot(T, frame='A')
                 trplot(T, frame='A', color='green')
                 trplot(T1, 'labels', 'NOA');

            """

            # TODO
            # animation
            # anaglyph

            # check input types
            if isrot(T, check=True):
                T = trn.r2t(T)
            else:
                assert ishom(T, check=True)

            if axes is None:
                # create an axes
                fig = plt.gcf()
                if fig.axes == []:
                    # no axes in the figure, create a 3D axes
                    ax = fig.add_subplot(111, projection='3d', proj_type=projection)
                    ax.autoscale(enable=True, axis='both')

                    # ax.set_aspect('equal')
                    ax.set_xlabel(labels[0])
                    ax.set_ylabel(labels[1])
                    ax.set_zlabel(labels[2])
                else:
                    # reuse an existing axis
                    ax = plt.gca()
            else:
                ax = axes

            if dims is not None:
                if len(dims) == 2:
                    dims = dims * 3
                ax.set_xlim(dims[0:2])
                ax.set_ylim(dims[2:4])
                ax.set_zlim(dims[4:6])

            # create unit vectors in homogeneous form
            o = T @ np.array([0, 0, 0, 1])
            x = T @ np.array([1, 0, 0, 1]) * length
            y = T @ np.array([0, 1, 0, 1]) * length
            z = T @ np.array([0, 0, 1, 1]) * length

            # draw the axes

            if rviz:
                ax.plot([o[0], x[0]], [o[1], x[1]], [o[2], x[2]], color='red', linewidth=5 * width)
                ax.plot([o[0], y[0]], [o[1], y[1]], [o[2], y[2]], color='lime', linewidth=5 * width)
                ax.plot([o[0], z[0]], [o[1], z[1]], [o[2], z[2]], color='blue', linewidth=5 * width)
            elif arrow:
                ax.quiver(o[0], o[1], o[2], x[0] - o[0], x[1] - o[1], x[2] - o[2], arrow_length_ratio=wtl, linewidth=width, facecolor=color, edgecolor=color)
                ax.quiver(o[0], o[1], o[2], y[0] - o[0], y[1] - o[1], y[2] - o[2], arrow_length_ratio=wtl, linewidth=width, facecolor=color, edgecolor=color)
                ax.quiver(o[0], o[1], o[2], z[0] - o[0], z[1] - o[1], z[2] - o[2], arrow_length_ratio=wtl, linewidth=width, facecolor=color, edgecolor=color)
                # plot an invisible point at the end of each arrow to allow auto-scaling to work
                ax.scatter(xs=[o[0], x[0], y[0], z[0]], ys=[o[1], x[1], y[1], z[1]], zs=[o[2], x[2], y[2], z[2]], s=[20, 0, 0, 0])
            else:
                ax.plot([o[0], x[0]], [o[1], x[1]], [o[2], x[2]], color=color, linewidth=width)
                ax.plot([o[0], y[0]], [o[1], y[1]], [o[2], y[2]], color=color, linewidth=width)
                ax.plot([o[0], z[0]], [o[1], z[1]], [o[2], z[2]], color=color, linewidth=width)

            # label the frame
            if frame:
                if textcolor is not None:
                    color = textcolor

                o1 = T @ np.array([-d1, -d1, -d1, 1])
                ax.text(o1[0], o1[1], o1[2], r'$\{' + frame + r'\}$', color=color, verticalalignment='top', horizontalalignment='center')

                # add the labels to each axis

                x = (x - o) * d2 + o
                y = (y - o) * d2 + o
                z = (z - o) * d2 + o

                ax.text(x[0], x[1], x[2], "$%c_{%s}$" % (labels[0], frame), color=color, horizontalalignment='center', verticalalignment='center')
                ax.text(y[0], y[1], y[2], "$%c_{%s}$" % (labels[1], frame), color=color, horizontalalignment='center', verticalalignment='center')
                ax.text(z[0], z[1], z[2], "$%c_{%s}$" % (labels[2], frame), color=color, horizontalalignment='center', verticalalignment='center')

        from spatialmath.base import animate as animate

        def tranimate(T, **kwargs):
            """
            Animate a 3D coordinate frame

            :param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
            :type: numpy.ndarray, shape=(3,3) or (4,4)
            :param nframes: number of steps in the animation [defaault 100]
            :type nframes: int
            :param repeat: animate in endless loop [default False]
            :type repeat: bool
            :param interval: number of milliseconds between frames [default 50]
            :type interval: int
            :param movie: name of file to write MP4 movie into
            :type movie: str

            Animates a 3D coordinate frame moving from the world frame to a frame represented by the SO(3) or SE(3) matrix to the current axes.

            - If no current figure, one is created
            - If current figure, but no axes, a 3d Axes is created

            Examples:

                 tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5])
                 tranimate(transl(1,2,3)@trotx(1), frame='A', arrow=False, dims=[0, 5], movie='spin.mp4')
            """
            anim = animate.Animate(**kwargs)
            anim.trplot(T, **kwargs)
            anim.run(**kwargs)

    if __name__ == '__main__':  # pragma: no cover
        import pathlib
        import os.path

        exec(open(os.path.join(pathlib.Path(__file__).parent.absolute(), "test_transforms.py")).read())