# Classes and functions

## Pose classes

### Pose in 2D

class `spatialmath.pose2d.``SO2`(arg=None, *, unit='rad', check=True)

  Bases: `spatialmath.super_pose.SMPose`

  `__init__`(arg=None, *, unit='rad', check=True)

    Construct new SO(2) object

    Parameters

      - unit (str, optional) – angular units ‘deg’ or ‘rad’ [default] if applicable

      - check (bool) – check for valid SO(2) elements if applicable, default to True

    Returns

      SO(2) rotation

    Return type

      SO2 instance

    - `SO2()` is an SO2 instance representing a null rotation – the identity matrix.

    - `SO2(theta)` is an SO2 instance representing a rotation by `theta` radians. If `theta` is array_like [theta1, theta2, … thetaN] then an SO2 instance containing a sequence of N rotations.

    - `SO2(theta, unit='deg')` is an SO2 instance representing a rotation by `theta` degrees. If `theta` is array_like [theta1, theta2, … thetaN] then an SO2 instance containing a sequence of N rotations.

    - `SO2(R)` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If `check` is `True` check the matrix belongs to SO(2).

    - `SO2([R1, R2, ... RN])` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix Ri which is a 2x2 numpy array. If `check` is `True` then check each matrix belongs to SO(2).

    - `SO2([X1, X2, ... XN])` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.

  classmethod `Rand`(*, range=[0, 6.283185307179586], unit='rad', N=1)

    Construct new SO(2) with random rotation

    Parameters

      - range (2-element array-like, optional) – rotation range, defaults to \([0, 2\pi)\).

      - unit (str, optional) – angular units as ‘deg or ‘rad’ [default]

      - N (int) – number of random rotations, defaults to 1

    Returns

      SO(2) rotation matrix

    Return type

      SO2 instance

    - `SO2.Rand()` is a random SO(2) rotation.

    - `SO2.Rand([-90, 90], unit='deg')` is a random SO(2) rotation between -90 and +90 degrees.

    - `SO2.Rand(N)` is a sequence of N random rotations.

    Rotations are uniform over the specified interval.

  classmethod `Exp`(S, check=True)

    Construct new SO(2) rotation matrix from so(2) Lie algebra

    Parameters

      - S (numpy ndarray) – element of Lie algebra so(2)

      - check (bool) – check that passed matrix is valid so(2), default True

    Returns

      SO(2) rotation matrix

    Return type

      SO2 instance

    - `SO2.Exp(S)` is an SO(2) rotation defined by its Lie algebra which is a 2x2 so(2) matrix (skew symmetric)

    Seealso

      `spatialmath.base.transforms2d.trexp()`, `spatialmath.base.transformsNd.skew()`

  static `isvalid`(x)

    Test if matrix is valid SO(2)

    Parameters

      x (numpy.ndarray) – matrix to test

    Returns

      True if the matrix is a valid element of SO(2), ie. it is a 2x2 orthonormal matrix with determinant of +1.

    Return type

      bool

    Seealso

      `isrot()`

  `inv`()

    Inverse of SO(2)

    Returns

      inverse rotation

    Return type

      SO2 instance

    - `x.inv()` is the inverse of x.

    Notes:

    > - for elements of SO(2) this is the transpose.

    > - if x contains a sequence, returns an SO2 with a sequence of inverses

  property `R`

    SO(2) or SE(2) as rotation matrix

    Returns

      rotational component

    Return type

      numpy.ndarray, shape=(2,2)

    `x.R` returns the rotation matrix, when x is SO2 or SE2. If len(x) is:

    - 1, return an ndarray with shape=(2,2)

    - N>1, return ndarray with shape=(N,2,2)

  `theta`(units='rad')

    SO(2) as a rotation angle

    Parameters

      unit (str, optional) – angular units ‘deg’ or ‘rad’ [default]

    Returns

      rotation angle

    Return type

      float or list

    `x.theta` is the rotation angle such that x is SO2(x.theta).

  `SE2`()

    Create SE(2) from SO(2)

    Returns

      SE(2) with same rotation but zero translation

    Return type

      SE2 instance

  property `A`

    Interal array representation (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – the pose object

    Returns

      The numeric array

    Return type

      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

  classmethod `Empty`()

    Construct a new pose object with zero items (superclass method)

    Parameters

      cls (SO2, SE2, SO3, SE3) – The pose subclass

    Returns

      a pose with zero values

    Return type

      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

  property `N`

    Dimension of the object’s group (superclass property)

    Returns

      dimension

    Return type

      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

  `__add__`(right)

    Overloaded `+` operator (superclass method)

    Parameters

      - left – left addend

      - right – right addend

    Returns

      sum

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar + Pose is handled by `__radd__`

    4. scalar addition is commutative

    5. 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]`

  `__eq__`(right)

    Overloaded `==` operator (superclass method)

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are equal

    Return type

      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]`

  `__mul__`(right)

    Overloaded `*` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar x Pose is handled by `__rmul__`

    4. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix

    5. 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:

    1. for the `SE2` and `SE3` case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.

  `__ne__`(right)

    Overloaded `!=` operator

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are not equal

    Return type

      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]`

  `__pow__`(n)

    Overloaded `**` operator (superclass method)

    Parameters

      n – pose

    Returns

      pose to the power n

    Raise all elements of pose to the specified power.

    - `X**n` raise all values in `X` to the power `n`

  `__sub__`(right)

    Overloaded `-` operator (superclass method)

    Parameters

      - left – left minuend

      - right – right subtrahend

    Returns

      difference

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar - Pose is handled by `__rsub__`

    4. 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]`

  `__truediv__`(right)

    Overloaded `/` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      product

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar multiplication is not a group operation so the result will be a matrix

    4. 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()`

  property `about`

    Succinct summary of object type and length (superclass property)

    Returns

      succinct summary

    Return type

      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]

  `animate`(*args, T0=None, **kwargs)

    Plot pose object as an animated coordinate frame (superclass method)

    Parameters

      **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

      `tranimate()`, `tranimate2()`

  `append`(x)

    Append a value to a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to append

    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

  `clear`() → None -- remove all items from S
  `extend`(x)

    Extend sequence of values of a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to extend

    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

  `insert`(i, value)

    Insert a value to a pose object (superclass method)

    Parameters

      - i (int) – element to insert value before

      - value (SO2, SE2, SO3, SE3 instance) – the value to insert

    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

  `interp`(s=None, T0=None)

    Interpolate pose (superclass method)

    Parameters

      - T0 (SO2, SE2, SO3, SE3) – initial pose

      - s (float or array_like) – interpolation coefficient, range 0 to 1

    Returns

      interpolated pose

    Return type

      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:

    1. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).

    Seealso

      `trinterp()`, `spatialmath.base.quaternions.slerp()`, `trinterp2()`

  property `isSE`

    Test if object belongs to SE(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SE2 or SE3

    Return type

      bool

  property `isSO`

    Test if object belongs to SO(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SO2 or SO3

    Return type

      bool

  `ishom`()

    Test if object belongs to SE(3) group (superclass method)

    Returns

      `True` if object is instance of SE3

    Return type

      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

  `ishom2`()

    Test if object belongs to SE(2) group (superclass method)

    Returns

      `True` if object is instance of SE2

    Return type

      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

  `isrot`()

    Test if object belongs to SO(3) group (superclass method)

    Returns

      `True` if object is instance of SO3

    Return type

      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

  `isrot2`()

    Test if object belongs to SO(2) group (superclass method)

    Returns

      `True` if object is instance of SO2

    Return type

      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

  `log`()

    Logarithm of pose (superclass method)

    Returns

      logarithm

    Return type

      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

      `trlog2()`, `trlog()`

  `norm`()

    Normalize pose (superclass method)

    Returns

      pose

    Return type

      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:

    1. Only the direction of A vector (the z-axis) is unchanged.

    2. Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.

    Seealso

      `trnorm()`, `trnorm2()`

  `plot`(*args, **kwargs)

    Plot pose object as a coordinate frame (superclass method)

    Parameters

      **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

      `trplot()`, `trplot2()`

  `pop`()

    Pop value of a pose object (superclass method)

    Returns

      the specific element of the pose

    Return type

      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

  `printline`(**kwargs)

    Print pose as a single line (superclass method)

    Parameters

      - label (str) – text label to put at start of line

      - file (str) – file to write formatted string to. [default, stdout]

      - fmt (str) – conversion format for each number as used by `format()`

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      optional formatted string

    Return type

      str

    For SO(3) or SE(3) also:

    Parameters

      orient (str) – 3-angle convention to use

    - `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

  `reverse`()

    S.reverse() – reverse IN PLACE

  property `shape`

    Shape of the object’s matrix representation (superclass property)

    Returns

      matrix shape

    Return type

      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)

class `spatialmath.pose2d.``SE2`(x=None, y=None, theta=None, *, unit='rad', check=True)

  Bases: `spatialmath.pose2d.SO2`

  `__init__`(x=None, y=None, theta=None, *, unit='rad', check=True)

    Construct new SE(2) object

    Parameters

      - unit (str, optional) – angular units ‘deg’ or ‘rad’ [default] if applicable

      - check (bool) – check for valid SE(2) elements if applicable, default to True

    Returns

      homogeneous rigid-body transformation matrix

    Return type

      SE2 instance

    - `SE2()` is an SE2 instance representing a null motion – the identity matrix

    - `SE2(x, y)` is an SE2 instance representing a pure translation of (`x`, `y`)

    - `SE2(t)` is an SE2 instance representing a pure translation of (`x`, `y`) where``t``=[x,y] is a 2-element array_like

    - `SE2(x, y, theta)` is an SE2 instance representing a translation of (`x`, `y`) and a rotation of `theta` radians

    - `SE2(x, y, theta, unit='deg')` is an SE2 instance representing a translation of (`x`, `y`) and a rotation of `theta` degrees

    - `SE2(t)` is an SE2 instance representing a translation of (`x`, `y`) and a rotation of `theta` where ``t``=[x,y,theta] is a 3-element array_like

    - `SE2(T)` is an SE2 instance with rigid-body motion described by the SE(2) matrix T which is a 3x3 numpy array. If `check` is `True` check the matrix belongs to SE(2).

    - `SE2([T1, T2, ... TN])` is an SE2 instance containing a sequence of N rigid-body motions, each described by an SE(2) matrix Ti which is a 3x3 numpy array. If `check` is `True` then check each matrix belongs to SE(2).

    - `SE2([X1, X2, ... XN])` is an SE2 instance containing a sequence of N rigid-body motions, where each Xi is an SE2 instance.

  classmethod `Rand`(*, xrange=[-1, 1], yrange=[-1, 1], trange=[0, 6.283185307179586], unit='rad', N=1)

    Construct a new random SE(2)

    Parameters

      - xrange (2-element sequence, optional) – x-axis range [min,max], defaults to [-1, 1]

      - yrange (2-element sequence, optional) – y-axis range [min,max], defaults to [-1, 1]

      - trange – theta range [min,max], defaults to \([0, 2\pi)\)

      - N (int) – number of random rotations, defaults to 1

    Returns

      homogeneous rigid-body transformation matrix

    Return type

      SE2 instance

    Return an SE2 instance with random rotation and translation.

    - `SE2.Rand()` is a random SE(2) rotation.

    - `SE2.Rand(N)` is an SE2 object containing a sequence of N random poses.

    Example, create random ten vehicles in the xy-plane:

        >>> x = SE3.Rand(N=10, xrange=[-2,2], yrange=[-2,2])
        >>> len(x)
        10

  classmethod `Exp`(S, check=True, se2=True)

    Construct a new SE(2) from se(2) Lie algebra

    Parameters

      - S (numpy ndarray) – element of Lie algebra se(2)

      - check (bool) – check that passed matrix is valid se(2), default True

      - se2 (bool) – input is an se(2) matrix (default True)

    Returns

      homogeneous transform matrix

    Return type

      SE2 instance

    - `SE2.Exp(S)` is an SE(2) rotation defined by its Lie algebra which is a 3x3 se(2) matrix (skew symmetric)

    - `SE2.Exp(t)` is an SE(2) rotation defined by a 3-element twist vector array_like (the unique elements of the se(2) skew-symmetric matrix)

    - `SE2.Exp(T)` is a sequence of SE(2) rigid-body motions defined by an Nx3 matrix of twist vectors, one per row.

    Note:

    - an input 3x3 matrix is ambiguous, it could be the first or third case above. In this case the argument `se2` is the decider.

    Seealso

      `spatialmath.base.transforms2d.trexp()`, `spatialmath.base.transformsNd.skew()`

  static `isvalid`(x)

    Test if matrix is valid SE(2)

    Parameters

      x (numpy.ndarray) – matrix to test

    Returns

      true if the matrix is a valid element of SE(2), ie. it is a 3x3 homogeneous rigid-body transformation matrix.

    Return type

      bool

    Seealso

      `ishom()`

  property `A`

    Interal array representation (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – the pose object

    Returns

      The numeric array

    Return type

      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

  classmethod `Empty`()

    Construct a new pose object with zero items (superclass method)

    Parameters

      cls (SO2, SE2, SO3, SE3) – The pose subclass

    Returns

      a pose with zero values

    Return type

      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

  property `N`

    Dimension of the object’s group (superclass property)

    Returns

      dimension

    Return type

      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

  property `R`

    SO(2) or SE(2) as rotation matrix

    Returns

      rotational component

    Return type

      numpy.ndarray, shape=(2,2)

    `x.R` returns the rotation matrix, when x is SO2 or SE2. If len(x) is:

    - 1, return an ndarray with shape=(2,2)

    - N>1, return ndarray with shape=(N,2,2)

  `SE2`()

    Create SE(2) from SO(2)

    Returns

      SE(2) with same rotation but zero translation

    Return type

      SE2 instance

  `__add__`(right)

    Overloaded `+` operator (superclass method)

    Parameters

      - left – left addend

      - right – right addend

    Returns

      sum

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar + Pose is handled by `__radd__`

    4. scalar addition is commutative

    5. 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]`

  `__eq__`(right)

    Overloaded `==` operator (superclass method)

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are equal

    Return type

      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]`

  `__mul__`(right)

    Overloaded `*` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar x Pose is handled by `__rmul__`

    4. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix

    5. 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:

    1. for the `SE2` and `SE3` case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.

  `__ne__`(right)

    Overloaded `!=` operator

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are not equal

    Return type

      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]`

  `__pow__`(n)

    Overloaded `**` operator (superclass method)

    Parameters

      n – pose

    Returns

      pose to the power n

    Raise all elements of pose to the specified power.

    - `X**n` raise all values in `X` to the power `n`

  `__sub__`(right)

    Overloaded `-` operator (superclass method)

    Parameters

      - left – left minuend

      - right – right subtrahend

    Returns

      difference

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar - Pose is handled by `__rsub__`

    4. 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]`

  `__truediv__`(right)

    Overloaded `/` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      product

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar multiplication is not a group operation so the result will be a matrix

    4. 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()`

  property `about`

    Succinct summary of object type and length (superclass property)

    Returns

      succinct summary

    Return type

      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]

  `animate`(*args, T0=None, **kwargs)

    Plot pose object as an animated coordinate frame (superclass method)

    Parameters

      **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

      `tranimate()`, `tranimate2()`

  `append`(x)

    Append a value to a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to append

    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

  `clear`() → None -- remove all items from S
  `extend`(x)

    Extend sequence of values of a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to extend

    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

  `insert`(i, value)

    Insert a value to a pose object (superclass method)

    Parameters

      - i (int) – element to insert value before

      - value (SO2, SE2, SO3, SE3 instance) – the value to insert

    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

  `interp`(s=None, T0=None)

    Interpolate pose (superclass method)

    Parameters

      - T0 (SO2, SE2, SO3, SE3) – initial pose

      - s (float or array_like) – interpolation coefficient, range 0 to 1

    Returns

      interpolated pose

    Return type

      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:

    1. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).

    Seealso

      `trinterp()`, `spatialmath.base.quaternions.slerp()`, `trinterp2()`

  property `isSE`

    Test if object belongs to SE(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SE2 or SE3

    Return type

      bool

  property `isSO`

    Test if object belongs to SO(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SO2 or SO3

    Return type

      bool

  `ishom`()

    Test if object belongs to SE(3) group (superclass method)

    Returns

      `True` if object is instance of SE3

    Return type

      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

  `ishom2`()

    Test if object belongs to SE(2) group (superclass method)

    Returns

      `True` if object is instance of SE2

    Return type

      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

  `isrot`()

    Test if object belongs to SO(3) group (superclass method)

    Returns

      `True` if object is instance of SO3

    Return type

      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

  `isrot2`()

    Test if object belongs to SO(2) group (superclass method)

    Returns

      `True` if object is instance of SO2

    Return type

      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

  `log`()

    Logarithm of pose (superclass method)

    Returns

      logarithm

    Return type

      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

      `trlog2()`, `trlog()`

  `norm`()

    Normalize pose (superclass method)

    Returns

      pose

    Return type

      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:

    1. Only the direction of A vector (the z-axis) is unchanged.

    2. Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.

    Seealso

      `trnorm()`, `trnorm2()`

  `plot`(*args, **kwargs)

    Plot pose object as a coordinate frame (superclass method)

    Parameters

      **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

      `trplot()`, `trplot2()`

  `pop`()

    Pop value of a pose object (superclass method)

    Returns

      the specific element of the pose

    Return type

      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

  `printline`(**kwargs)

    Print pose as a single line (superclass method)

    Parameters

      - label (str) – text label to put at start of line

      - file (str) – file to write formatted string to. [default, stdout]

      - fmt (str) – conversion format for each number as used by `format()`

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      optional formatted string

    Return type

      str

    For SO(3) or SE(3) also:

    Parameters

      orient (str) – 3-angle convention to use

    - `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

  `reverse`()

    S.reverse() – reverse IN PLACE

  property `shape`

    Shape of the object’s matrix representation (superclass property)

    Returns

      matrix shape

    Return type

      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)

  property `t`

    Translational component of SE(2)

    Parameters

      self (SE2 instance) – SE(2)

    Returns

      translational component

    Return type

      numpy.ndarray

    `x.t` is the translational vector component. If `len(x)` is:

    - 1, return an ndarray with shape=(2,)

    - N>1, return an ndarray with shape=(N,2)

  `theta`(units='rad')

    SO(2) as a rotation angle

    Parameters

      unit (str, optional) – angular units ‘deg’ or ‘rad’ [default]

    Returns

      rotation angle

    Return type

      float or list

    `x.theta` is the rotation angle such that x is SO2(x.theta).

  `xyt`()

    SE(2) as a configuration vector

    Returns

      An array \([x, y, \theta]\)

    Return type

      numpy.ndarray

    `x.xyt` is the rigidbody motion in minimal form as a translation and rotation expressed in vector form as \([x, y, \theta]\). If `len(x)` is:

    - 1, return an ndarray with shape=(3,)

    - N>1, return an ndarray with shape=(N,3)

  `inv`()

    Inverse of SE(2)

    Parameters

      self (SE2 instance) – pose

    Returns

      inverse

    Return type

      SE2

    Notes:

    > - for elements of SE(2) this takes into account the matrix structure \(T^{-1} = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]\)

    > - if x contains a sequence, returns an SE2 with a sequence of inverses

  `SE3`(z=0)

    Create SE(3) from SE(2)

    Parameters

      z (float) – default z coordinate, defaults to 0

    Returns

      SE(2) with same rotation but zero translation

    Return type

      SE2 instance

    “Lifts” 2D rigid-body motion to 3D, rotation in the xy-plane (about the z-axis) and z-coordinate is settable.

