SE(2) matrix
- class SE2(*args, **kwargs)[source]
Bases:
spatialmath.pose2d.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).
- 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 classX
withN
default values, ie.len(X)
will beN
.X
can be considered a vector of pose objects, and those elements can be referencedX[i]
or assigned toX[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 classTwist2
orTwist3
it is a zero vector, for aUnitQuaternion
orQuaternion
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
- 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.
- 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-axisExample:
File "<input>", line 1, in <module> NameError: name 'SE2' is not defined
- Seealso
- 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
- 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 ofX
andY
X + s
is the element-wise sum of the matrix value ofX
and scalars
s + X
is the element-wise sum of the scalars
and the matrix value ofX
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
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
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 matrixSE2(θ)
is an SE2 instance representing a pure rotation ofθ
radiansSE2(θ, unit='deg')
as above butθ
in degreesSE2(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_likeSE2(x, y, θ)
is an SE2 instance representing a translation of (x
,y
) and a rotation ofθ
radiansSE2(x, y, θ, unit='deg')
as above butθ
in degreesSE2(t)
wheret``=[x,y] is a 2-element array_like, is an SE2 instance representing a pure translation of (``x
,y
)SE2(q)
whereq``=[x,y,θ] is a 3-element array_like, is an SE2 instance representing a translation of (``x
,y
) and a rotation ofθ
radiansSE2(t, unit='deg')
as above butθ
in degreesSE2(T)
is an SE2 instance with rigid-body motion described by the SE(2) matrix T which is a 3x3 numpy array. Ifcheck
isTrue
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. Ifcheck
isTrue
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 posesX
andY
X * s
performs element-wise multiplication of the elements ofX
bys
s * X
performs element-wise multiplication of the elements ofX
bys
X * v
linear transformation of the vectorv
wherev
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
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
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
andSE3
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. Ifn
< 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 ofX
andY
X - s
is the element-wise difference of the matrix value ofX
and the scalars
s - X
is the element-wise difference of the scalars
and the matrix value ofX
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
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
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 posesX
andY.inv()
X / s
performs elementwise division of the elements ofX
bys
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
orSE3
instanceN is 2 for
SO2
,SE2
; 3 forSO3
orSE3
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
X.animate()
displays the poseX
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 poseX
as a coordinate frame moving from poseX1
, 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
- Return type
None
- 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
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.- Return type
None
- arghandler(arg, convertfrom=(), check=True)
Standard constructor support (BasePoseList superclass method)
- Parameters
arg (
Any
) – initial valueconvertfrom (
Tuple
) – list of classes to accept and convert fromcheck (bool) – check value is valid, defaults to True
- Type
tuple of typles
- Raises
ValueError – bad type passed
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, ifcheck
is True the numerical value is inspected) and converted to the required internal format by the_import
method. The default_import
method calls theisvalid
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 classA
, and an instance of classB
is passed andB
is an element of theconvertfrom
argument, thenB.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())
- Return type
bool
- 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
whereX
andY
are both subclasses ofBasePoseList
. 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
isFalse
.If the right operand is not a
BasePoseList
subclass, but is a numeric scalar or array then thenop2
is invokedFor 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 andlist1
isFalse
.
- 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 ofx
.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
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.- Return type
None
- 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
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- Return type
None
- interp(end=None, s=None, shortest=True)
Interpolate between poses (superclass method)
- Parameters
end (same as
self
) – final poses (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 inN
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
- SymPy
not supported
- interp1(s=None)
Interpolate pose (superclass method)
- Parameters
end (same as
self
) – final poses (array_like) – interpolation coefficient, range 0 to 1
- Returns
interpolated pose
- Return type
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
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
- SymPy
not supported
- norm()
Normalize pose (superclass method)
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’.
- plot(*args, **kwargs)
Plot pose object as a coordinate frame (superclass method)
- Parameters
**kwargs – plotting options
X.plot()
displays the poseX
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)
- 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
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
- Return type
None
- 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
andSE3
onlyunit (str) – angular units: ‘rad’ [default], or ‘deg’
file (file object) – file to write formatted string to. [default, stdout]
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 asfmt='{:.3g}'
- Seealso
strline()
trprint()
,trprint2()
- Return type
None
- 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 byx
, 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 bynorm=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
andSE3
onlyunit (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 asfmt='{:.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
, ora 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]\). Iflen(x)
is:1, return an ndarray with shape=(3,)
N>1, return an ndarray with shape=(N,3)
- property A: Union[List[numpy.ndarray[Any, numpy.dtype[numpy.typing._generic_alias.ScalarType]]], numpy.ndarray[Any, numpy.dtype[numpy.typing._generic_alias.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 byX.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
orSE2
, and 3 forSO3
orSE3
. 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)
- property isSO: bool
Test if object belongs to SO(n) group (superclass property)
- 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
x.t
is the translational vector component. Iflen(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. Iflen(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. Iflen(x)
is:1, return an float
N>1, return an ndarray with shape=(N,)