SE(2) matrix

class SE2(*args, **kwargs)[source]

Bases: SO2

SE(2) matrix class

This subclass represents rigid-body motion (pose) in 2D space. Internally it is a 3x3 homogeneous transformation matrix belonging to the group SE(2).

Inheritance diagram of spatialmath.pose2d.SE2

classmethod Alloc(n=1)

Construct an instance with N default values (BasePoseList superclass method)

Parameters:: n (int, optional) – Number of values, defaults to 1
Return type:: Self
Returns:: pose instance with n default values

X.Alloc(N) creates an instance of the pose class X with N default values, ie. len(X) will be N.

X can be considered a vector of pose objects, and those elements can be referenced X[i] or assigned to X[i] = ....

Note

The default value depends on the pose class and is the result of the empty constructor. For SO2, SE2, SO3, SE3 it is an identity matrix, for a twist class Twist2 or Twist3 it is a zero vector, for a UnitQuaternion or Quaternion it is a zero vector.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10

where X is any of the SMTB classes.

classmethod Empty()

Construct an empty instance (BasePoseList superclass method)

Return type:: Self
Returns:: pose instance with zero values

Example:

>>> x = X.Empty()
>>> len(x)
0

where X is any of the SMTB classes.

classmethod Exp(S, check=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

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()

classmethod Rand(N=1, xrange=(-1, 1), yrange=(-1, 1), arange=(0, 6.283185307179586), unit='rad')[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]
arange (2-element sequence, optional) – angle range [min,max], defaults to \([0, 2\pi)\)
N (int) – number of random rotations, defaults to 1
unit (str, optional) – angular units ‘deg’ or ‘rad’ [default] if applicable

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 Rot(theta, unit='rad')[source]

Create an SE(2) rotation

Parameters:

theta (float) – rotation angle in radians
unit (str) – angular units: “rad” [default] or “deg”

Returns:

SE(2) matrix

Return type:

SE2 instance

SE2.Rot(theta) is an SE(2) rotation of theta

Example:

  File "<input>", line 1, in <module>
NameError: name 'SE2' is not defined

Seealso:: transl()
SymPy:: supported

SE2()

Create SE(2) from SO(2)

Returns:: SE(2) with same rotation but zero translation
Return type:: SE2 instance

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.

Twist2()[source]

classmethod Tx(x)[source]

Create an SE(2) translation along the X-axis

Parameters:: x (float) – translation distance along the X-axis
Returns:: SE(2) matrix
Return type:: SE2 instance

SE2.Tx(x) is an SE(2) translation of x along the x-axis

Example:

  File "<input>", line 1, in <module>
NameError: name 'SE2' is not defined

Seealso:: transl()
SymPy:: supported

classmethod Ty(y)[source]

Create an SE(2) translation along the Y-axis

Parameters:: y (float) – translation distance along the Y-axis
Returns:: SE(2) matrix
Return type:: SE2 instance

SE2.Ty(y) is an SE(2) translation of ``y` along the y-axis

Example:

  File "<input>", line 1, in <module>
NameError: name 'SE2' is not defined

Seealso:: transl()
SymPy:: supported

__add__(right)

Overloaded + operator (superclass method)

Returns:: Sum of two operands
Return type:: NumPy array, shape=(N,N)
Raises:: ValueError – for incompatible arguments

Add the 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 scalar s
s + X is the element-wise sum of the scalar s and the matrix value of 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

Note

Pose is an SO2, SE2, SO3 or SE3 instance
N is 2 for SO2, SE2; 3 for SO3 or SE3
scalar + Pose is handled by __radd__()
Addition is commutative
Any other input combinations result in a ValueError.

For pose addition either or both operands may hold more than one value which results in the sum holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	`sum = left + right`
1	M	M	`sum[i] = left + right[i]`
N	1	M	`sum[i] = left[i] + right`
M	M	M	`sum[i] = left[i] + right[i]`

__eq__(right)

Overloaded == operator (superclass method)

Returns:: Equality of two operands
Return type:: bool or list of bool

Test two poses for equality

X == Y is true of the poses are of the same type and numerically equal.

If either or both operands may hold more than one value which results in the equality test holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	`eq = left == right`
1	M	M	`eq[i] = left == right[i]`
N	1	M	`eq[i] = left[i] == right`
M	M	M	`eq[i] = left[i] == right[i]`

__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:

SE(2) matrix

Return type:

SE2 instance

SE2() is an SE2 instance representing a null motion – the identity matrix
SE2(θ) is an SE2 instance representing a pure rotation of θ radians
SE2(θ, unit='deg') as above but θ in degrees
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, θ) is an SE2 instance representing a translation of (x, y) and a rotation of θ radians
SE2(x, y, θ, unit='deg') as above but θ in degrees
SE2(t) where t``=[x,y] is a 2-element array_like, is an SE2 instance representing a pure translation of (``x, y)
SE2(q) where q``=[x,y,θ] is a 3-element array_like, is an SE2 instance representing a translation of (``x, y) and a rotation of θ radians
SE2(t, unit='deg') as above but θ in degrees
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.

__mul__(right)

Overloaded * operator (superclass method)

Returns:: Product of two operands
Return type:: Pose instance or NumPy array
Raises:: NotImplemented – for incompatible arguments

Pose composition, scaling or vector transformation:

X * Y compounds the poses X and Y
X * s performs element-wise multiplication of the elements of X by s
s * X performs element-wise multiplication of the elements of X by s
X * v linear transformation of the vector v where v is array-like

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

Note

Pose is an SO2, SE2, SO3 or SE3 instance
N is 2 for SO2, SE2; 3 for SO3 or SE3
Scalar x Pose is handled by __rmul__`
Scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.

For pose composition either or both operands may hold more than one value which results in the composition holding more than one value according to:

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

Example:

>>> SE3.Rx(pi/2) * SE3.Ry(pi/2)
SE3(array([[0., 0., 1., 0.],
        [1., 0., 0., 0.],
        [0., 1., 0., 0.],
        [0., 0., 0., 1.]]))
>>> SE3.Rx(pi/2) * 2
array([[ 2.0000000e+00,  0.0000000e+00,  0.0000000e+00,  0.0000000e+00],
       [ 0.0000000e+00,  1.2246468e-16, -2.0000000e+00,  0.0000000e+00],
       [ 0.0000000e+00,  2.0000000e+00,  1.2246468e-16,  0.0000000e+00],
       [ 0.0000000e+00,  0.0000000e+00,  0.0000000e+00,  2.0000000e+00]])

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

Note

The vector is an array-like, a 1D NumPy array or a list/tuple
For the SE2 and SE3 case the vectors are converted to homogeneous form, transformed, then converted back to Euclidean form.

Example:

>>> SE3.Rx(pi/2) * [0, 1, 0]
array([0.000000e+00, 6.123234e-17, 1.000000e+00])
>>> SE3.Rx(pi/2) * np.r_[0, 0, 1]
array([ 0.000000e+00, -1.000000e+00,  6.123234e-17])

__ne__(right)

Overloaded != operator (superclass method)

Returns:: Inequality of two operands
Return type:: bool or list of bool

Test two poses for inequality

X != Y is true of the poses are of the same type but not numerically equal.

If either or both operands may hold more than one value which results in the inequality test holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	`ne = left != right`
1	M	M	`ne[i] = left != right[i]`
N	1	M	`ne[i] = left[i] != right`
M	M	M	`ne[i] = left[i] != right[i]`

__pow__(n)

Overloaded ** operator (superclass method)

Parameters:: n (int) – exponent
Returns:: pose to the power n
Return type:: pose instance

X**n raise all values held in X to the specified power using repeated multiplication. If n < 0 then the result is inverted.

Example:

>>> from spatialmath import SE3
>>> SE3.Rx(0.1) ** 2
SE3(array([[ 1.    ,  0.    ,  0.    ,  0.    ],
           [ 0.    ,  0.9801, -0.1987,  0.    ],
           [ 0.    ,  0.1987,  0.9801,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))
>>> SE3.Rx([0, 0.1]) ** 2
SE3([
array([[1., 0., 0., 0.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.],
       [0., 0., 0., 1.]]),
array([[ 1.    ,  0.    ,  0.    ,  0.    ],
       [ 0.    ,  0.9801, -0.1987,  0.    ],
       [ 0.    ,  0.1987,  0.9801,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  1.    ]]) ])

__sub__(right)

Overloaded - operator (superclass method)

Returns:: Difference of two operands
Return type:: NumPy array, shape=(N,N)
Raises:: ValueError – for incompatible arguments

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 the scalar s
s - X is the element-wise difference of the scalar 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

Note

Pose is SO2, SE2, SO3 or SE3 instance
N is 2 for SO2, SE2; 3 for SO3 or SE3
scalar - Pose is handled by __rsub__()
Any other input combinations result in a ValueError.

For pose subtraction either or both operands may hold more than one value which results in the difference holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	`diff = left - right`
1	M	M	`diff[i] = left - right[i]`
N	1	M	`diff[i] = left[i] - right`
M	M	M	`diff[i] = left[i] right[i]`

__truediv__(right)

Overloaded / operator (superclass method)

Returns:: Product of right operand and inverse of left operand
Return type:: pose instance or NumPy array
Raises:: ValueError – for incompatible arguments

Pose composition or scaling:

X / Y compounds the poses X and Y.inv()
X / s performs elementwise division 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

Note

Pose is SO2, SE2, SO3 or SE3 instance
N is 2 for SO2, SE2; 3 for SO3 or SE3
Scalar multiplication is not a group operation so the result will be a matrix
Any other input combinations result in a ValueError.

For pose composition either or both operands may hold more than one value which results in the composition holding more than one value according to:

len(left)	len(right)	len	operation
1	1	1	`quo = left * right.inv()`
1	M	M	`quo[i] = left * right[i].inv()`
N	1	M	`quo[i] = left[i] * right.inv()`
M	M	M	`quo[i] = left[i] * right[i].inv()`

animate(*args, start=None, **kwargs)

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

Parameters:

start (same as self) – initial pose, defaults to null/identity
**kwargs – plotting options

Return type:

None

X.animate() displays the pose X as a coordinate frame moving from the origin in either 2D or 3D. There are many options, see the links below.
X.animate(*args, start=X1) displays the pose X as a coordinate frame moving from pose X1, in either 2D or 3D. There are many options, see the links below.

Example:

>>> X = SE3.Rx(0.3)
>>> X.animate(frame='A', color='green')
>>> X.animate(start=SE3.Ry(0.2))

Seealso:: tranimate(), tranimate2()

append(item)

Append a value to an instance (BasePoseList superclass method)

Parameters:: x (Quaternion or UnitQuaternion instance) – the value to append
Raises:: ValueError – incorrect type of appended object
Return type:: None

Appends the argument to the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.append(X())   # append to the list
>>> len(x)
11

where X is any of the SMTB classes.

arghandler(arg, convertfrom=(), check=True)

Standard constructor support (BasePoseList superclass method)

Parameters:

arg (Any) – initial value
convertfrom (Tuple) – list of classes to accept and convert from
check (bool) – check value is valid, defaults to True

Type:

tuple of typles

Raises:

ValueError – bad type passed

Return type:

bool

The value arg can be any of:

None, an identity value is created
a numpy.ndarray of the appropriate shape and value which is valid for the subclass
a list whose elements all meet the criteria above
an instance of the subclass
a list whose elements are all singelton instances of the subclass

For cases 2 and 3, a NumPy array or a list of NumPy array is passed. Each NumPyarray is tested for validity (if check is False a cursory check of shape is made, if check is True the numerical value is inspected) and converted to the required internal format by the _import method. The default _import method calls the isvalid method for checking. This mechanism allows equivalent forms to be passed, ie. 6x1 or 4x4 for an se(3).

If self is an instance of class A, and an instance of class B is passed and B is an element of the convertfrom argument, then B.A() will be invoked to perform the type conversion.

Examples:

SE3()
SE3(np.identity(4))
SE3([np.identity(4), np.identity(4)])
SE3(SE3())
SE3([SE3(), SE3()])
Twist3(SE3())

binop(right, op, op2=None, list1=True)

Perform binary operation

Parameters:

left (BasePoseList subclass) – left operand
right (BasePoseList subclass, scalar or array) – right operand
op (callable) – binary operation
op2 (callable) – binary operation
list1 (bool) – return single array as a list, default True

Raises:

ValueError – arguments are not compatible

Returns:

list of values

Return type:

list

The is a helper method for implementing binary operation with overloaded operators such as X * Y where X and Y are both subclasses of BasePoseList. Each operand has a list of one or more values and this methods computes a list of result values according to:

Inputs		Output
len(left)	len(right)	len	operation
1	1	1	`ret = op(left, right)`
1	M	M	`ret[i] = op(left, right[i])`
M	1	M	`ret[i] = op(left[i], right)`
M	M	M	`ret[i] = op(left[i], right[i])`

The arguments to op are the internal numeric values, ie. as returned by the ._A property.

The result is always a list, except for the first case above and list1 is False.

If the right operand is not a BasePoseList subclass, but is a numeric scalar or array then then op2 is invoked

For example:

X._binop(Y, lambda x, y: x + y)

Input	Output
len(left)	len	operation
1	1	`ret = op2(left, right)`
M	M	`ret[i] = op2(left[i], right)`

There is no check on the shape of right if it is an array. The result is always a list, except for the first case above and list1 is False.

clear() → None -- remove all items from S

conjugation(A)

Matrix conjugation

Parameters:: A (ndarray) – matrix to conjugate
Returns:: conjugated matrix
Return type:: ndarray

Compute the conjugation \(\mat{X} \mat{A} \mat{X}^{-1}\) where \(\mat{X}\) is the current object.

Example:

>>> from spatialmath import SO2
>>> import numpy as np
>>> R = SO2(0.5)
>>> A = np.array([[10, 0], [0, 1]])
>>> print(R * A * R.inv())
[[7.9314 3.7866]
 [3.7866 3.0686]]
>>> print(R.conjugation(A))
[[7.9314 3.7866]
 [3.7866 3.0686]]

det()

Determinant of rotational component (superclass method)

Returns:: Determinant of rotational component
Return type:: float or NumPy array

x.det() is the determinant of the rotation component of the values of x.

Example:

>>> x=SE3.Rand()
>>> x.det()
1.0000000000000004
>>> x=SE3.Rand(N=2)
>>> x.det()
[0.9999999999999997, 1.0000000000000002]

SymPy:: not supported

extend(iterable)

Extend sequence of values in an instance (BasePoseList superclass method)

Parameters:: x (instance of same type) – the value to extend
Raises:: ValueError – incorrect type of appended object
Return type:: None

Appends the argument’s values to the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.append(X.Alloc(5))   # extend the list
>>> len(x)
15

where X is any of the SMTB classes.

insert(i, item)

Insert a value to an instance (BasePoseList superclass method)

Parameters:

i (int) – element to insert value before
item (instance of same type) – the value to insert

Raises:

ValueError – incorrect type of inserted value

Return type:

None

Inserts the argument into the object’s internal list of values.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> x.insert(0, X())   # insert at start of list
>>> len(x)
11
>>> x.insert(10, X())   # append to the list
>>> len(x)
11

where X is any of the SMTB classes.

Note

If i is beyond the end of the list, the item is appended to the list

interp(end=None, s=None, shortest=True)

Interpolate between poses (superclass method)

Parameters:

end (same as self) – final pose
s (array_like or int) – interpolation coefficient, range 0 to 1, or number of steps
shortest (bool, default to True) – take the shortest path along the great circle for the rotation

Returns:

interpolated pose

Return type:

same as self

X.interp(Y, s) interpolates pose between X between when s=0 and Y when s=1.
X.interp(Y, N) interpolates pose between X and Y in N steps.

Example:

>>> x = SE3(-1, -2, 0) * SE3.Rx(-0.3)
>>> y = SE3(1, 2, 0) * SE3.Rx(0.3)
>>> x.interp(y, 0)    # this is x
SE3(array([[ 1.    ,  0.    ,  0.    , -1.    ],
           [ 0.    ,  0.9553,  0.2955, -2.    ],
           [ 0.    , -0.2955,  0.9553,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))
>>> x.interp(y, 1)    # this is y
SE3(array([[ 1.    ,  0.    ,  0.    ,  1.    ],
           [ 0.    ,  0.9553, -0.2955,  2.    ],
           [ 0.    ,  0.2955,  0.9553,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))
>>> x.interp(y, 0.5)  # this is in between
SE3(array([[1., 0., 0., 0.],
           [0., 1., 0., 0.],
           [0., 0., 1., 0.],
           [0., 0., 0., 1.]]))
>>> z = x.interp(y, 11)  # in 11 steps
>>> len(z)
11
>>> z[0]              # this is x
SE3(array([[ 1.    ,  0.    ,  0.    , -1.    ],
           [ 0.    ,  0.9553,  0.2955, -2.    ],
           [ 0.    , -0.2955,  0.9553,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))
>>> z[5]              # this is in between
SE3(array([[1., 0., 0., 0.],
           [0., 1., 0., 0.],
           [0., 0., 1., 0.],
           [0., 0., 0., 1.]]))

Note

For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
Values of s outside the range [0,1] are silently clipped

Seealso:: interp1(), trinterp(), qslerp(), trinterp2()
SymPy:: not supported

interp1(s=None)

Interpolate pose (superclass method)

Parameters:

end (same as self) – final pose
s (array_like) – interpolation coefficient, range 0 to 1

Returns:

interpolated pose

Return type:

SO2, SE2, SO3, SE3 instance

X.interp(s) interpolates pose between identity when s=0, and X when s=1.

len(X)

len(s)

len(result)

Result

1

1

1

Y = interp(X, s)

M

1

M

Y[i] = interp(X[i], s)

1

M

M

Y[i] = interp(X, s[i])

Example:

>>> x = SE3.Rx(0.3)
>>> print(x.interp(0))
SE3(array([[1., 0., 0., 0.],
           [0., 1., 0., 0.],
           [0., 0., 1., 0.],
           [0., 0., 0., 1.]]))
>>> print(x.interp(1))
SE3(array([[ 1.        ,  0.        ,  0.        ,  0.        ],
           [ 0.        ,  0.95533649, -0.29552021,  0.        ],
           [ 0.        ,  0.29552021,  0.95533649,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]))
>>> y = x.interp(x, np.linspace(0, 1, 10))
>>> len(y)
10
>>> y[5]
SE3(array([[ 1.        ,  0.        ,  0.        ,  0.        ],
           [ 0.        ,  0.98614323, -0.16589613,  0.        ],
           [ 0.        ,  0.16589613,  0.98614323,  0.        ],
           [ 0.        ,  0.        ,  0.        ,  1.        ]]))

Notes:

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

Seealso:: interp(), trinterp(), qslerp(), trinterp2()
SymPy:: not supported

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 = \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

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

static isvalid(x, check=True)[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()

log(twist=False)

Logarithm of pose (superclass method)

Returns:: logarithm
Return type:: 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()

SymPy:: not supported

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:

Only the direction of A vector (the z-axis) is unchanged.
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
Return type:: None

X.plot() displays the pose X as a coordinate frame in either 2D or 3D. There are many options, see the links below.

Example:

>>> X = SE3.Rx(0.3)
>>> X.plot(frame='A', color='green')

(Source code, png, hires.png, pdf)

Seealso:: trplot(), trplot2()

pop(i=-1)

Pop value from an instance (BasePoseList superclass method)

Parameters:: i (int) – item in the list to pop, default is last
Returns:: the popped value
Return type:: instance of same type
Raises:: IndexError – if there are no values to pop

Removes a value from the value list and returns it. The original instance is modified.

Example:

>>> x = X.Alloc(10)
>>> len(x)
10
>>> y = x.pop()  # pop the last value x[9]
>>> len(x)
9
>>> y = x.pop(0)  # pop the first value x[0]
>>> len(x)
8

where X is any of the SMTB classes.

print(label=None, file=None)

Print pose as a matrix (superclass method)

Parameters:

label (str, optional) – label to print before the matrix, defaults to None
file (file object, optional) – file to write to, defaults to None

Return type:

None

Print the pose as a matrix, with an optional line beforehand. By default the matrix is printed to stdout.

Example:

>>> from spatialmath import SE3
>>> SE3().print()
   1         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1         

>>> SE3().print("pose is:")
pose is:
   1         0         0         0         
   0         1         0         0         
   0         0         1         0         
   0         0         0         1         

Seealso:: printline() strline()

printline(*args, **kwargs)

Print pose in compact single line format (superclass method)

Parameters:

arg (str) – value for orient option, optional
label (str) – text label to put at start of line
fmt (str) – conversion format for each number as used by format()
label – text label to put at start of line
orient (str) – 3-angle convention to use, optional, SO3 and SE3 only
unit (str) – angular units: ‘rad’ [default], or ‘deg’
file (file object) – file to write formatted string to. [default, stdout]

Return type:

None

Print pose in a compact single line format. If X has multiple values, print one per line.

Orientation can be displayed in various formats:

`orient`	description
`'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:

>>> from spatialmath import SE2, SE3
>>> x = SE3.Rx(0.3)
>>> x.printline()
t = 0, 0, 0; rpy/zyx = 17.2°, 0°, 0°
>>> x = SE3.Rx([0.2, 0.3])
>>> x.printline()
t = 0, 0, 0; rpy/zyx = 11.5°, 0°, 0°
t = 0, 0, 0; rpy/zyx = 17.2°, 0°, 0°
>>> x.printline('angvec')
t = 0, 0, 0; angvec = (11.5° | 1, 0, 0)
t = 0, 0, 0; angvec = (17.2° | 1, 0, 0)
>>> x.printline(orient='angvec', fmt="{:.6f}")
t = 0.000000, 0.000000, 0.000000; angvec = (11.459156° | 1.000000, 0.000000, 0.000000)
t = 0.000000, 0.000000, 0.000000; angvec = (17.188734° | 1.000000, 0.000000, 0.000000)
>>> x = SE2(1, 2, 0.3)
>>> x.printline()
t = 1, 2; 17.2°

Note

Default formatting is for compact display of data
For tabular data set fmt to a fixed width format such as fmt='{:.3g}'

Seealso:: strline() trprint(), trprint2()

prod(norm=False, check=True)

Product of elements (superclass method)

Parameters:

norm (bool, optional) – normalize the product, defaults to False
check – check that computed matrix is valid member of group, default True

Bool check:

bool, optional

Returns:

Product of elements

Return type:

pose instance

x.prod() is the product of the values held by x, ie. \(\prod_i^N T_i\).

>>> from spatialmath import SE3
>>> x = SE3.Rx([0, 0.1, 0.2, 0.3])
>>> x.prod()
SE3(array([[ 1.    ,  0.    ,  0.    ,  0.    ],
           [ 0.    ,  0.8253, -0.5646,  0.    ],
           [ 0.    ,  0.5646,  0.8253,  0.    ],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))

Note

When compounding many transformations the product may become denormalized resulting in a result that is not a proper member of the group. You can either disable membership checking by check=False which is risky, or normalize the result by norm=True.

reverse(): S.reverse() – reverse IN PLACE

simplify()

Symbolically simplify matrix values (superclass method)

Returns:: pose with symbolic elements
Return type:: pose instance

Apply symbolic simplification to every element of every value in the pose instance.

Example:

>>> a = SE3.Rx(sympy.symbols('theta'))
>>> b = a * a
>>> b
SE3(array([[1, 0, 0, 0.0],
[0, -sin(theta)**2 + cos(theta)**2, -2*sin(theta)*cos(theta), 0],
[0, 2*sin(theta)*cos(theta), -sin(theta)**2 + cos(theta)**2, 0],
[0.0, 0, 0, 1.0]], dtype=object)
>>> b.simplify()
SE3(array([[1, 0, 0, 0],
[0, cos(2*theta), -sin(2*theta), 0],
[0, sin(2*theta), cos(2*theta), 0],
[0, 0, 0, 1.00000000000000]], dtype=object))

SymPy:: supported

stack()

Convert to 3-dimensional matrix

Returns:: 3-dimensional NumPy array
Return type:: ndarray(n,n,m)

Converts the value to a 3-dimensional NumPy array where the values are stacked along the third axis. The first two dimensions are given by self.shape.

strline(*args, **kwargs)

Convert pose to compact single line string (superclass method)

Parameters:

label (str) – text label to put at start of line
fmt (str) – conversion format for each number as used by format()
label – text label to put at start of line
orient (str) – 3-angle convention to use, optional, SO3 and SE3 only
unit (str) – angular units: ‘rad’ [default], or ‘deg’

Returns:

pose in string format

Return type:

str

Convert pose in a compact single line format. If X has multiple values, the string has one pose per line.

Orientation can be displayed in various formats:

`orient`	description
`'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:

>>> from spatialmath import SE2, SE3
>>> x = SE3.Rx(0.3)
>>> x.strline()
't = 0, 0, 0; rpy/zyx = 17.2°, 0°, 0°'
>>> x = SE3.Rx([0.2, 0.3])
>>> x.strline()
't = 0, 0, 0; rpy/zyx = 11.5°, 0°, 0°t = 0, 0, 0; rpy/zyx = 17.2°, 0°, 0°'
>>> x.strline('angvec')
't = 0, 0, 0; angvec = (11.5° | 1, 0, 0)t = 0, 0, 0; angvec = (17.2° | 1, 0, 0)'
>>> x.strline(orient='angvec', fmt="{:.6f}")
't = 0.000000, 0.000000, 0.000000; angvec = (11.459156° | 1.000000, 0.000000, 0.000000)t = 0.000000, 0.000000, 0.000000; angvec = (17.188734° | 1.000000, 0.000000, 0.000000)'
>>> x = SE2(1, 2, 0.3)
>>> x.strline()
't = 1, 2; 17.2°'

Note

Default formatting is for compact display of data
For tabular data set fmt to a fixed width format such as fmt='{:.3g}'

Seealso:: printline() trprint(), trprint2()

theta(unit='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).

unop(op, matrix=False)

Perform unary operation

Parameters:

self (BasePoseList subclass) – operand
op (callable) – unnary operation
matrix (bool) – return array instead of list, default False

Returns:

operation results

Return type:

list or NumPy array

The is a helper method for implementing unary operations where the operand has multiple value. This method computes the value of the operation for all input values and returns the result as either a list or as a matrix which vertically stacks the results.

Input	Output
len(self)	len	operation
1	1	`ret = op(self)`
M	M	`ret[i] = op(self[i])`
M	M	`ret[i,;] = op(self[i])`

The result is:

a list of values if matrix==False, or
a 2D NumPy stack of values if matrix==True, it is assumed that the value is a 1D array.

xyt()[source]

SE(2) as a configuration vector

Returns:: An array \([x, y, \theta]\) :rtype: 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)

property A: List[ndarray[Any, dtype[ScalarType]]] | ndarray[Any, dtype[ScalarType]]

Array value of an instance (BasePoseList superclass method)

Returns:: NumPy array value of this instance
Return type:: ndarray

X.A is a NumPy array that represents the value of this instance, and has a shape given by X.shape.

Note

This assumes that len(X) == 1, ie. it is a single-valued instance.

property N: int

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:

>>> SE3().N
3
>>> SE2().N
2

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)

property about: str

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]

property isSE: bool

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: bool

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

property shape

Shape of the object’s interal matrix representation

Returns:: (3,3)
Return type:: tuple

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)

property x

First element of the translational component of SE(2)

Parameters:: self (SE2 instance) – SE(2)
Returns:: translational component
Return type:: float

v.x is the first element of the translational vector component. If len(x) is:

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

property y

Second element of the translational component of SE(2)

Parameters:: self (SE2 instance) – SE(2)
Returns:: translational component
Return type:: float

v.y is the second element of the translational vector component. If len(x) is:

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

len(X)	len(s)	len(result)	Result
1	1	1	Y = interp(X, s)
M	1	M	Y[i] = interp(X[i], s)
1	M	M	Y[i] = interp(X, s[i])