### Pose in 3D

class `spatialmath.pose3d.``SO3`(arg=None, *, check=True)

  Bases: `spatialmath.super_pose.SMPose`

  SO(3) subclass

  This subclass represents rotations in 3D space. Internally it is a 3x3 orthogonal matrix belonging to the group SO(3).

  `__init__`(arg=None, *, check=True)

    Construct new SO(3) object

    - `SO3()` is an SO3 instance representing null rotation – the identity matrix

    - `SO3(R)` is an SO3 instance with rotation matrix R which is a 3x3 numpy array representing an valid rotation matrix. If `check` is `True` check the matrix value.

    - `SO3([R1, R2, ... RN])` where each Ri is a 3x3 numpy array of rotation matrices, is an SO3 instance containing N rotations. If `check` is `True` then each matrix is checked for validity.

    - `SO3([X1, X2, ... XN])` where each Xi is an SO3 instance, is an SO3 instance containing N rotations.

    Seealso

      SMPose.pose_arghandler

  property `R`

    SO(3) or SE(3) as rotation matrix

    Returns

      rotational component

    Return type

      numpy.ndarray, shape=(3,3)

    `x.R` returns the rotation matrix, when x is SO3 or SE3. If len(x) is:

    - 1, return an ndarray with shape=(3,3)

    - N>1, return ndarray with shape=(N,3,3)

  property `n`

    Normal vector of SO(3) or SE(3)

    Returns

      normal vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the first column of the rotation submatrix, sometimes called the normal vector. Parallel to the x-axis of the frame defined by this pose.

  property `o`

    Orientation vector of SO(3) or SE(3)

    Returns

      orientation vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the second column of the rotation submatrix, sometimes called the orientation vector. Parallel to the y-axis of the frame defined by this pose.

  property `a`

    Approach vector of SO(3) or SE(3)

    Returns

      approach vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the third column of the rotation submatrix, sometimes called the approach vector. Parallel to the z-axis of the frame defined by this pose.

  `inv`()

    Inverse of SO(3)

    Parameters

      self (SE3 instance) – pose

    Returns

      inverse

    Return type

      SO2

    Returns the inverse, which for elements of SO(3) is the transpose.

  `eul`(unit='deg')

    SO(3) or SE(3) as Euler angles

    Parameters

      unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      3-vector of Euler angles

    Return type

      numpy.ndarray, shape=(3,)

    `x.eul` is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

    If len(x) is:

    - 1, return an ndarray with shape=(3,)

    - N>1, return ndarray with shape=(N,3)

    - ndarray with shape=(3,), if len(R) == 1

    - ndarray with shape=(N,3), if len(R) = N > 1

    Seealso

      `Eul()`, :`spatialmath.base.transforms3d.tr2eul()`

  `rpy`(unit='deg', order='zyx')

    SO(3) or SE(3) as roll-pitch-yaw angles

    Parameters

      - order (str) – angle sequence order, default to ‘zyx’

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      3-vector of roll-pitch-yaw angles

    Return type

      numpy.ndarray, shape=(3,)

    `x.rpy` is the roll-pitch-yaw angle representation of the rotation. The angles are a 3-vector \((r, p, y)\) which 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.

    If len(x) is:

    - 1, return an ndarray with shape=(3,)

    - N>1, return ndarray with shape=(N,3)

    Seealso

      `RPY()`, :`spatialmath.base.transforms3d.tr2rpy()`

  `Ad`()

    Adjoint of SO(3)

    Returns

      adjoint matrix

    Return type

      numpy.ndarray, shape=(6,6)

    - `SE3.Ad` is the 6x6 adjoint matrix

    Seealso

      Twist.ad.

  static `isvalid`(x)

    Test if matrix is valid SO(3)

    Parameters

      x (numpy.ndarray) – matrix to test

    Returns

      true if the matrix is a valid element of SO(3), ie. it is a 3x3 orthonormal matrix with determinant of +1.

    Return type

      bool

    Seealso

      `isrot()`

  classmethod `Rx`(theta, unit='rad')

    Construct a new SO(3) from X-axis rotation

    Parameters

      - theta (float or array_like) – rotation angle about the X-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    - `SE3.Rx(theta)` is an SO(3) rotation of `theta` radians about the x-axis

    - `SE3.Rx(theta, "deg")` as above but `theta` is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

    Example:

        >>> x = SO3.Rx(np.linspace(0, math.pi, 20))
        >>> len(x)
        20
        >>> x[7]
        SO3(array([[ 1.        ,  0.        ,  0.        ],
                   [ 0.        ,  0.40169542, -0.91577333],
                   [ 0.        ,  0.91577333,  0.40169542]]))

  classmethod `Ry`(theta, unit='rad')

    Construct a new SO(3) from Y-axis rotation

    Parameters

      - theta (float or array_like) – rotation angle about Y-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    - `SO3.Ry(theta)` is an SO(3) rotation of `theta` radians about the y-axis

    - `SO3.Ry(theta, "deg")` as above but `theta` is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

    Example:

        >>> x = SO3.Ry(np.linspace(0, math.pi, 20))
        >>> len(x)
        20
        >>> x[7]
        >>> x[7]
        SO3(array([[ 0.40169542,  0.        ,  0.91577333],
                   [ 0.        ,  1.        ,  0.        ],
                   [-0.91577333,  0.        ,  0.40169542]]))

  classmethod `Rz`(theta, unit='rad')

    Construct a new SO(3) from Z-axis rotation

    Parameters

      - theta (float or array_like) – rotation angle about Z-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    - `SO3.Rz(theta)` is an SO(3) rotation of `theta` radians about the z-axis

    - `SO3.Rz(theta, "deg")` as above but `theta` is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

    Example:

        >>> x = SE3.Rz(np.linspace(0, math.pi, 20))
        >>> len(x)
        20
        SO3(array([[ 0.40169542, -0.91577333,  0.        ],
                   [ 0.91577333,  0.40169542,  0.        ],
                   [ 0.        ,  0.        ,  1.        ]]))

  classmethod `Rand`(N=1)

    Construct a new SO(3) from random rotation

    Parameters

      N (int) – number of random rotations

    Returns

      SO(3) rotation matrix

    Return type

      SO3 instance

    - `SO3.Rand()` is a random SO(3) rotation.

    - `SO3.Rand(N)` is a sequence of N random rotations.

    Example:

        >>> x = SO3.Rand()
        >>> x
        SO3(array([[ 0.1805082 , -0.97959019,  0.08842995],
                   [-0.98357187, -0.17961408,  0.01803234],
                   [-0.00178104, -0.0902322 , -0.99591916]]))

    Seealso

      `spatialmath.quaternion.UnitQuaternion.Rand()`

  classmethod `Eul`(angles, *, unit='rad')

    Construct a new SO(3) from Euler angles

    Parameters

      - angles (array_like or numpy.ndarray with shape=(N,3)) – Euler angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    `SO3.Eul(angles)` is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, \theta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

    If `angles` is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles correponding to the rows of `angles`.

    Seealso

      `eul()`, `Eul()`, `spatialmath.base.transforms3d.eul2r()`

  classmethod `RPY`(angles, *, order='zyx', unit='rad')

    Construct a new SO(3) from roll-pitch-yaw angles

    Parameters

      - angles (array_like or numpy.ndarray with shape=(N,3)) – roll-pitch-yaw angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    `SO3.RPY(angles)` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((r, p, y)\)

      which 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.

    If `angles` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles correponding to the rows of angles.

    Seealso

      `rpy()`, `RPY()`, `spatialmath.base.transforms3d.rpy2r()`

  classmethod `OA`(o, a)

    Construct a new SO(3) from two vectors

    Parameters

      - o (array_like) – 3-vector parallel to Y- axis

      - a – 3-vector parallel to the Z-axis

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    `SO3.OA(O, A)` is an SO(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its 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.

    Notes:

    - Only the `A` vector is 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

    Seealso

      `spatialmath.base.transforms3d.oa2r()`

  classmethod `AngVec`(theta, v, *, unit='rad')

    Construct a new SO(3) rotation matrix from rotation angle and axis

    Parameters

      - theta (float) – rotation

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - v (array_like) – rotation axis, 3-vector

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    `SO3.AngVec(theta, V)` is an SO(3) rotation defined by a rotation of `THETA` about the vector `V`.

    If \(\theta \eq 0\) the result in an identity matrix, otherwise `V` must have a finite length, ie. \(|V| > 0\).

    Seealso

      `angvec()`, `spatialmath.base.transforms3d.angvec2r()`

  classmethod `Exp`(S, check=True, so3=True)

    Create an SO(3) rotation matrix from so(3)

    Parameters

      - S (numpy ndarray) – Lie algebra so(3)

      - check (bool) – check that passed matrix is valid so(3), default True

      - so3 (bool) – input is an so(3) matrix (default True)

    Returns

      SO(3) rotation

    Return type

      SO3 instance

    - `SO3.Exp(S)` is an SO(3) rotation defined by its Lie algebra which is a 3x3 so(3) matrix (skew symmetric)

    - `SO3.Exp(t)` is an SO(3) rotation defined by a 3-element twist vector (the unique elements of the so(3) skew-symmetric matrix)

    - `SO3.Exp(T)` is a sequence of SO(3) rotations defined by an Nx3 matrix of twist vectors, one per row.

    Note: - if :math:` heta eq 0` the result in an identity matrix - an input 3x3 matrix is ambiguous, it could be the first or third case above. In this

    > case the parameter so3 is the decider.

    Seealso

      `spatialmath.base.transforms3d.trexp()`, `spatialmath.base.transformsNd.skew()`

  property `A`

    Interal array representation (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – the pose object

    Returns

      The numeric array

    Return type

      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

  classmethod `Empty`()

    Construct a new pose object with zero items (superclass method)

    Parameters

      cls (SO2, SE2, SO3, SE3) – The pose subclass

    Returns

      a pose with zero values

    Return type

      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

  property `N`

    Dimension of the object’s group (superclass property)

    Returns

      dimension

    Return type

      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

  `__add__`(right)

    Overloaded `+` operator (superclass method)

    Parameters

      - left – left addend

      - right – right addend

    Returns

      sum

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar + Pose is handled by `__radd__`

    4. scalar addition is commutative

    5. 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]`

  `__eq__`(right)

    Overloaded `==` operator (superclass method)

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are equal

    Return type

      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]`

  `__mul__`(right)

    Overloaded `*` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar x Pose is handled by `__rmul__`

    4. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix

    5. 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:

    1. for the `SE2` and `SE3` case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.

  `__ne__`(right)

    Overloaded `!=` operator

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are not equal

    Return type

      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]`

  `__pow__`(n)

    Overloaded `**` operator (superclass method)

    Parameters

      n – pose

    Returns

      pose to the power n

    Raise all elements of pose to the specified power.

    - `X**n` raise all values in `X` to the power `n`

  `__sub__`(right)

    Overloaded `-` operator (superclass method)

    Parameters

      - left – left minuend

      - right – right subtrahend

    Returns

      difference

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar - Pose is handled by `__rsub__`

    4. 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]`

  `__truediv__`(right)

    Overloaded `/` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      product

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar multiplication is not a group operation so the result will be a matrix

    4. 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()`

  property `about`

    Succinct summary of object type and length (superclass property)

    Returns

      succinct summary

    Return type

      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]

  `animate`(*args, T0=None, **kwargs)

    Plot pose object as an animated coordinate frame (superclass method)

    Parameters

      **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

      `tranimate()`, `tranimate2()`

  `append`(x)

    Append a value to a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to append

    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

  `clear`() → None -- remove all items from S
  `extend`(x)

    Extend sequence of values of a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to extend

    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

  `insert`(i, value)

    Insert a value to a pose object (superclass method)

    Parameters

      - i (int) – element to insert value before

      - value (SO2, SE2, SO3, SE3 instance) – the value to insert

    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

  `interp`(s=None, T0=None)

    Interpolate pose (superclass method)

    Parameters

      - T0 (SO2, SE2, SO3, SE3) – initial pose

      - s (float or array_like) – interpolation coefficient, range 0 to 1

    Returns

      interpolated pose

    Return type

      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:

    1. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).

    Seealso

      `trinterp()`, `spatialmath.base.quaternions.slerp()`, `trinterp2()`

  property `isSE`

    Test if object belongs to SE(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SE2 or SE3

    Return type

      bool

  property `isSO`

    Test if object belongs to SO(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SO2 or SO3

    Return type

      bool

  `ishom`()

    Test if object belongs to SE(3) group (superclass method)

    Returns

      `True` if object is instance of SE3

    Return type

      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

  `ishom2`()

    Test if object belongs to SE(2) group (superclass method)

    Returns

      `True` if object is instance of SE2

    Return type

      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

  `isrot`()

    Test if object belongs to SO(3) group (superclass method)

    Returns

      `True` if object is instance of SO3

    Return type

      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

  `isrot2`()

    Test if object belongs to SO(2) group (superclass method)

    Returns

      `True` if object is instance of SO2

    Return type

      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

  `log`()

    Logarithm of pose (superclass method)

    Returns

      logarithm

    Return type

      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

      `trlog2()`, `trlog()`

  `norm`()

    Normalize pose (superclass method)

    Returns

      pose

    Return type

      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:

    1. Only the direction of A vector (the z-axis) is unchanged.

    2. Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.

    Seealso

      `trnorm()`, `trnorm2()`

  `plot`(*args, **kwargs)

    Plot pose object as a coordinate frame (superclass method)

    Parameters

      **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

      `trplot()`, `trplot2()`

  `pop`()

    Pop value of a pose object (superclass method)

    Returns

      the specific element of the pose

    Return type

      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

  `printline`(**kwargs)

    Print pose as a single line (superclass method)

    Parameters

      - label (str) – text label to put at start of line

      - file (str) – file to write formatted string to. [default, stdout]

      - fmt (str) – conversion format for each number as used by `format()`

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      optional formatted string

    Return type

      str

    For SO(3) or SE(3) also:

    Parameters

      orient (str) – 3-angle convention to use

    - `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

  `reverse`()

    S.reverse() – reverse IN PLACE

  property `shape`

    Shape of the object’s matrix representation (superclass property)

    Returns

      matrix shape

    Return type

      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)

