Classes and functions

Pose classes

Pose in 2D

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

Bases: spatialmath.super_pose.SMPose

__init__(arg=None, *, unit='rad', check=True)[source]

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)[source]

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)[source]

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)[source]

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()[source]

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')[source]

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()[source]

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)[source]

Bases: spatialmath.pose2d.SO2

__init__(x=None, y=None, theta=None, *, unit='rad', check=True)[source]

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)[source]

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)[source]

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)[source]

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()[source]

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()[source]

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)[source]

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)[source]

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)[source]

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()[source]

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')[source]

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')[source]

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()[source]

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)[source]

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')[source]

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')[source]

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')[source]

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)[source]

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')[source]

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')[source]

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)[source]

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')[source]

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)[source]

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)[source]

Bases: spatialmath.pose3d.SO3

__init__(x=None, y=None, z=None, *, check=True)[source]

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()[source]

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)[source]

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)[source]

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')[source]

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')[source]

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')[source]

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)[source]

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')[source]

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')[source]

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)[source]

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')[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]
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)[source]
__eq__(other)[source]

Return self==value.

__ne__(other)[source]

Return self!=value.

__mul__(right)[source]

multiply quaternion

Parameters
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)[source]
__truediv__(other)[source]
__add__(right)[source]

add quaternions

Parameters
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)[source]

subtract quaternions

Parameters
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)[source]

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)[source]

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')[source]

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')[source]

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')[source]

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)[source]

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')[source]

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')[source]

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)[source]

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')[source]

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)[source]
classmethod Vec3(vec)[source]
property inv
classmethod omega(w)[source]
static qvmul(qv1, qv2)[source]
__add__(right)

add quaternions

Parameters
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
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)[source]
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)[source]
__mul__(right)[source]

Multiply unit quaternion

Parameters
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)[source]
__pow__(n)[source]
__eq__(right)[source]

Return self==value.

__ne__(right)[source]

Return self!=value.

interp(s=0, dest=None, shortest=False)[source]

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)[source]
property rpy
property eul
property angvec
property SO3
property SE3

Geometry

Geometry in 3D

class spatialmath.geom3d.Plane(c)[source]

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)[source]

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

static PN(p, n)[source]

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)[source]

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)[source]
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)[source]

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:

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]
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)[source]

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)[source]

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)[source]

Test if two lines are equivalent

Parameters
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)[source]

Test if two lines are not equivalent

Parameters
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)[source]

Test if lines are parallel

Parameters
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)[source]

Test if lines are parallel as a binary operator

Parameters
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)[source]

Test if lines intersect as a binary operator

Parameters
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)[source]

Intersection point of two lines

Parameters
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)[source]

Minimum distance between lines

Parameters
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)[source]

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)[source]

Common perpendicular to two lines

Parameters
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)[source]

Reciprocal product

Parameters
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)[source]

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)[source]

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)[source]

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)[source]

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)[source]
spatialmath.base.transforms2d.colvec(v)[source]
spatialmath.base.transforms2d.rot2(theta, unit='rad')[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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')[source]

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)[source]

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)[source]

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)[source]
spatialmath.base.transforms3d.colvec(v)[source]
spatialmath.base.transforms3d.rotx(theta, unit='rad')[source]

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')[source]

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')[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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')[source]

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')[source]

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')[source]

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')[source]

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')[source]

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')[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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')[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]
spatialmath.base.vectors.unitvec(v)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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()[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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)[source]

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()[source]

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)[source]

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)[source]

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)[source]

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)[source]

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'>)[source]

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.