class `spatialmath.pose3d.``SE3`(x=None, y=None, z=None, *, check=True)

  Bases: `spatialmath.pose3d.SO3`

  `__init__`(x=None, y=None, z=None, *, check=True)

    Construct new SE(3) object

    Parameters

      - x (float) – translation distance along the X-axis

      - y (float) – translation distance along the Y-axis

      - z (float) – translation distance along the Z-axis

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    - `SE3()` is a null motion – the identity matrix

    - `SE3(x, y, z)` is a pure translation of (x,y,z)

    - `SE3(T)` where T is a 4x4 numpy array representing an SE(3) matrix. If `check` is `True` check the matrix belongs to SE(3).

    - `SE3([T1, T2, ... TN])` where each Ti is a 4x4 numpy array representing an SE(3) matrix, is an SE3 instance containing N rotations. If `check` is `True` check the matrix belongs to SE(3).

    - `SE3([X1, X2, ... XN])` where each Xi is an SE3 instance, is an SE3 instance containing N rotations.

  property `t`

    Translational component of SE(3)

    Parameters

      self (SE3 instance) – SE(3)

    Returns

      translational component

    Return type

      numpy.ndarray

    `T.t` returns an:

    - ndarray with shape=(3,), if len(T) == 1

    - ndarray with shape=(N,3), if len(T) = N > 1

  `inv`()

    Inverse of SE(3)

    Returns

      inverse

    Return type

      SE3

    Returns the inverse taking into account its structure

    \(T = \left[ \begin{array}{cc} R & t \\ 0 & 1 \end{array} \right], T^{-1} = \left[ \begin{array}{cc} R^T & -R^T t \\ 0 & 1 \end{array} \right]\)

    Seealso

      `trinv()`

  `delta`(X2)

    Difference of SE(3)

    Parameters

      X1 (SE3) –

    Returns

      differential motion vector

    Return type

      numpy.ndarray, shape=(6,)

    - `X1.delta(T2)` is the differential motion (6x1) corresponding to infinitessimal motion (in the X1 frame) from pose X1 to X2.

    The vector \(d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z\) represents infinitessimal translation and rotation.

    Notes:

    - the displacement is only an approximation to the motion T, and assumes that X1 ~ X2.

    - 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

      `tr2delta()`

  static `isvalid`(x)

    Test if matrix is valid SE(3)

    Parameters

      x (numpy.ndarray) – matrix to test

    Returns

      true of the matrix is 4x4 and a valid element of SE(3), ie. it is an homogeneous transformation matrix.

    Return type

      bool

    Seealso

      `ishom()`

  classmethod `Rx`(theta, unit='rad')

    Create SE(3) pure rotation about the X-axis

    Parameters

      - theta (float) – rotation angle about X-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    - `SE3.Rx(THETA)` is an SO(3) rotation of THETA radians about the x-axis

    - `SE3.Rx(THETA, "deg")` as above but THETA is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

  classmethod `Ry`(theta, unit='rad')

    Create SE(3) pure rotation about the Y-axis

    Parameters

      - theta (float) – rotation angle about X-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    - `SE3.Ry(THETA)` is an SO(3) rotation of THETA radians about the y-axis

    - `SE3.Ry(THETA, "deg")` as above but THETA is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

  classmethod `Rz`(theta, unit='rad')

    Create SE(3) pure rotation about the Z-axis

    Parameters

      - theta (float) – rotation angle about Z-axis

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    - `SE3.Rz(THETA)` is an SO(3) rotation of THETA radians about the z-axis

    - `SE3.Rz(THETA, "deg")` as above but THETA is in degrees

    If `theta` is an array then the result is a sequence of rotations defined by consecutive elements.

  classmethod `Rand`(*, xrange=[-1, 1], yrange=[-1, 1], zrange=[-1, 1], N=1)

    Create a random SE(3)

    Parameters

      - xrange (2-element sequence, optional) – x-axis range [min,max], defaults to [-1, 1]

      - yrange (2-element sequence, optional) – y-axis range [min,max], defaults to [-1, 1]

      - zrange (2-element sequence, optional) – z-axis range [min,max], defaults to [-1, 1]

      - N (int) – number of random transforms

    Returns

      homogeneous transformation matrix

    Return type

      SE3 instance

    Return an SE3 instance with random rotation and translation.

    - `SE3.Rand()` is a random SE(3) translation.

    - `SE3.Rand(N)` is an SE3 object containing a sequence of N random poses.

    Seealso

      ~spatialmath.quaternion.UnitQuaternion.Rand

  classmethod `Eul`(angles, unit='rad')

    Create an SE(3) pure rotation from Euler angles

    Parameters

      - angles (array_like or numpy.ndarray with shape=(N,3)) – 3-vector of Euler angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    `SE3.Eul(ANGLES)` is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

    If `angles` is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles correponding to the rows of angles.

    Seealso

      `eul()`, `Eul()`, `spatialmath.base.transforms3d.eul2r()`

  classmethod `RPY`(angles, order='zyx', unit='rad')

    Create an SO(3) pure rotation from roll-pitch-yaw angles

    Parameters

      - angles (array_like or numpy.ndarray with shape=(N,3)) – 3-vector of roll-pitch-yaw angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    `SE3.RPY(ANGLES)` is an SE(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((r, p, y)\)

      which 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.

    If `angles` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles correponding to the rows of angles.

    Seealso

      `rpy()`, `RPY()`, `spatialmath.base.transforms3d.rpy2r()`

  classmethod `OA`(o, a)

    Create SE(3) pure rotation from two vectors

    Parameters

      - o (array_like) – 3-vector parallel to Y- axis

      - a – 3-vector parallel to the Z-axis

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    `SE3.OA(O, A)` is an SE(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its 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.

    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

      `spatialmath.base.transforms3d.oa2r()`

  classmethod `AngVec`(theta, v, *, unit='rad')

    Create an SE(3) pure rotation matrix from rotation angle and axis

    Parameters

      - theta (float) – rotation

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - v (array_like) – rotation axis, 3-vector

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    `SE3.AngVec(THETA, V)` is an SE(3) rotation defined by 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

      `angvec()`, `spatialmath.base.transforms3d.angvec2r()`

  classmethod `Exp`(S)

    Create an SE(3) rotation matrix from se(3)

    Parameters

      S (numpy ndarray) – Lie algebra se(3)

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    - `SE3.Exp(S)` is an SE(3) rotation defined by its Lie algebra which is a 3x3 se(3) matrix (skew symmetric)

    - `SE3.Exp(t)` is an SE(3) rotation defined by a 6-element twist vector (the unique elements of the se(3) skew-symmetric matrix)

    Seealso

      `spatialmath.base.transforms3d.trexp()`, `spatialmath.base.transformsNd.skew()`

  classmethod `Tx`(x)

    Create SE(3) translation along the X-axis

    Parameters

      theta (float) – translation distance along the X-axis

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    SE3.Tz(D)` is an SE(3) translation of D along the x-axis

  classmethod `Ty`(y)

    Create SE(3) translation along the Y-axis

    Parameters

      theta (float) – translation distance along the Y-axis

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    SE3.Tz(D)` is an SE(3) translation of D along the y-axis

  classmethod `Tz`(z)

    Create SE(3) translation along the Z-axis

    Parameters

      theta (float) – translation distance along the Z-axis

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    SE3.Tz(D)` is an SE(3) translation of D along the z-axis

  property `A`

    Interal array representation (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – the pose object

    Returns

      The numeric array

    Return type

      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

  `Ad`()

    Adjoint of SO(3)

    Returns

      adjoint matrix

    Return type

      numpy.ndarray, shape=(6,6)

    - `SE3.Ad` is the 6x6 adjoint matrix

    Seealso

      Twist.ad.

  classmethod `Delta`(d)

    Create SE(3) from diffential motion

    Parameters

      d (6-element array_like) – differential motion

    Returns

      4x4 homogeneous transformation matrix

    Return type

      SE3 instance

    - `T = delta2tr(d)` is an SE(3) representing differential motion \(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

      `delta()`, `delta2tr()`

  classmethod `Empty`()

    Construct a new pose object with zero items (superclass method)

    Parameters

      cls (SO2, SE2, SO3, SE3) – The pose subclass

    Returns

      a pose with zero values

    Return type

      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

  property `N`

    Dimension of the object’s group (superclass property)

    Returns

      dimension

    Return type

      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

  property `R`

    SO(3) or SE(3) as rotation matrix

    Returns

      rotational component

    Return type

      numpy.ndarray, shape=(3,3)

    `x.R` returns the rotation matrix, when x is SO3 or SE3. If len(x) is:

    - 1, return an ndarray with shape=(3,3)

    - N>1, return ndarray with shape=(N,3,3)

  `__add__`(right)

    Overloaded `+` operator (superclass method)

    Parameters

      - left – left addend

      - right – right addend

    Returns

      sum

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar + Pose is handled by `__radd__`

    4. scalar addition is commutative

    5. 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]`

  `__eq__`(right)

    Overloaded `==` operator (superclass method)

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are equal

    Return type

      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]`

  `__mul__`(right)

    Overloaded `*` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar x Pose is handled by `__rmul__`

    4. scalar multiplication is commutative but the result is not a group operation so the result will be a matrix

    5. 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:

    1. for the `SE2` and `SE3` case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.

  `__ne__`(right)

    Overloaded `!=` operator

    Parameters

      - left – left side of comparison

      - right – right side of comparison

    Returns

      poses are not equal

    Return type

      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]`

  `__pow__`(n)

    Overloaded `**` operator (superclass method)

    Parameters

      n – pose

    Returns

      pose to the power n

    Raise all elements of pose to the specified power.

    - `X**n` raise all values in `X` to the power `n`

  `__sub__`(right)

    Overloaded `-` operator (superclass method)

    Parameters

      - left – left minuend

      - right – right subtrahend

    Returns

      difference

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar - Pose is handled by `__rsub__`

    4. 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]`

  `__truediv__`(right)

    Overloaded `/` operator (superclass method)

    Parameters

      - left – left multiplicand

      - right – right multiplicand

    Returns

      product

    Raises

      ValueError – for incompatible arguments

    Returns

      matrix

    Return type

      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:

    1. Pose is `SO2`, `SE2`, `SO3` or `SE3` instance

    2. N is 2 for `SO2`, `SE2`; 3 for `SO3` or `SE3`

    3. scalar multiplication is not a group operation so the result will be a matrix

    4. 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()`

  property `a`

    Approach vector of SO(3) or SE(3)

    Returns

      approach vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the third column of the rotation submatrix, sometimes called the approach vector. Parallel to the z-axis of the frame defined by this pose.

  property `about`

    Succinct summary of object type and length (superclass property)

    Returns

      succinct summary

    Return type

      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]

  `animate`(*args, T0=None, **kwargs)

    Plot pose object as an animated coordinate frame (superclass method)

    Parameters

      **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

      `tranimate()`, `tranimate2()`

  `append`(x)

    Append a value to a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to append

    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

  `clear`() → None -- remove all items from S
  `eul`(unit='deg')

    SO(3) or SE(3) as Euler angles

    Parameters

      unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      3-vector of Euler angles

    Return type

      numpy.ndarray, shape=(3,)

    `x.eul` is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

    If len(x) is:

    - 1, return an ndarray with shape=(3,)

    - N>1, return ndarray with shape=(N,3)

    - ndarray with shape=(3,), if len(R) == 1

    - ndarray with shape=(N,3), if len(R) = N > 1

    Seealso

      `Eul()`, :`spatialmath.base.transforms3d.tr2eul()`

  `extend`(x)

    Extend sequence of values of a pose object (superclass method)

    Parameters

      x (SO2, SE2, SO3, SE3 instance) – the value to extend

    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

  `insert`(i, value)

    Insert a value to a pose object (superclass method)

    Parameters

      - i (int) – element to insert value before

      - value (SO2, SE2, SO3, SE3 instance) – the value to insert

    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

  `interp`(s=None, T0=None)

    Interpolate pose (superclass method)

    Parameters

      - T0 (SO2, SE2, SO3, SE3) – initial pose

      - s (float or array_like) – interpolation coefficient, range 0 to 1

    Returns

      interpolated pose

    Return type

      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:

    1. For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).

    Seealso

      `trinterp()`, `spatialmath.base.quaternions.slerp()`, `trinterp2()`

  property `isSE`

    Test if object belongs to SE(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SE2 or SE3

    Return type

      bool

  property `isSO`

    Test if object belongs to SO(n) group (superclass property)

    Parameters

      self (SO2, SE2, SO3, SE3 instance) – object to test

    Returns

      `True` if object is instance of SO2 or SO3

    Return type

      bool

  `ishom`()

    Test if object belongs to SE(3) group (superclass method)

    Returns

      `True` if object is instance of SE3

    Return type

      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

  `ishom2`()

    Test if object belongs to SE(2) group (superclass method)

    Returns

      `True` if object is instance of SE2

    Return type

      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

  `isrot`()

    Test if object belongs to SO(3) group (superclass method)

    Returns

      `True` if object is instance of SO3

    Return type

      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

  `isrot2`()

    Test if object belongs to SO(2) group (superclass method)

    Returns

      `True` if object is instance of SO2

    Return type

      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

  `log`()

    Logarithm of pose (superclass method)

    Returns

      logarithm

    Return type

      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

      `trlog2()`, `trlog()`

  property `n`

    Normal vector of SO(3) or SE(3)

    Returns

      normal vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the first column of the rotation submatrix, sometimes called the normal vector. Parallel to the x-axis of the frame defined by this pose.

  `norm`()

    Normalize pose (superclass method)

    Returns

      pose

    Return type

      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:

    1. Only the direction of A vector (the z-axis) is unchanged.

    2. Used to prevent finite word length arithmetic causing transforms to become ‘unnormalized’.

    Seealso

      `trnorm()`, `trnorm2()`

  property `o`

    Orientation vector of SO(3) or SE(3)

    Returns

      orientation vector

    Return type

      numpy.ndarray, shape=(3,)

    Is the second column of the rotation submatrix, sometimes called the orientation vector. Parallel to the y-axis of the frame defined by this pose.

  `plot`(*args, **kwargs)

    Plot pose object as a coordinate frame (superclass method)

    Parameters

      **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

      `trplot()`, `trplot2()`

  `pop`()

    Pop value of a pose object (superclass method)

    Returns

      the specific element of the pose

    Return type

      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

  `printline`(**kwargs)

    Print pose as a single line (superclass method)

    Parameters

      - label (str) – text label to put at start of line

      - file (str) – file to write formatted string to. [default, stdout]

      - fmt (str) – conversion format for each number as used by `format()`

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      optional formatted string

    Return type

      str

    For SO(3) or SE(3) also:

    Parameters

      orient (str) – 3-angle convention to use

    - `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

  `reverse`()

    S.reverse() – reverse IN PLACE

  `rpy`(unit='deg', order='zyx')

    SO(3) or SE(3) as roll-pitch-yaw angles

    Parameters

      - order (str) – angle sequence order, default to ‘zyx’

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      3-vector of roll-pitch-yaw angles

    Return type

      numpy.ndarray, shape=(3,)

    `x.rpy` is the roll-pitch-yaw angle representation of the rotation. The angles are a 3-vector \((r, p, y)\) which 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.

    If len(x) is:

    - 1, return an ndarray with shape=(3,)

    - N>1, return ndarray with shape=(N,3)

    Seealso

      `RPY()`, :`spatialmath.base.transforms3d.tr2rpy()`

  property `shape`

    Shape of the object’s matrix representation (superclass property)

    Returns

      matrix shape

    Return type

      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)

class `spatialmath.quaternion.``Quaternion`(s=None, v=None, check=True, norm=True)

  Bases: `collections.UserList`

  A quaternion is a compact method of representing a 3D rotation that has computational advantages including speed and numerical robustness.

  A quaternion has 2 parts, a scalar s, and a 3-vector v and is typically written:

    q = s <vx vy vz>

  `__init__`(s=None, v=None, check=True, norm=True)

    A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}. A quaternion can be considered as a rotation about a vector in space where q = cos (theta/2) sin(theta/2) <vx vy vz> where <vx vy vz> is a unit vector. :param s: scalar :param v: vector

  `append`(x)

    S.append(value) – append value to the end of the sequence

  property `s`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      real part of quaternion

    Return type

      float or numpy.ndarray

    - If the quaternion is of length one, a scalar float is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,) is returned.

  property `v`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      vector part of quaternion

    Return type

      numpy ndarray

    - If the quaternion is of length one, a numpy array shape=(3,) is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,3) is returned.

  property `vec`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      quaternion expressed as a vector

    Return type

      numpy ndarray

    - If the quaternion is of length one, a numpy array shape=(4,) is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,4) is returned.

  classmethod `pure`(v)
  property `conj`
  property `norm`

    Return the norm of this quaternion. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: number @return: the norm

  property `unit`

    Return an equivalent unit quaternion Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: quaternion @return: equivalent unit quaternion

  property `matrix`
  `inner`(other)
  `__eq__`(other)

    Return self==value.

  `__ne__`(other)

    Return self!=value.

  `__mul__`(right)

    multiply quaternion

    Parameters

      - left (Quaternion, UnitQuaternion, float) – left multiplicand

      - right – right multiplicand

    Returns

      product

    Return type

      Quaternion

    Raises

      ValueError

    Multiplicands | Product
    --- | ---
    left | right | type | result
    Quaternion | Quaternion | Quaternion | Hamilton product
    Quaternion | UnitQuaternion | Quaternion | Hamilton product
    Quaternion | scalar | Quaternion | scalar product

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left * right`
    1 | N | N | `prod[i] = left * right[i]`
    N | 1 | N | `prod[i] = left[i] * right`
    N | N | N | `prod[i] = left[i] * right[i]`
    N | M |  | `ValueError`

  `__pow__`(n)
  `__truediv__`(other)
  `__add__`(right)

    add quaternions

    Parameters

      - left (Quaternion, UnitQuaternion) – left addend

      - right (Quaternion, UnitQuaternion, float) – right addend

    Returns

      sum

    Return type

      Quaternion, UnitQuaternion

    Raises

      ValueError

    Operands | Sum
    --- | ---
    left | right | type | result
    Quaternion | Quaternion | Quaternion | elementwise sum
    Quaternion | UnitQuaternion | Quaternion | elementwise sum
    Quaternion | scalar | Quaternion | add to each element
    UnitQuaternion | Quaternion | Quaternion | elementwise sum
    UnitQuaternion | UnitQuaternion | Quaternion | elementwise sum
    UnitQuaternion | scalar | Quaternion | add to each element

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left + right`
    1 | N | N | `prod[i] = left + right[i]`
    N | 1 | N | `prod[i] = left[i] + right`
    N | N | N | `prod[i] = left[i] + right[i]`
    N | M |  | `ValueError`

    A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

  `__sub__`(right)

    subtract quaternions

    Parameters

      - left (Quaternion, UnitQuaternion) – left minuend

      - right (Quaternion, UnitQuaternion, float) – right subtahend

    Returns

      difference

    Return type

      Quaternion, UnitQuaternion

    Raises

      ValueError

    Operands | Difference
    --- | ---
    left | right | type | result
    Quaternion | Quaternion | Quaternion | elementwise sum
    Quaternion | UnitQuaternion | Quaternion | elementwise sum
    Quaternion | scalar | Quaternion | subtract from each element
    UnitQuaternion | Quaternion | Quaternion | elementwise sum
    UnitQuaternion | UnitQuaternion | Quaternion | elementwise sum
    UnitQuaternion | scalar | Quaternion | subtract from each element

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left - right`
    1 | N | N | `prod[i] = left - right[i]`
    N | 1 | N | `prod[i] = left[i] - right`
    N | N | N | `prod[i] = left[i] - right[i]`
    N | M |  | `ValueError`

    A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

  `clear`() → None -- remove all items from S
  `extend`(other)

    S.extend(iterable) – extend sequence by appending elements from the iterable

  `insert`(i, item)

    S.insert(index, value) – insert value before index

  `pop`([index]) → item -- remove and return item at index (default last).

    Raise IndexError if list is empty or index is out of range.

  `reverse`()

    S.reverse() – reverse IN PLACE

class `spatialmath.quaternion.``UnitQuaternion`(s=None, v=None, norm=True, check=True)

  Bases: `spatialmath.quaternion.Quaternion`

  A unit-quaternion is is a quaternion with unit length, that is \(s^2+v_x^2+v_y^2+v_z^2 = 1\).

  A unit-quaternion can be considered as a rotation \(\theta\) where \(q = \cos \theta/2 \sin \theta/2 <v_x v_y v_z>\).

  `__init__`(s=None, v=None, norm=True, check=True)

    Construct a UnitQuaternion object

    Parameters

      - norm (bool) – explicitly normalize the quaternion [default True]

      - check (bool) – explicitly check dimension of passed lists [default True]

    Returns

      new unit uaternion

    Return type

      UnitQuaternion

    Raises

      ValueError

    Single element quaternion:

    - `UnitQuaternion()` constructs the identity quaternion 1<0,0,0>

    - `UnitQuaternion(s, v)` constructs a unit quaternion with specified real `s` and `v` vector parts. `v` is a 3-vector given as a list, tuple, numpy.ndarray

    - `UnitQuaternion(v)` constructs a unit quaternion with specified elements from `v` which is a 4-vector given as a list, tuple, numpy.ndarray

    - `UnitQuaternion(R)` constructs a unit quaternion from an orthonormal rotation matrix given as a 3x3 numpy.ndarray. If `check` is True test the matrix for orthogonality.

    Multi-element quaternion:

    - `UnitQuaternion(V)` constructs a unit quaternion list with specified elements from `V` which is an Nx4 numpy.ndarray, each row is a quaternion. If `norm` is True explicitly normalize each row.

    - `UnitQuaternion(L)` constructs a unit quaternion list from a list of 4-element numpy.ndarrays. If `check` is True test each element of the list is a 4-vector. If `norm` is True explicitly normalize each vector.

  property `R`
  property `vec3`
  classmethod `Rx`(angle, unit='rad')

    Construct a UnitQuaternion object representing rotation about X-axis

    Parameters

      - angle – rotation angle

      - unit (str) – rotation unit ‘rad’ [default] or ‘deg’

    Returns

      new unit-quaternion

    Return type

      UnitQuaternion

    - `UnitQuaternion(theta)` constructs a unit quaternion representing a rotation of theta radians about the X-axis.

    - `UnitQuaternion(theta, 'deg')` constructs a unit quaternion representing a rotation of theta degrees about the X-axis.

  classmethod `Ry`(angle, unit='rad')

    Construct a UnitQuaternion object representing rotation about Y-axis

    Parameters

      - angle – rotation angle

      - unit (str) – rotation unit ‘rad’ [default] or ‘deg’

    Returns

      new unit-quaternion

    Return type

      UnitQuaternion

    - `UnitQuaternion(theta)` constructs a unit quaternion representing a rotation of theta radians about the Y-axis.

    - `UnitQuaternion(theta, 'deg')` constructs a unit quaternion representing a rotation of theta degrees about the Y-axis.

  classmethod `Rz`(angle, unit='rad')

    Construct a UnitQuaternion object representing rotation about Z-axis

    Parameters

      - angle – rotation angle

      - unit (str) – rotation unit ‘rad’ [default] or ‘deg’

    Returns

      new unit-quaternion

    Return type

      UnitQuaternion

    - `UnitQuaternion(theta)` constructs a unit quaternion representing a rotation of theta radians about the Z-axis.

    - `UnitQuaternion(theta, 'deg')` constructs a unit quaternion representing a rotation of theta degrees about the Z-axis.

  classmethod `Rand`(N=1)

    Create SO(3) with random rotation

    Parameters

      N (int) – number of random rotations

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    - `SO3.Rand()` is a random SO(3) rotation.

    - `SO3.Rand(N)` is an SO3 object containing a sequence of N random rotations.

    Seealso

      `spatialmath.quaternion.UnitQuaternion.Rand()`

  classmethod `Eul`(angles, *, unit='rad')

    Create an SO(3) rotation from Euler angles

    Parameters

      - angles (array_like) – 3-vector of Euler angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    `SO3.Eul(ANGLES)` is an SO(3) rotation defined by a 3-vector of Euler angles \((\phi, heta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.

    Seealso

      `eul()`, `Eul()`, `spatialmath.base.transforms3d.eul2r()`

  classmethod `RPY`(angles, *, order='zyx', unit='rad')

    Create an SO(3) rotation from roll-pitch-yaw angles

    Parameters

      - angles (array_like) – 3-vector of roll-pitch-yaw angles

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    `SO3.RPY(ANGLES)` is an SO(3) rotation defined by a 3-vector of roll, pitch, yaw angles \((r, p, y)\)

      which 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.

    Seealso

      `rpy()`, `RPY()`, `spatialmath.base.transforms3d.rpy2r()`

  classmethod `OA`(o, a)

    Create SO(3) rotation from two vectors

    Parameters

      - o (array_like) – 3-vector parallel to Y- axis

      - a – 3-vector parallel to the Z-axis

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    `SO3.OA(O, A)` is an SO(3) rotation defined in terms of vectors parallel to the Y- and Z-axes of its 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.

    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

      `spatialmath.base.transforms3d.oa2r()`

  classmethod `AngVec`(theta, v, *, unit='rad')

    Create an SO(3) rotation matrix from rotation angle and axis

    Parameters

      - theta (float) – rotation

      - unit (str) – angular units: ‘rad’ [default], or ‘deg’

      - v (array_like) – rotation axis, 3-vector

    Returns

      3x3 rotation matrix

    Return type

      SO3 instance

    `SO3.AngVec(THETA, V)` is an SO(3) rotation defined by 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

      `angvec()`, `spatialmath.base.transforms3d.angvec2r()`

  classmethod `Omega`(w)
  classmethod `Vec3`(vec)
  property `inv`
  classmethod `omega`(w)
  static `qvmul`(qv1, qv2)
  `__add__`(right)

    add quaternions

    Parameters

      - left (Quaternion, UnitQuaternion) – left addend

      - right (Quaternion, UnitQuaternion, float) – right addend

    Returns

      sum

    Return type

      Quaternion, UnitQuaternion

    Raises

      ValueError

    Operands | Sum
    --- | ---
    left | right | type | result
    Quaternion | Quaternion | Quaternion | elementwise sum
    Quaternion | UnitQuaternion | Quaternion | elementwise sum
    Quaternion | scalar | Quaternion | add to each element
    UnitQuaternion | Quaternion | Quaternion | elementwise sum
    UnitQuaternion | UnitQuaternion | Quaternion | elementwise sum
    UnitQuaternion | scalar | Quaternion | add to each element

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left + right`
    1 | N | N | `prod[i] = left + right[i]`
    N | 1 | N | `prod[i] = left[i] + right`
    N | N | N | `prod[i] = left[i] + right[i]`
    N | M |  | `ValueError`

    A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

  `__sub__`(right)

    subtract quaternions

    Parameters

      - left (Quaternion, UnitQuaternion) – left minuend

      - right (Quaternion, UnitQuaternion, float) – right subtahend

    Returns

      difference

    Return type

      Quaternion, UnitQuaternion

    Raises

      ValueError

    Operands | Difference
    --- | ---
    left | right | type | result
    Quaternion | Quaternion | Quaternion | elementwise sum
    Quaternion | UnitQuaternion | Quaternion | elementwise sum
    Quaternion | scalar | Quaternion | subtract from each element
    UnitQuaternion | Quaternion | Quaternion | elementwise sum
    UnitQuaternion | UnitQuaternion | Quaternion | elementwise sum
    UnitQuaternion | scalar | Quaternion | subtract from each element

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left - right`
    1 | N | N | `prod[i] = left - right[i]`
    N | 1 | N | `prod[i] = left[i] - right`
    N | N | N | `prod[i] = left[i] - right[i]`
    N | M |  | `ValueError`

    A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

  `append`(x)

    S.append(value) – append value to the end of the sequence

  `clear`() → None -- remove all items from S
  property `conj`
  `dot`(omega)
  `extend`(other)

    S.extend(iterable) – extend sequence by appending elements from the iterable

  `inner`(other)
  `insert`(i, item)

    S.insert(index, value) – insert value before index

  property `matrix`
  property `norm`

    Return the norm of this quaternion. Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: number @return: the norm

  `pop`([index]) → item -- remove and return item at index (default last).

    Raise IndexError if list is empty or index is out of range.

  classmethod `pure`(v)
  `reverse`()

    S.reverse() – reverse IN PLACE

  property `s`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      real part of quaternion

    Return type

      float or numpy.ndarray

    - If the quaternion is of length one, a scalar float is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,) is returned.

  property `unit`

    Return an equivalent unit quaternion Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py Original authors: Luis Fernando Lara Tobar and Peter Corke @rtype: quaternion @return: equivalent unit quaternion

  property `v`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      vector part of quaternion

    Return type

      numpy ndarray

    - If the quaternion is of length one, a numpy array shape=(3,) is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,3) is returned.

  property `vec`
    Parameters

      q (Quaternion, UnitQuaternion) – input quaternion

    Returns

      quaternion expressed as a vector

    Return type

      numpy ndarray

    - If the quaternion is of length one, a numpy array shape=(4,) is returned.

    - If the quaternion is of length >1, a numpy array shape=(N,4) is returned.

  `dotb`(omega)
  `__mul__`(right)

    Multiply unit quaternion

    Parameters

      - left (UnitQuaternion, Quaternion, 3-vector, 3xN array, float) – left multiplicand

      - right – right multiplicand

    Returns

      product

    Return type

      Quaternion, UnitQuaternion

    Raises

      ValueError

    Multiplicands | Product
    --- | ---
    left | right | type | result
    UnitQuaternion | Quaternion | Quaternion | Hamilton product
    UnitQuaternion | UnitQuaternion | UnitQuaternion | Hamilton product
    UnitQuaternion | scalar | Quaternion | scalar product
    UnitQuaternion | 3-vector | 3-vector | vector rotation
    UnitQuaternion | 3xN array | 3xN array | vector rotations

    Any other input combinations result in a ValueError.

    Note that left and right can have a length greater than 1 in which case:

    left | right | len | operation
    --- | --- | --- | ---
    1 | 1 | 1 | `prod = left * right`
    1 | N | N | `prod[i] = left * right[i]`
    N | 1 | N | `prod[i] = left[i] * right`
    N | N | N | `prod[i] = left[i] * right[i]`
    N | M |  | `ValueError`

    A scalar of length N is a list, tuple or numpy array. A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.

    Seealso

      `__mul__()`

  `__truediv__`(right)
  `__pow__`(n)
  `__eq__`(right)

    Return self==value.

  `__ne__`(right)

    Return self!=value.

  `interp`(s=0, dest=None, shortest=False)

    Algorithm source: https://en.wikipedia.org/wiki/Slerp :param qr: UnitQuaternion :param shortest: Take the shortest path along the great circle :param s: interpolation in range [0,1] :type s: float :return: interpolated UnitQuaternion

  `plot`(*args, **kwargs)
  property `rpy`
  property `eul`
  property `angvec`
  property `SO3`
  property `SE3`

## Geometry

### Geometry in 3D

class `spatialmath.geom3d.``Plane`(c)

  Bases: `object`

  Create a plane object from linear coefficients

  Parameters

    c (4-element array_like) – Plane coefficients

  Returns

    a Plane object

  Return type

    Plane

  Planes are represented by the 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).

  `__init__`(c)

    Initialize self. See help(type(self)) for accurate signature.

  static `PN`(p, n)

    Create a plane object from point and normal

    Parameters

      - p (3-element array_like) – Point in the plane

      - n (3-element array_like) – Normal to the plane

    Returns

      a Plane object

    Return type

      Plane

  static `P3`(p)

    Create a plane object from three points

    Parameters

      p (numpy.ndarray, shape=(3,3)) – Three points in the plane

    Returns

      a Plane object

    Return type

      Plane

  property `n`

    Normal to the plane

    Returns

      Normal to the plane

    Return type

      3-element array_like

    For a plane \(\pi: ax + by + cz + d=0\) this is the vector \([a,b,c]\).

  property `d`

    Plane offset

    Returns

      Offset of the plane

    Return type

      float

    For a plane \(\pi: ax + by + cz + d=0\) this is the scalar \(d\).

  `contains`(p, tol=2.220446049250313e-15)
    Parameters

      - p (3-element array_like) – A 3D point

      - tol (float, optional) – Tolerance, defaults to 10*_eps

    Returns

      if the point is in the plane

    Return type

      bool

  `__eq__`()

    Return self==value.

  `__ne__`()

    Return self!=value.

class `spatialmath.geom3d.``Plucker`(v=None, w=None)

  Bases: `collections.UserList`

  Plucker coordinate class

  Concrete class to represent a 3D line using Plucker coordinates.

  Methods:

  Plucker Contructor from points Plucker.planes Constructor from planes Plucker.pointdir Constructor from point and direction

  Information and test methods:: closest closest point on line commonperp common perpendicular for two lines contains test if point is on line distance minimum distance between two lines intersects intersection point for two lines intersect_plane intersection points with a plane intersect_volume intersection points with a volume pp principal point ppd principal point distance from origin point generate point on line

  Conversion methods:: char convert to human readable string double convert to 6-vector skew convert to 4x4 skew symmetric matrix

  Display and print methods:: display display in human readable form plot plot line

  Operators: * multiply Plucker matrix by a general matrix | test if lines are parallel ^ test if lines intersect == test if two lines are equivalent ~= test if lines are not equivalent

  Notes:

  > - This is reference (handle) class object

  > - Plucker objects can be used in vectors and arrays

  References:

  > - Ken Shoemake, “Ray Tracing News”, Volume 11, Number 1 http://www.realtimerendering.com/resources/RTNews/html/rtnv11n1.html#art3

  > - Matt Mason lecture notes http://www.cs.cmu.edu/afs/cs/academic/class/16741-s07/www/lectures/lecture9.pdf

  > - Robotics, Vision & Control: Second Edition, P. Corke, Springer 2016; p596-7.

  Implementation notes:

  > - The internal representation is a 6-vector [v, w] where v (moment), w (direction).

  > - There is a huge variety of notation used across the literature, as well as the ordering of the direction and moment components in the 6-vector.

  Copyright (C) 1993-2019 Peter I. Corke

  `__init__`(v=None, w=None)

    Create a Plucker 3D line object

    Parameters

      - v (6-element array_like, Plucker instance, 3-element array_like) – Plucker vector, Plucker object, Plucker moment

      - w (3-element array_like, optional) – Plucker direction, optional

    Raises

      ValueError – bad arguments

    Returns

      Plucker line

    Return type

      Plucker

    - `L = Plucker(X)` creates a Plucker object from the Plucker coordinate vector `X` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.

    - `L = Plucker(L)` creates a copy of the Plucker object `L`.

    - `L = Plucker(V, W)` creates a Plucker object from moment `V` (3-vector) and line direction `W` (3-vector).

    Notes:

    - The Plucker object inherits from `collections.UserList` and has list-like behaviours.

    - A single Plucker object contains a 1D array of Plucker coordinates.

    - The elements of the array are guaranteed to be Plucker coordinates.

    - The number of elements is given by `len(L)`

    - The elements can be accessed using index and slice notation, eg. `L[1]` or `L[2:3]`

    - The Plucker instance can be used as an iterator in a for loop or list comprehension.

    - Some methods support operations on the internal list.

    Seealso

      Plucker.PQ, Plucker.Planes, Plucker.PointDir

  static `PQ`(P=None, Q=None)

    Create Plucker line object from two 3D points

    Parameters

      - P (3-element array_like) – First 3D point

      - Q (3-element array_like) – Second 3D point

    Returns

      Plucker line

    Return type

      Plucker

    `L = Plucker(P, Q)` create a Plucker object that represents the line joining the 3D points `P` (3-vector) and `Q` (3-vector). The direction is from `Q` to `P`.

    Seealso

      Plucker, Plucker.Planes, Plucker.PointDir

  static `Planes`(pi1, pi2)

    Create Plucker line from two planes

    Parameters

      - pi1 (4-element array_like, or Plane) – First plane

      - pi2 (4-element array_like, or Plane) – Second plane

    Returns

      Plucker line

    Return type

      Plucker

    `L = Plucker.planes(PI1, PI2)` is a Plucker object that represents the line formed by the intersection of two planes `PI1` and `PI2`.

    Planes are represented by the 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).

    Seealso

      Plucker, Plucker.PQ, Plucker.PointDir

  static `PointDir`(point, dir)

    Create Plucker line from point and direction

    Parameters

      - point (3-element array_like) – A 3D point

      - dir (3-element array_like) – Direction vector

    Returns

      Plucker line

    Return type

      Plucker

    `L = Plucker.pointdir(P, W)` is a Plucker object that represents the line containing the point `P` and parallel to the direction vector `W`.

    Seealso

      Plucker, Plucker.Planes, Plucker.PQ

  `append`(x)
    Parameters

      x (Plucker) – Plucker object

    Raises

      ValueError – Attempt to append a non Plucker object

    Returns

      Plucker object with new Plucker line appended

    Return type

      Plucker

  property `A`
  property `v`

    Moment vector

    Returns

      the moment vector

    Return type

      numpy.ndarray, shape=(3,)

  property `w`

    Direction vector

    Returns

      the direction vector

    Return type

      numpy.ndarray, shape=(3,)

    Seealso

      Plucker.uw

  property `uw`

    Line direction as a unit vector

    Returns

      Line direction

    Return type

      numpy.ndarray, shape=(3,)

    `line.uw` is a unit-vector parallel to the line.

  property `vec`

    Line as a Plucker coordinate vector

    Returns

      Coordinate vector

    Return type

      numpy.ndarray, shape=(6,)

    `line.vec` is the Plucker coordinate vector `X` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.

  property `skew`

    Line as a Plucker skew-matrix

    Returns

      Skew-symmetric matrix form of Plucker coordinates

    Return type

      numpy.ndarray, shape=(4,4)

    `M = line.skew()` is the Plucker matrix, a 4x4 skew-symmetric matrix representation of the line.

    Notes:

    > - For two homogeneous points P and Q on the line, \(PQ^T-QP^T\) is also skew symmetric.

    > - The projection of Plucker line by a perspective camera is a homogeneous line (3x1) given by \(\vee C M C^T\) where \(C \in \mathbf{R}^{3 \times 4}\) is the camera matrix.

  property `pp`

    Principal point of the line

    `line.pp` is the point on the line that is closest to the origin.

    Notes:

    > - Same as Plucker.point(0)

    Seealso

      Plucker.ppd, Plucker.point

  property `ppd`

    Distance from principal point to the origin

    Returns

      Distance from principal point to the origin

    Return type

      float

    `line.ppd` is the distance from the principal point to the origin. This is the smallest distance of any point on the line to the origin.

    Seealso

      Plucker.pp

  `point`(lam)

    Generate point on line

    Parameters

      lam (float) – Scalar distance from principal point

    Returns

      Distance from principal point to the origin

    Return type

      float

    `line.point(LAMBDA)` is a point on the line, where `LAMBDA` is the parametric distance along the line from the principal point of the line such that \(P = P_p + \lambda \hat{d}\) and \(\hat{d}\) is the line direction given by `line.uw`.

    Seealso

      Plucker.pp, Plucker.closest, Plucker.uw

  `contains`(x, tol=1.1102230246251565e-14)

    Test if points are on the line

    Parameters

      - x (3-element array_like, or numpy.ndarray, shape=(3,N)) – 3D point

      - tol (float, optional) – Tolerance, defaults to 50*_eps

    Raises

      ValueError – Bad argument

    Returns

      Whether point is on the line

    Return type

      bool or numpy.ndarray(N) of bool

    `line.contains(X)` is true if the point `X` lies on the line defined by the Plucker object self.

    If `X` is an array with 3 rows, the test is performed on every column and an array of booleans is returned.

  `__eq__`(l2)

    Test if two lines are equivalent

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      Plucker

    Returns

      line equivalence

    Return type

      bool

    `L1 == L2` is true if the Plucker objects describe the same line in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different.

  `__ne__`(l2)

    Test if two lines are not equivalent

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      line inequivalence

    Return type

      bool

    `L1 != L2` is true if the Plucker objects describe different lines in space. Note that because of the over parameterization, lines can be equivalent even if their coordinate vectors are different.

  `isparallel`(l2, tol=2.220446049250313e-15)

    Test if lines are parallel

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      lines are parallel

    Return type

      bool

    `l1.isparallel(l2)` is true if the two lines are parallel.

    `l1 | l2` as above but in binary operator form

    Seealso

      Plucker.or, Plucker.intersects

  `__or__`(l2)

    Test if lines are parallel as a binary operator

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      lines are parallel

    Return type

      bool

    `l1 | l2` is an operator which is true if the two lines are parallel.

    Seealso

      Plucker.isparallel, Plucker.__xor__

  `__xor__`(l2)

    Test if lines intersect as a binary operator

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      lines intersect

    Return type

      bool

    `l1 ^ l2` is an operator which is true if the two lines intersect at a point.

    Notes:

    > - Is false if the lines are equivalent since they would intersect at an infinite number of points.

    Seealso

      Plucker.intersects, Plucker.parallel

  `intersects`(l2)

    Intersection point of two lines

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      3D intersection point

    Return type

      numpy.ndarray, shape=(3,) or None

    `l1.intersects(l2)` is the point of intersection of the two lines, or `None` if the lines do not intersect or are equivalent.

    Seealso

      Plucker.commonperp, Plucker.eq, Plucker.__xor__

  `distance`(l2)

    Minimum distance between lines

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      Closest distance

    Return type

      float

    ``l1.distance(l2) is the minimum distance between two lines.

    Notes:

    > - Works for parallel, skew and intersecting lines.

  `closest`(x)

    Point on line closest to given point

    Parameters

      - line – A line

      - l2 (3-element array_like) – An arbitrary 3D point

    Returns

      Point on the line and distance to line

    Return type

      collections.namedtuple

    - `line.closest(x).p` is the coordinate of a point on the line that is closest to `x`.

    - `line.closest(x).d` is the distance between the point on the line and `x`.

    The return value is a named tuple with elements:

    > - `.p` for the point on the line as a numpy.ndarray, shape=(3,)

    > - `.d` for the distance to the point from `x`

    > - `.lam` the lambda value for the point on the line.

    Seealso

      Plucker.point

  `commonperp`(l2)

    Common perpendicular to two lines

    Parameters

      - l1 (Plucker) – First line

      - l2 (Plucker) – Second line

    Returns

      Perpendicular line

    Return type

      Plucker or None

    `l1.commonperp(l2)` is the common perpendicular line between the two lines. Returns `None` if the lines are parallel.

    Seealso

      Plucker.intersect

  `__mul__`(right)

    Reciprocal product

    Parameters

      - left (Plucker) – Left operand

      - right (Plucker) – Right operand

    Returns

      reciprocal product

    Return type

      float

    `left * right` is the scalar reciprocal product \(\hat{w}_L \dot m_R + \hat{w}_R \dot m_R\).

    Notes:

    > - Multiplication or composition of Plucker lines is not defined.

    > - Pre-multiplication by an SE3 object is supported, see `__rmul__`.

    Seealso

      Plucker.__rmul__

  `__rmul__`(left)

    Line transformation

    Parameters

      - left (SE3) – Rigid-body transform

      - right (Plucker) – Right operand

    Returns

      transformed line

    Return type

      Plucker

    `T * line` is the line transformed by the rigid body transformation `T`.

    Seealso

      Plucker.__mul__

  `intersect_plane`(plane)

    Line intersection with a plane

    Parameters

      - line (Plucker) – A line

      - plane (4-element array_like or Plane) – A plane

    Returns

      Intersection point

    Return type

      collections.namedtuple

    - `line.intersect_plane(plane).p` is the point where the line intersects the plane, or None if no intersection.

    - `line.intersect_plane(plane).lam` is the lambda value for the point on the line that intersects the plane.

    The plane can be specified as:

    > - a 4-vector \([a, b, c, d]\) which describes the plane \(\pi: ax + by + cz + d=0\).

    > - a `Plane` object

    > The return value is a named tuple with elements:

    > > - `.p` for the point on the line as a numpy.ndarray, shape=(3,)

    > > - `.lam` the lambda value for the point on the line.

    See also Plucker.point.

  `intersect_volume`(bounds)

    Line intersection with a volume

    Parameters

      - line (Plucker) – A line

      - bounds – Bounds of an axis-aligned rectangular cuboid

    Returns

      Intersection point

    Return type

      collections.namedtuple

    `line.intersect_volume(bounds).p` is a matrix (3xN) with columns that indicate where the line intersects the faces of the volume specified by `bounds` = [xmin xmax ymin ymax zmin zmax]. The number of columns N is either:

    - 0, when the line is outside the plot volume or,

    - 2 when the line pierces the bounding volume.

    `line.intersect_volume(bounds).lam` is an array of shape=(N,) where N is as above.

    The return value is a named tuple with elements:

    > - `.p` for the points on the line as a numpy.ndarray, shape=(3,N)

    > - `.lam` for the lambda values for the intersection points as a numpy.ndarray, shape=(N,).

    See also Plucker.plot, Plucker.point.

  `plot`(bounds=None, **kwargs)

    > Plot a line

    Parameters

      - line (Plucker) – A line

      - bounds – Bounds of an axis-aligned rectangular cuboid as [xmin xmax ymin ymax zmin zmax], optional

      - **kwargs –

        Extra arguents passed to Line2D

    Returns

      Plotted line

    Return type

      Line3D or None

    - `line.plot(bounds)` adds a line segment to the current axes, and the handle of the line is returned. The line segment is defined by the intersection of the line and the given rectangular cuboid. If the line does not intersect the plotting volume None is returned.

    - `line.plot()` as above but the bounds are taken from the axis limits of the current axes.

    The line color or style is specified by:

    > - a MATLAB-style linestyle like ‘k–’

    > - additional arguments passed to Line2D

    Seealso

      Plucker.intersect_volume

  `clear`() → None -- remove all items from S
  `copy`()
  `count`(value) → integer -- return number of occurrences of value
  `extend`(other)

    S.extend(iterable) – extend sequence by appending elements from the iterable

  `index`(value[, start[, stop]]) → integer -- return first index of value.

    Raises ValueError if the value is not present.

    Supporting start and stop arguments is optional, but recommended.

  `insert`(i, item)

    S.insert(index, value) – insert value before index

  `pop`([index]) → item -- remove and return item at index (default last).

    Raise IndexError if list is empty or index is out of range.

  `remove`(item)

    S.remove(value) – remove first occurrence of value. Raise ValueError if the value is not present.

  `reverse`()

    S.reverse() – reverse IN PLACE

  `sort`(*args, **kwds)

## Functions (base)

### Transforms in 2D

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

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

`spatialmath.base.transforms2d.``issymbol`(x)

`spatialmath.base.transforms2d.``colvec`(v)

`spatialmath.base.transforms2d.``rot2`(theta, unit='rad')

  Create SO(2) rotation

  Parameters

    - theta (float) – rotation angle

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    2x2 rotation matrix

  Return type

    numpy.ndarray, shape=(2,2)

  - `ROT2(THETA)` is an SO(2) rotation matrix (2x2) representing a rotation of THETA radians.

  - `ROT2(THETA, 'deg')` as above but THETA is in degrees.

`spatialmath.base.transforms2d.``trot2`(theta, unit='rad', t=None)

  Create SE(2) pure rotation

  Parameters

    - theta (float) – rotation angle about X-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - t (array_like :return: 3x3 homogeneous transformation matrix) – translation 2-vector, defaults to [0,0]

  Return type

    numpy.ndarray, shape=(3,3)

  - `TROT2(THETA)` is a homogeneous transformation (3x3) representing a rotation of THETA radians.

  - `TROT2(THETA, 'deg')` as above but THETA is in degrees.

  Notes: - Translational component is zero.

`spatialmath.base.transforms2d.``transl2`(x, y=None)

  Create SE(2) pure translation, or extract translation from SE(2) matrix

  Parameters

    - x (float) – translation along X-axis

    - y (float) – translation along Y-axis

  Returns

    homogeneous transform matrix or the translation elements of a homogeneous transform

  Return type

    numpy.ndarray, shape=(3,3)

  Create a translational SE(2) matrix:

  - `T = transl2([X, Y])` is an SE(2) homogeneous transform (3x3) representing a pure translation.

  - `T = transl2( V )` as above but the translation is given by a 2-element list, dict, or a numpy array, row or column vector.

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

  P = TRANSL2(T) is the translational part of a homogeneous transform as a 2-element numpy array.

`spatialmath.base.transforms2d.``ishom2`(T, check=False)

  Test if matrix belongs to SE(2)

  Parameters

    - T (numpy.ndarray) – matrix to test

    - check (bool) – check validity of rotation submatrix

  Returns

    whether matrix is an SE(2) homogeneous transformation matrix

  Return type

    bool

  - `ISHOM2(T)` is True if the argument `T` is of dimension 3x3

  - `ISHOM2(T, check=True)` as above, but also checks orthogonality of the rotation sub-matrix and validitity of the bottom row.

  Seealso

    isR, isrot2, ishom, isvec

`spatialmath.base.transforms2d.``isrot2`(R, check=False)

  Test if matrix belongs to SO(2)

  Parameters

    - R (numpy.ndarray) – matrix to test

    - check (bool) – check validity of rotation submatrix

  Returns

    whether matrix is an SO(2) rotation matrix

  Return type

    bool

  - `ISROT(R)` is True if the argument `R` is of dimension 2x2

  - `ISROT(R, check=True)` as above, but also checks orthogonality of the rotation matrix.

  Seealso

    isR, ishom2, isrot

`spatialmath.base.transforms2d.``trlog2`(T, check=True)

  Logarithm of SO(2) or SE(2) matrix

  Parameters

    T (numpy.ndarray, shape=(2,2) or (3,3)) – SO(2) or SE(2) matrix

  Returns

    logarithm

  Return type

    numpy.ndarray, shape=(2,2) or (3,3)

  Raises

    ValueError

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

  - `trlog2(R)` is the logarithm of the passed rotation matrix `R` which will be 2x2 skew-symmetric matrix. The equivalent vector from `vex()` is parallel to rotation axis and its norm is the amount of rotation about that axis.

  - `trlog(T)` is the logarithm of the passed homogeneous transformation matrix `T` which will be 3x3 augumented skew-symmetric matrix. The equivalent vector from `vexa()` is the twist vector (6x1) comprising [v w].

  Seealso

    `trexp()`, `vex()`, `vexa()`

`spatialmath.base.transforms2d.``trexp2`(S, theta=None)

  Exponential of so(2) or se(2) matrix

  Parameters

    - S – so(2), se(2) matrix or equivalent velctor

    - theta (float) – motion

  Returns

    2x2 or 3x3 matrix exponential in SO(2) or SE(2)

  Return type

    numpy.ndarray, shape=(2,2) or (3,3)

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

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

  - `trexp2(S)` is the matrix exponential of the so(3) element `S` which is a 2x2 skew-symmetric matrix.

  - `trexp2(S, THETA)` as above but for an so(3) motion of S*THETA, where `S` is unit-norm skew-symmetric matrix representing a rotation axis and a rotation magnitude given by `THETA`.

  - `trexp2(W)` is the matrix exponential of the so(2) element `W` expressed as a 1-vector (array_like).

  - `trexp2(W, THETA)` as above but for an so(3) motion of W*THETA where `W` is a unit-norm vector representing a rotation axis and a rotation magnitude given by `THETA`. `W` is expressed as a 1-vector (array_like).

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

  - `trexp2(SIGMA)` is the matrix exponential of the se(2) element `SIGMA` which is a 3x3 augmented skew-symmetric matrix.

  - `trexp2(SIGMA, THETA)` as above but for an se(3) motion of SIGMA*THETA, where `SIGMA` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.

  - `trexp2(TW)` is the matrix exponential of the se(3) element `TW` represented as a 3-vector which can be considered a screw motion.

  - `trexp2(TW, THETA)` as above but for an se(2) motion of TW*THETA, where `TW` must represent a unit-twist, ie. the rotational component is a unit-norm skew-symmetric matrix.

  > seealso

  >   trlog, trexp2

`spatialmath.base.transforms2d.``trinterp2`(T0, T1=None, s=None)

  Interpolate SE(2) matrices

  Parameters

    - T0 (np.ndarray, shape=(3,3)) – first SE(2) matrix

    - T1 (np.ndarray, shape=(3,3)) – second SE(2) matrix

    - s (float) – interpolation coefficient, range 0 to 1

  Returns

    SE(2) matrix

  Return type

    np.ndarray, shape=(3,3)

  - `trinterp2(T0, T1, S)` is a homogeneous transform (3x3) interpolated between T0 when S=0 and T1 when S=1. T0 and T1 are both homogeneous transforms (3x3).

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

  Notes:

  - Rotation angle is linearly interpolated.

  Seealso

    `trinterp()`

  %## 2d homogeneous trajectory

`spatialmath.base.transforms2d.``trprint2`(T, label=None, file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>, fmt='{:8.2g}', unit='deg')

  Compact display of SO(2) or SE(2) matrices

  Parameters

    - T (numpy.ndarray, shape=(2,2) or (3,3)) – matrix to format

    - label (str) – text label to put at start of line

    - file (str) – file to write formatted string to

    - fmt (str) – conversion format for each number

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    optional formatted string

  Return type

    str

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

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

        [LABEL:] THETA UNIT

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

        [LABEL:] [t=X, Y;] THETA UNIT

  Example:

      >>> T = transl2(1,2)@trot2(0.3)
      >>> trprint2(a, file=None, label='T')
      'T: t =        1,        2;       17 deg'

  Seealso

    trprint

`spatialmath.base.transforms2d.``trplot2`(T, axes=None, dims=None, color='blue', frame=None, textcolor=None, labels=['X', 'Y'], length=1, arrow=True, rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs)

  Plot a 2D coordinate frame

  Parameters

    - T – an SO(3) or SE(3) pose to be displayed as coordinate frame

    - axes (Axes3D reference) – the axes to plot into, defaults to current axes

    - dims (array_like) – dimension of plot volume as [xmin, xmax, ymin, ymax]

    - color (str) – color of the lines defining the frame

    - textcolor (str) – color of text labels for the frame, default color of lines above

    - frame (str) – label the frame, name is shown below the frame and as subscripts on the frame axis labels

    - labels (3-tuple of strings) – labels for the axes, defaults to X, Y and Z

    - length (float) – length of coordinate frame axes, default 1

    - arrow (bool) – show arrow heads, default True

    - wtl (float) – width-to-length ratio for arrows, default 0.2

    - rviz (bool) – show Rviz style arrows, default False

    - projection (str) – 3D projection: ortho [default] or persp

    - width (float) – width of lines, default 1

    - d1 – distance of frame axis label text from origin, default 1.15

  Type

    numpy.ndarray, shape=(2,2) or (3,3)

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

  - If no current figure, one is created

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

  Examples:

  > trplot2(T, frame=’A’) trplot2(T, frame=’A’, color=’green’) trplot2(T1, ‘labels’, ‘AB’);

`spatialmath.base.transforms2d.``tranimate2`(T, **kwargs)

  Animate a 2D coordinate frame

  Parameters

    - T – an SO(2) or SE(2) pose to be displayed as coordinate frame

    - nframes (int) – number of steps in the animation [defaault 100]

    - repeat (bool) – animate in endless loop [default False]

    - interval (int) – number of milliseconds between frames [default 50]

    - movie (str) – name of file to write MP4 movie into

  Type

    numpy.ndarray, shape=(2,2) or (3,3)

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

  - If no current figure, one is created

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

  Examples:

  > tranimate2(transl(1,2)@trot2(1), frame=’A’, arrow=False, dims=[0, 5]) tranimate2(transl(1,2)@trot2(1), frame=’A’, arrow=False, dims=[0, 5], movie=’spin.mp4’)

### Transforms in 3D

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

`spatialmath.base.transforms3d.``issymbol`(x)

`spatialmath.base.transforms3d.``colvec`(v)

`spatialmath.base.transforms3d.``rotx`(theta, unit='rad')

  Create SO(3) rotation about X-axis

  Parameters

    - theta (float) – rotation angle about X-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    3x3 rotation matrix

  Return type

    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

    `trotx()`

`spatialmath.base.transforms3d.``roty`(theta, unit='rad')

  Create SO(3) rotation about Y-axis

  Parameters

    - theta (float) – rotation angle about Y-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    3x3 rotation matrix

  Return type

    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

    `troty()`

`spatialmath.base.transforms3d.``rotz`(theta, unit='rad')

  Create SO(3) rotation about Z-axis

  Parameters

    - theta (float) – rotation angle about Z-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    3x3 rotation matrix

  Return type

    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

    `yrotz()`

`spatialmath.base.transforms3d.``trotx`(theta, unit='rad', t=None)

  Create SE(3) pure rotation about X-axis

  Parameters

    - theta (float) – rotation angle about X-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - t (array_like :return: 4x4 homogeneous transformation matrix) – translation 3-vector, defaults to [0,0,0]

  Return type

    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

    `rotx()`

`spatialmath.base.transforms3d.``troty`(theta, unit='rad', t=None)

  Create SE(3) pure rotation about Y-axis

  Parameters

    - theta (float) – rotation angle about Y-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - t (array_like) – translation 3-vector, defaults to [0,0,0]

  Returns

    4x4 homogeneous transformation matrix as a numpy array

  Return type

    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

    `roty()`

`spatialmath.base.transforms3d.``trotz`(theta, unit='rad', t=None)

  Create SE(3) pure rotation about Z-axis

  Parameters

    - theta (float) – rotation angle about Z-axis

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - t (array_like) – translation 3-vector, defaults to [0,0,0]

  Returns

    4x4 homogeneous transformation matrix

  Return type

    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

    `rotz()`

`spatialmath.base.transforms3d.``transl`(x, y=None, z=None)

  Create SE(3) pure translation, or extract translation from SE(3) matrix

  Parameters

    - x (float) – translation along X-axis

    - y (float) – translation along Y-axis

    - z (float) – translation along Z-axis

  Returns

    4x4 homogeneous transformation matrix

  Return type

    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

    `transl2()`

`spatialmath.base.transforms3d.``ishom`(T, check=False, tol=10)

  Test if matrix belongs to SE(3)

  Parameters

    - T (numpy.ndarray) – matrix to test

    - check (bool) – check validity of rotation submatrix

  Returns

    whether matrix is an SE(3) homogeneous transformation matrix

  Return type

    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

    `isR()`, `isrot()`, `ishom2()`

`spatialmath.base.transforms3d.``isrot`(R, check=False, tol=10)

  Test if matrix belongs to SO(3)

  Parameters

    - R (numpy.ndarray) – matrix to test

    - check (bool) – check validity of rotation submatrix

  Returns

    whether matrix is an SO(3) rotation matrix

  Return type

    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

    `isR()`, `isrot2()`, `ishom()`

`spatialmath.base.transforms3d.``rpy2r`(roll, pitch=None, yaw=None, *, unit='rad', order='zyx')

  Create an SO(3) rotation matrix from roll-pitch-yaw angles

  Parameters

    - roll (float) – roll angle

    - pitch (float) – pitch angle

    - yaw (float) – yaw angle

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

  Returns

    3x3 rotation matrix

  Return type

    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

    `eul2r()`, `rpy2tr()`, `tr2rpy()`

`spatialmath.base.transforms3d.``rpy2tr`(roll, pitch=None, yaw=None, unit='rad', order='zyx')

  Create an SE(3) rotation matrix from roll-pitch-yaw angles

  Parameters

    - roll (float) – roll angle

    - pitch (float) – pitch angle

    - yaw (float) – yaw angle

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - unit – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’

  Returns

    3x3 rotation matrix

  Return type

    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

    `eul2tr()`, `rpy2r()`, `tr2rpy()`

`spatialmath.base.transforms3d.``eul2r`(phi, theta=None, psi=None, unit='rad')

  Create an SO(3) rotation matrix from Euler angles

  Parameters

    - phi (float) – Z-axis rotation

    - theta (float) – Y-axis rotation

    - psi (float) – Z-axis rotation

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    3x3 rotation matrix

  Return type

    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

    `rpy2r()`, `eul2tr()`, `tr2eul()`

`spatialmath.base.transforms3d.``eul2tr`(phi, theta=None, psi=None, unit='rad')

  Create an SE(3) pure rotation matrix from Euler angles

  Parameters

    - phi (float) – Z-axis rotation

    - theta (float) – Y-axis rotation

    - psi (float) – Z-axis rotation

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

  Returns

    4x4 homogeneous transformation matrix

  Return type

    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

    `rpy2tr()`, `eul2r()`, `tr2eul()`

`spatialmath.base.transforms3d.``angvec2r`(theta, v, unit='rad')

  Create an SO(3) rotation matrix from rotation angle and axis

  Parameters

    - theta (float) – rotation

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - v (array_like) – rotation axis, 3-vector

  Returns

    3x3 rotation matrix

  Return type

    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

    `angvec2tr()`, `tr2angvec()`

`spatialmath.base.transforms3d.``angvec2tr`(theta, v, unit='rad')

  Create an SE(3) pure rotation from rotation angle and axis

  Parameters

    - theta (float) – rotation

    - unit (str) – angular units: ‘rad’ [default], or ‘deg’

    - v (: array_like) – rotation axis, 3-vector

  Returns

    4x4 homogeneous transformation matrix

  Return type

    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

    `angvec2r()`, `tr2angvec()`

`spatialmath.base.transforms3d.``oa2r`(o, a=None)

  Create SO(3) rotation matrix from two vectors

  Parameters

    - o (array_like) – 3-vector parallel to Y- axis

    - a – 3-vector parallel to the Z-axis

  Returns

    3x3 rotation matrix

  Return type

    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

    `oa2tr()`

`spatialmath.base.transforms3d.``oa2tr`(o, a=None)

  Create SE(3) pure rotation from two vectors

  Parameters

    - o (array_like) – 3-vector parallel to Y- axis

    - a – 3-vector parallel to the Z-axis

  Returns

    4x4 homogeneous transformation matrix

  Return type

    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

    `oa2r()`

`spatialmath.base.transforms3d.``tr2angvec`(T, unit='rad', check=False)

  Convert SO(3) or SE(3) to angle and rotation vector

  Parameters

    - R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix

    - unit (str) – ‘rad’ or ‘deg’

    - check (bool) – check that rotation matrix is valid

  Returns

    \((\theta, {\bf v})\)

  Return type

    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

    `angvec2r()`, `angvec2tr()`, `tr2rpy()`, `tr2eul()`

`spatialmath.base.transforms3d.``tr2eul`(T, unit='rad', flip=False, check=False)

  Convert SO(3) or SE(3) to ZYX Euler angles

  Parameters

    - R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix

    - unit (str) – ‘rad’ or ‘deg’

    - flip (bool) – choose first Euler angle to be in quadrant 2 or 3

    - check (bool) – check that rotation matrix is valid

  Returns

    ZYZ Euler angles

  Return type

    numpy.ndarray, shape=(3,)

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

  The 3 angles \([\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 \(\theta=0\) in which case \(\phi\) is arbitrarily set to zero and \(\phi\) is set to \(\phi+\psi\).

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

  Seealso

    `eul2r()`, `eul2tr()`, `tr2rpy()`, `tr2angvec()`

`spatialmath.base.transforms3d.``tr2rpy`(T, unit='rad', order='zyx', check=False)

  Convert SO(3) or SE(3) to roll-pitch-yaw angles

  Parameters

    - R (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix

    - unit (str) – ‘rad’ or ‘deg’

    - order – ‘xyz’, ‘zyx’ or ‘yxz’ [default ‘zyx’]

    - check (bool) – check that rotation matrix is valid

  Returns

    Roll-pitch-yaw angles

  Return type

    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

    `rpy2r()`, `rpy2tr()`, `tr2eul()`, `tr2angvec()`

`spatialmath.base.transforms3d.``trlog`(T, check=True)

  Logarithm of SO(3) or SE(3) matrix

  Parameters

    T (numpy.ndarray, shape=(3,3) or (4,4)) – SO(3) or SE(3) matrix

  Returns

    logarithm

  Return type

    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

    `trexp()`, `vex()`, `vexa()`

`spatialmath.base.transforms3d.``trexp`(S, theta=None)

  Exponential of so(3) or se(3) matrix

  Parameters

    - S – so(3), se(3) matrix or equivalent velctor

    - theta (float) – motion

  Returns

    3x3 or 4x4 matrix exponential in SO(3) or SE(3)

  Return type

    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

  >   `trexp2()`

`spatialmath.base.transforms3d.``trnorm`(T)

  Normalize an SO(3) or SE(3) matrix

  Parameters

    - T – SO(3) or SE(3) matrix

    - T1 (np.ndarray, shape=(3,3) or (4,4)) – second SE(3) matrix

  Returns

    SO(3) or SE(3) matrix

  Return type

    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’.

`spatialmath.base.transforms3d.``trinterp`(T0, T1=None, s=None)

  Interpolate SE(3) matrices

  Parameters

    - T0 (np.ndarray, shape=(4,4)) – first SE(3) matrix

    - T1 (np.ndarray, shape=(4,4)) – second SE(3) matrix

    - s (float) – interpolation coefficient, range 0 to 1

  Returns

    SE(3) matrix

  Return type

    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

    `spatialmath.base.quaternions.slerp()`, `trinterp2()`

`spatialmath.base.transforms3d.``delta2tr`(d)

  Convert differential motion to SE(3)

  Parameters

    d (array_like) – differential motion as a 6-vector

  Returns

    SE(3) matrix

  Return type

    np.ndarray, shape=(4,4)

  `T = delta2tr(d)` is an SE(3) matrix representing differential motion \(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

    `tr2delta()`

`spatialmath.base.transforms3d.``trinv`(T)

  Invert an SE(3) matrix

  Parameters

    T (np.ndarray, shape=(4,4)) – an SE(3) matrix

  Returns

    SE(3) matrix

  Return type

    np.ndarray, shape=(4,4)

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

  \(\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}\)

`spatialmath.base.transforms3d.``tr2delta`(T0, T1=None)

  Difference of SE(3) matrices as differential motion

  Parameters

    - T0 (np.ndarray, shape=(4,4)) – first SE(3) matrix

    - T1 (np.ndarray, shape=(4,4)) – second SE(3) matrix

  Returns

    Sdifferential motion as a 6-vector

  Return type

    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 \(d = [\delta_x, \delta_y, \delta_z, heta_x, heta_y, heta_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

    `delta2tr()`

`spatialmath.base.transforms3d.``tr2jac`(T, samebody=False)

  SE(3) adjoint

  Parameters

    T (np.ndarray, shape=(4,4)) – an SE(3) matrix

  Returns

    adjoint matrix

  Return type

    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 = \({}^A {f 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.

`spatialmath.base.transforms3d.``trprint`(T, orient='rpy/zyx', label=None, file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>, 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

  >   `trprint2()`, `tr2eul()`, `tr2rpy()`, `tr2angvec()`

`spatialmath.base.transforms3d.``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

  Parameters

    - T – an SO(3) or SE(3) pose to be displayed as coordinate frame

    - axes (Axes3D reference) – the axes to plot into, defaults to current axes

    - dims (array_like) – 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.

    - color (str) – color of the lines defining the frame

    - textcolor (str) – color of text labels for the frame, default color of lines above

    - frame (str) – label the frame, name is shown below the frame and as subscripts on the frame axis labels

    - labels (3-tuple of strings) – labels for the axes, defaults to X, Y and Z

    - length (float) – length of coordinate frame axes, default 1

    - arrow (bool) – show arrow heads, default True

    - wtl (float) – width-to-length ratio for arrows, default 0.2

    - rviz (bool) – show Rviz style arrows, default False

    - projection (str) – 3D projection: ortho [default] or persp

    - width (float) – width of lines, default 1

    - d1 – distance of frame axis label text from origin, default 1.15

  Type

    numpy.ndarray, shape=(3,3) or (4,4)

  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’);

`spatialmath.base.transforms3d.``tranimate`(T, **kwargs)

  Animate a 3D coordinate frame

  Parameters

    - T – an SO(3) or SE(3) pose to be displayed as coordinate frame

    - nframes (int) – number of steps in the animation [defaault 100]

    - repeat (bool) – animate in endless loop [default False]

    - interval (int) – number of milliseconds between frames [default 50]

    - movie (str) – name of file to write MP4 movie into

  Type

    numpy.ndarray, shape=(3,3) or (4,4)

  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’)

### Transforms in ND

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

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

Versions:

> 1. Luis Fernando Lara Tobar and Peter Corke, 2008

> 2. Josh Carrigg Hodson, Aditya Dua, Chee Ho Chan, 2017

> 3. Peter Corke, 2020

`spatialmath.base.transformsNd.``r2t`(R, check=False)

  Convert SO(n) to SE(n)

  Parameters

    - R – rotation matrix

    - check – check if rotation matrix is valid (default False, no check)

  Returns

    homogeneous transformation matrix

  Return type

    numpy.ndarray, shape=(3,3) or (4,4)

  `T = r2t(R)` is an SE(2) or SE(3) homogeneous transform equivalent to an SO(2) or SO(3) orthonormal rotation matrix `R` with a zero translational component

  - if `R` is 2x2 then `T` is 3x3: SO(2) -> SE(2)

  - if `R` is 3x3 then `T` is 4x4: SO(3) -> SE(3)

  Seealso

    t2r, rt2tr

`spatialmath.base.transformsNd.``t2r`(T, check=False)

  Convert SE(n) to SO(n)

  Parameters

    - T – homogeneous transformation matrix

    - check – check if rotation matrix is valid (default False, no check)

  Returns

    rotation matrix

  Return type

    numpy.ndarray, shape=(2,2) or (3,3)

  `R = T2R(T)` is the orthonormal rotation matrix component of homogeneous transformation matrix `T`

  - if `T` is 3x3 then `R` is 2x2: SE(2) -> SO(2)

  - if `T` is 4x4 then `R` is 3x3: SE(3) -> SO(3)

  Any translational component of T is lost.

  Seealso

    r2t, tr2rt

`spatialmath.base.transformsNd.``tr2rt`(T, check=False)

  Convert SE(3) to SO(3) and translation

  Parameters

    - T – homogeneous transform matrix

    - check – check if rotation matrix is valid (default False, no check)

  Returns

    Rotation matrix and translation vector

  Return type

    tuple: numpy.ndarray, shape=(2,2) or (3,3); numpy.ndarray, shape=(2,) or (3,)

  (R,t) = tr2rt(T) splits a homogeneous transformation matrix (NxN) into an orthonormal rotation matrix R (MxM) and a translation vector T (Mx1), where N=M+1.

  - if `T` is 3x3 - in SE(2) - then `R` is 2x2 and `t` is 2x1.

  - if `T` is 4x4 - in SE(3) - then `R` is 3x3 and `t` is 3x1.

  Seealso

    rt2tr, tr2r

`spatialmath.base.transformsNd.``rt2tr`(R, t, check=False)

  Convert SO(3) and translation to SE(3)

  Parameters

    - R – rotation matrix

    - t – translation vector

    - check – check if rotation matrix is valid (default False, no check)

  Returns

    homogeneous transform

  Return type

    numpy.ndarray, shape=(3,3) or (4,4)

  `T = rt2tr(R, t)` is a homogeneous transformation matrix (N+1xN+1) formed from an orthonormal rotation matrix `R` (NxN) and a translation vector `t` (Nx1).

  - If `R` is 2x2 and `t` is 2x1, then `T` is 3x3

  - If `R` is 3x3 and `t` is 3x1, then `T` is 4x4

  Seealso

    rt2m, tr2rt, r2t

`spatialmath.base.transformsNd.``rt2m`(R, t, check=False)

  Pack rotation and translation to matrix

  Parameters

    - R – rotation matrix

    - t – translation vector

    - check – check if rotation matrix is valid (default False, no check)

  Returns

    homogeneous transform

  Return type

    numpy.ndarray, shape=(3,3) or (4,4)

  `T = rt2m(R, t)` is a matrix (N+1xN+1) formed from a matrix `R` (NxN) and a vector `t` (Nx1). The bottom row is all zeros.

  - If `R` is 2x2 and `t` is 2x1, then `T` is 3x3

  - If `R` is 3x3 and `t` is 3x1, then `T` is 4x4

  Seealso

    rt2tr, tr2rt, r2t

`spatialmath.base.transformsNd.``isR`(R, tol=100)

  Test if matrix belongs to SO(n)

  Parameters

    - R (numpy.ndarray) – matrix to test

    - tol (float) – tolerance in units of eps

  Returns

    whether matrix is a proper orthonormal rotation matrix

  Return type

    bool

  Checks orthogonality, ie. \({\bf R} {\bf R}^T = {\bf I}\) and \(\det({\bf R}) > 0\). For the first test we check that the norm of the residual is less than `tol * eps`.

  Seealso

    isrot2, isrot

`spatialmath.base.transformsNd.``isskew`(S, tol=10)

  Test if matrix belongs to so(n)

  Parameters

    - S (numpy.ndarray) – matrix to test

    - tol (float) – tolerance in units of eps

  Returns

    whether matrix is a proper skew-symmetric matrix

  Return type

    bool

  Checks skew-symmetry, ie. \({\bf S} + {\bf S}^T = {\bf 0}\). We check that the norm of the residual is less than `tol * eps`.

  Seealso

    isskewa

`spatialmath.base.transformsNd.``isskewa`(S, tol=10)

  Test if matrix belongs to se(n)

  Parameters

    - S (numpy.ndarray) – matrix to test

    - tol (float) – tolerance in units of eps

  Returns

    whether matrix is a proper skew-symmetric matrix

  Return type

    bool

  Check if matrix is augmented skew-symmetric, ie. the top left (n-1xn-1) partition `S` is skew-symmetric \({\bf S} + {\bf S}^T = {\bf 0}\), and the bottom row is zero We check that the norm of the residual is less than `tol * eps`.

  Seealso

    isskew

`spatialmath.base.transformsNd.``iseye`(S, tol=10)

  Test if matrix is identity

  Parameters

    - S (numpy.ndarray) – matrix to test

    - tol (float) – tolerance in units of eps

  Returns

    whether matrix is a proper skew-symmetric matrix

  Return type

    bool

  Check if matrix is an identity matrix. We test that the trace tom row is zero We check that the norm of the residual is less than `tol * eps`.

  Seealso

    isskew, isskewa

`spatialmath.base.transformsNd.``skew`(v)

  Create skew-symmetric metrix from vector

  Parameters

    v (array_like) – 1- or 3-vector

  Returns

    skew-symmetric matrix in so(2) or so(3)

  Return type

    numpy.ndarray, shape=(2,2) or (3,3)

  Raises

    ValueError

  `skew(V)` is a skew-symmetric matrix formed from the elements of `V`.

  - `len(V)` is 1 then `S` = \(\left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\)

  - `len(V)` is 3 then `S` = \(\left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\)

  Notes:

  - This is the inverse of the function `vex()`.

  - These are the generator matrices for the Lie algebras so(2) and so(3).

  Seealso

    vex, skewa

`spatialmath.base.transformsNd.``vex`(s)

  Convert skew-symmetric matrix to vector

  Parameters

    s (numpy.ndarray, shape=(2,2) or (3,3)) – skew-symmetric matrix

  Returns

    vector of unique values

  Return type

    numpy.ndarray, shape=(1,) or (3,)

  Raises

    ValueError

  `vex(S)` is the vector which has the corresponding skew-symmetric matrix `S`.

  - `S` is 2x2 - so(2) case - where `S` \(= \left[ \begin{array}{cc} 0 & -v \\ v & 0 \end{array} \right]\) then return \([v]\)

  - `S` is 3x3 - so(3) case - where `S` \(= \left[ \begin{array}{ccc} 0 & -v_z & v_y \\ v_z & 0 & -v_x \\ -v_y & v_x & 0\end{array} \right]\) then return \([v_x, v_y, v_z]\).

  Notes:

  - This is the inverse of the function `skew()`.

  - Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.

  - The function takes the mean of the two elements that correspond to each unique element of the matrix.

  Seealso

    skew, vexa

`spatialmath.base.transformsNd.``skewa`(v)

  Create augmented skew-symmetric metrix from vector

  Parameters

    v (array_like) – 3- or 6-vector

  Returns

    augmented skew-symmetric matrix in se(2) or se(3)

  Return type

    numpy.ndarray, shape=(3,3) or (4,4)

  Raises

    ValueError

  `skewa(V)` is an augmented skew-symmetric matrix formed from the elements of `V`.

  - `len(V)` is 3 then S = \(\left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\)

  - `len(V)` is 6 then S = \(\left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\)

  Notes:

  - This is the inverse of the function `vexa()`.

  - These are the generator matrices for the Lie algebras se(2) and se(3).

  - Map twist vectors in 2D and 3D space to se(2) and se(3).

  Seealso

    vexa, skew

`spatialmath.base.transformsNd.``vexa`(Omega)

  Convert skew-symmetric matrix to vector

  Parameters

    s (numpy.ndarray, shape=(3,3) or (4,4)) – augmented skew-symmetric matrix

  Returns

    vector of unique values

  Return type

    numpy.ndarray, shape=(3,) or (6,)

  Raises

    ValueError

  `vex(S)` is the vector which has the corresponding skew-symmetric matrix `S`.

  - `S` is 3x3 - se(2) case - where `S` \(= \left[ \begin{array}{ccc} 0 & -v_3 & v_1 \\ v_3 & 0 & v_2 \\ 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3]\).

  - `S` is 4x4 - se(3) case - where `S` \(= \left[ \begin{array}{cccc} 0 & -v_6 & v_5 & v_1 \\ v_6 & 0 & -v_4 & v_2 \\ -v_5 & v_4 & 0 & v_3 \\ 0 & 0 & 0 & 0 \end{array} \right]\) then return \([v_1, v_2, v_3, v_4, v_5, v_6]\).

  Notes:

  - This is the inverse of the function `skewa`.

  - Only rudimentary checking (zero diagonal) is done to ensure that the matrix is actually skew-symmetric.

  - The function takes the mean of the two elements that correspond to each unique element of the matrix.

  Seealso

    skewa, vex

`spatialmath.base.transformsNd.``h2e`(v)

  Convert from homogeneous to Euclidean form

  Parameters

    v (array_like) – homogeneous vector or matrix

  Returns

    Euclidean vector

  Return type

    numpy.ndarray

  - If `v` is an array, shape=(N,), return an array shape=(N-1,) where the elements have all been scaled by the last element of `v`.

  - If `v` is a matrix, shape=(N,M), return a matrix shape=(N-1,N), where each column has been scaled by its last element.

  Seealso

    e2h

`spatialmath.base.transformsNd.``e2h`(v)

  Convert from Euclidean to homogeneous form

  Parameters

    v (array_like) – Euclidean vector or matrix

  Returns

    homogeneous vector

  Return type

    numpy.ndarray

  - If `v` is an array, shape=(N,), return an array shape=(N+1,) where a value of 1 has been appended

  - If `v` is a matrix, shape=(N,M), return a matrix shape=(N+1,N), where each column has been appended with a value of 1, ie. a row of ones has been appended to the matrix.

  Seealso

    e2h

### Vectors

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

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

`spatialmath.base.vectors.``colvec`(v)

`spatialmath.base.vectors.``unitvec`(v)

  Create a unit vector

  Parameters

    v (array_like) – n-dimensional vector

  Returns

    a unit-vector parallel to V.

  Return type

    numpy.ndarray

  Raises

    ValueError – for zero length vector

  `unitvec(v)` is a vector parallel to v of unit length.

  Seealso

    norm

`spatialmath.base.vectors.``norm`(v)

  Norm of vector

  Parameters

    v – n-vector as a list, dict, or a numpy array, row or column vector

  Returns

    norm of vector

  Return type

    float

  `norm(v)` is the 2-norm (length or magnitude) of the vector `v`.

  Seealso

    unit

`spatialmath.base.vectors.``isunitvec`(v, tol=10)

  Test if vector has unit length

  Parameters

    - v (numpy.ndarray) – vector to test

    - tol (float) – tolerance in units of eps

  Returns

    whether vector has unit length

  Return type

    bool

  Seealso

    unit, isunittwist

`spatialmath.base.vectors.``iszerovec`(v, tol=10)

  Test if vector has zero length

  Parameters

    - v (numpy.ndarray) – vector to test

    - tol (float) – tolerance in units of eps

  Returns

    whether vector has zero length

  Return type

    bool

  Seealso

    unit, isunittwist

`spatialmath.base.vectors.``isunittwist`(v, tol=10)

  Test if vector represents a unit twist in SE(2) or SE(3)

  Parameters

    - v (array_like) – vector to test

    - tol (float) – tolerance in units of eps

  Returns

    whether vector has unit length

  Return type

    bool

  Vector is is intepretted as \([v, \omega]\) where \(v \in \mathbb{R}^n\) and \(\omega \in \mathbb{R}^1\) for SE(2) and \(\omega \in \mathbb{R}^3\) for SE(3).

  A unit twist can be a:

  - unit rotational twist where \(|| \omega || = 1\), or

  - unit translational twist where \(|| \omega || = 0\) and \(|| v || = 1\).

  Seealso

    unit, isunitvec

`spatialmath.base.vectors.``isunittwist2`(v, tol=10)

  Test if vector represents a unit twist in SE(2) or SE(3)

  Parameters

    - v (array_like) – vector to test

    - tol (float) – tolerance in units of eps

  Returns

    whether vector has unit length

  Return type

    bool

  Vector is is intepretted as \([v, \omega]\) where \(v \in \mathbb{R}^n\) and \(\omega \in \mathbb{R}^1\) for SE(2) and \(\omega \in \mathbb{R}^3\) for SE(3).

  A unit twist can be a:

  - unit rotational twist where \(|| \omega || = 1\), or

  - unit translational twist where \(|| \omega || = 0\) and \(|| v || = 1\).

  Seealso

    unit, isunitvec

`spatialmath.base.vectors.``unittwist`(S, tol=10)

  Convert twist to unit twist

  Parameters

    - S (array_like) – twist as a 6-vector

    - tol (float) – tolerance in units of eps

  Returns

    unit twist and scalar motion

  Return type

    np.ndarray, shape=(6,)

  A unit twist is a twist where:

  - the rotation part has unit magnitude

  - if the rotational part is zero, then the translational part has unit magnitude

  Returns None if the twist has zero magnitude

`spatialmath.base.vectors.``unittwist_norm`(S, tol=10)

  Convert twist to unit twist and norm

  Parameters

    - S (array_like) – twist as a 6-vector

    - tol (float) – tolerance in units of eps

  Returns

    unit twist and scalar motion

  Return type

    tuple (np.ndarray shape=(6,), theta)

  A unit twist is a twist where:

  - the rotation part has unit magnitude

  - if the rotational part is zero, then the translational part has unit magnitude

  Returns (None,None) if the twist has zero magnitude

`spatialmath.base.vectors.``unittwist2`(S)

  Convert twist to unit twist

  Parameters

    S (array_like) – twist as a 3-vector

  Returns

    unit twist and scalar motion

  Return type

    tuple (unit_twist, theta)

  A unit twist is a twist where:

  - the rotation part has unit magnitude

  - if the rotational part is zero, then the translational part has unit magnitude

`spatialmath.base.vectors.``angdiff`(a, b)

  Angular difference

  Parameters

    - a (scalar or array_like) – angle in radians

    - b (scalar or array_like) – angle in radians

  Returns

    angular difference a-b

  Return type

    scalar or array_like

  - If `a` and `b` are both scalars, the result is scalar

  - If `a` is array_like, the result is a vector a[i]-b

  - If `a` is array_like, the result is a vector a-b[i]

  - If `a` and `b` are both vectors of the same length, the result is a vector a[i]-b[i]

### Quaternions

Created on Fri Apr 10 14:12:56 2020

@author: Peter Corke

`spatialmath.base.quaternions.``eye`()

  Create an identity quaternion

  Returns

    an identity quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Creates an identity quaternion, with the scalar part equal to one, and a zero vector value.

`spatialmath.base.quaternions.``pure`(v)

  Create a pure quaternion

  Parameters

    v (array_like) – vector from a 3-vector

  Returns

    pure quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Creates a pure quaternion, with a zero scalar value and the vector part equal to the passed vector value.

`spatialmath.base.quaternions.``qnorm`(q)

  Norm of a quaternion

  Parameters

    q – input quaternion as a 4-vector

  Returns

    norm of the quaternion

  Return type

    float

  Returns the norm, length or magnitude of the input quaternion which is \(\sqrt{s^2 + v_x^2 + v_y^2 + v_z^2}\)

  Seealso

    unit

`spatialmath.base.quaternions.``unit`(q, tol=10)

  Create a unit quaternion

  Parameters

    v (array_like) – quaterion as a 4-vector

  Returns

    a pure quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Creates a unit quaternion, with unit norm, by scaling the input quaternion.

  See also

  norm

`spatialmath.base.quaternions.``isunit`(q, tol=10)

  Test if quaternion has unit length

  Parameters

    - v (array_like) – quaternion as a 4-vector

    - tol (float) – tolerance in units of eps

  Returns

    whether quaternion has unit length

  Return type

    bool

  Seealso

    unit

`spatialmath.base.quaternions.``isequal`(q1, q2, tol=100, unitq=False)

  Test if quaternions are equal

  Parameters

    - q1 (array_like) – quaternion as a 4-vector

    - q2 (array_like) – quaternion as a 4-vector

    - unitq (bool) – quaternions are unit quaternions

    - tol (float) – tolerance in units of eps

  Returns

    whether quaternion has unit length

  Return type

    bool

  Tests if two quaternions are equal.

  For unit-quaternions `unitq=True` the double mapping is taken into account, that is `q` and `-q` represent the same orientation and `isequal(q, -q, unitq=True)` will return `True`.

`spatialmath.base.quaternions.``q2v`(q)

  Convert unit-quaternion to 3-vector

  Parameters

    q – unit-quaternion as a 4-vector

  Returns

    a unique 3-vector

  Return type

    numpy.ndarray, shape=(3,)

  Returns a unique 3-vector representing the input unit-quaternion. The sign of the scalar part is made positive, if necessary by multiplying the entire quaternion by -1, then the vector part is taken.

  Warning

  There is no check that the passed value is a unit-quaternion.

  See also

  v2q

`spatialmath.base.quaternions.``v2q`(v)

  Convert 3-vector to unit-quaternion

  Parameters

    v (array_like) – vector part of unit quaternion, a 3-vector

  Returns

    a unit quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Returns a unit-quaternion reconsituted from just its vector part. Assumes that the scalar part was positive, so \(s = \sqrt{1-||v||}\).

  See also

  q2v

`spatialmath.base.quaternions.``qqmul`(q1, q2)

  Quaternion multiplication

  Parameters

    - q0 (: array_like) – left-hand quaternion as a 4-vector

    - q1 (array_like) – right-hand quaternion as a 4-vector

  Returns

    quaternion product

  Return type

    numpy.ndarray, shape=(4,)

  This is the quaternion or Hamilton product. If both operands are unit-quaternions then the product will be a unit-quaternion.

  Seealso

    qvmul, inner, vvmul

`spatialmath.base.quaternions.``inner`(q1, q2)

  Quaternion innert product

  Parameters

    - q0 (: array_like) – quaternion as a 4-vector

    - q1 (array_like) – uaternion as a 4-vector

  Returns

    inner product

  Return type

    numpy.ndarray, shape=(4,)

  This is the inner or dot product of two quaternions, it is the sum of the element-wise product.

  Seealso

    qvmul

`spatialmath.base.quaternions.``qvmul`(q, v)

  Vector rotation

  Parameters

    - q (array_like) – unit-quaternion as a 4-vector

    - v (list, tuple, numpy.ndarray) – 3-vector to be rotated

  Returns

    rotated 3-vector

  Return type

    numpy.ndarray, shape=(3,)

  The vector v is rotated about the origin by the SO(3) equivalent of the unit quaternion.

  Warning

  There is no check that the passed value is a unit-quaternions.

  Seealso

    qvmul

`spatialmath.base.quaternions.``vvmul`(qa, qb)

  Quaternion multiplication

  Parameters

    - qa (: array_like) – left-hand quaternion as a 3-vector

    - qb (array_like) – right-hand quaternion as a 3-vector

  Returns

    quaternion product

  Return type

    numpy.ndarray, shape=(3,)

  This is the quaternion or Hamilton product of unit-quaternions defined only by their vector components. The product will be a unit-quaternion, defined only by its vector component.

  Seealso

    qvmul, inner

`spatialmath.base.quaternions.``pow`(q, power)

  Raise quaternion to a power

  Parameters

    - q – quaternion as a 4-vector

    - power (int) – exponent

  Returns

    input quaternion raised to the specified power

  Return type

    numpy.ndarray, shape=(4,)

  Raises a quaternion to the specified power using repeated multiplication.

  Notes:

  - power must be an integer

  - power can be negative, in which case the conjugate is taken

`spatialmath.base.quaternions.``conj`(q)

  Quaternion conjugate

  Parameters

    q – quaternion as a 4-vector

  Returns

    conjugate of input quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Conjugate of quaternion, the vector part is negated.

`spatialmath.base.quaternions.``q2r`(q)

  Convert unit-quaternion to SO(3) rotation matrix

  Parameters

    q – unit-quaternion as a 4-vector

  Returns

    corresponding SO(3) rotation matrix

  Return type

    numpy.ndarray, shape=(3,3)

  Returns an SO(3) rotation matrix corresponding to this unit-quaternion.

  Warning

  There is no check that the passed value is a unit-quaternion.

  Seealso

    r2q

`spatialmath.base.quaternions.``r2q`(R, check=True)

  Convert SO(3) rotation matrix to unit-quaternion

  Parameters

    R (numpy.ndarray, shape=(3,3)) – rotation matrix

  Returns

    unit-quaternion

  Return type

    numpy.ndarray, shape=(3,)

  Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.

  Warning

  There is no check that the passed matrix is a valid rotation matrix.

  Seealso

    q2r

`spatialmath.base.quaternions.``slerp`(q0, q1, s, shortest=False)

  Quaternion conjugate

  Parameters

    - q0 (array_like) – initial unit quaternion as a 4-vector

    - q1 (array_like) – final unit quaternion as a 4-vector

    - s (float) – interpolation coefficient in the range [0,1]

    - shortest (bool) – choose shortest distance [default False]

  Returns

    interpolated unit-quaternion

  Return type

    numpy.ndarray, shape=(4,)

  An interpolated quaternion between `q0` when `s` = 0 to `q1` when `s` = 1.

  Interpolation is performed on a great circle on a 4D hypersphere. This is a rotation about a single fixed axis in space which yields the straightest and shortest path between two points.

  For large rotations the path may be the long way around the circle, the option `'shortest'` ensures always the shortest path.

  Warning

  There is no check that the passed values are unit-quaternions.

`spatialmath.base.quaternions.``rand`()

  Random unit-quaternion

  Returns

    random unit-quaternion

  Return type

    numpy.ndarray, shape=(4,)

  Computes a uniformly distributed random unit-quaternion which can be considered equivalent to a random SO(3) rotation.

`spatialmath.base.quaternions.``matrix`(q)

  Convert to 4x4 matrix equivalent

  Parameters

    q – quaternion as a 4-vector

  Returns

    equivalent matrix

  Return type

    numpy.ndarray, shape=(4,4)

  Hamilton multiplication between two quaternions can be considered as a matrix-vector product, the left-hand quaternion is represented by an equivalent 4x4 matrix and the right-hand quaternion as 4x1 column vector.

  Seealso

    qqmul

`spatialmath.base.quaternions.``dot`(q, w)

  Rate of change of unit-quaternion

  Parameters

    - q0 (array_like) – unit-quaternion as a 4-vector

    - w (array_like) – angular velocity in world frame as a 3-vector

  Returns

    rate of change of unit quaternion

  Return type

    numpy.ndarray, shape=(4,)

  `dot(q, w)` is the rate of change of the elements of the unit quaternion `q` which represents the orientation of a body frame with angular velocity `w` in the world frame.

  Warning

  There is no check that the passed values are unit-quaternions.

`spatialmath.base.quaternions.``dotb`(q, w)

  Rate of change of unit-quaternion

  Parameters

    - q0 (array_like) – unit-quaternion as a 4-vector

    - w (array_like) – angular velocity in body frame as a 3-vector

  Returns

    rate of change of unit quaternion

  Return type

    numpy.ndarray, shape=(4,)

  `dot(q, w)` is the rate of change of the elements of the unit quaternion `q` which represents the orientation of a body frame with angular velocity `w` in the body frame.

  Warning

  There is no check that the passed values are unit-quaternions.

`spatialmath.base.quaternions.``angle`(q1, q2)

  Angle between two unit-quaternions

  Parameters

    - q0 (array_like) – unit-quaternion as a 4-vector

    - q1 (array_like) – unit-quaternion as a 4-vector

  Returns

    angle between the rotations [radians]

  Return type

    float

  If each of the input quaternions is considered a rotated coordinate frame, then the angle is the smallest rotation required about a fixed axis, to rotate the first frame into the second.

  References: Metrics for 3D rotations: comparison and analysis, Du Q. Huynh, % J.Math Imaging Vis. DOFI 10.1007/s10851-009-0161-2.

  Warning

  There is no check that the passed values are unit-quaternions.

`spatialmath.base.quaternions.``qprint`(q, delim=('<', '>'), fmt='%f', file=<_io.TextIOWrapper name='<stdout>' mode='w' encoding='UTF-8'>)

  Format a quaternion

  Parameters

    - q (array_like) – unit-quaternion as a 4-vector

    - delim (list or tuple of strings) – 2-list of delimeters [default (‘<’, ‘>’)]

    - fmt (str) – printf-style format soecifier [default ‘%f’]

    - file (file object) – destination for formatted string [default sys.stdout]

  Returns

    formatted string

  Return type

    str

  Format the quaternion in a human-readable form as:

      S  D1  VX VY VZ D2

  where S, VX, VY, VZ are the quaternion elements, and D1 and D2 are a pair of delimeters given by delim.

  By default the string is written to sys.stdout.

  If file=None then a string is returned.