SE(3) matrix
- class SE3(*args, **kwargs)[source]
Bases:
SO3
SE(3) matrix class
This subclass represents rigid-body motion in 3D space. Internally it is a 4x4 homogeneous transformation matrix belonging to the group SE(3).
- Ad()[source]
Adjoint of SE(3)
- Returns:
adjoint matrix
- Return type:
ndarray(6,6)
SE3.Ad
is the 6x6 adjoint matrixIf spatial velocity \(\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T\) and the SE(3) represents the pose of {B} relative to {A}, ie. \({}^A {\bf T}_B\), and the adjoint is \(\mathbf{A}\) then \({}^{A}\!\nu = \mathbf{A} {}^{B}\!\nu\).
Warning
Do not use this method to map velocities between robot base and end-effector frames - use
jacob()
.Note
Use this method to map velocities between two frames on the same rigid-body.
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- Seealso:
SE3.jacob, Twist.ad,
tr2jac()
- SymPy:
supported
- 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 AngVec(theta, v, *, unit='rad')[source]
Create an SE(3) pure rotation matrix from rotation angle and axis
- Parameters:
θ (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like(3)) – rotation axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.AngVec(θ, v)
is an SE(3) rotation defined by a rotation ofθ
about the vectorv
.Deprecated since version 0.9.8: Use
AngleAxis()
instead.- Seealso:
- classmethod AngleAxis(theta, v, *, unit='rad')[source]
Create an SE(3) pure rotation matrix from rotation angle and axis
- Parameters:
θ (float) – rotation
unit (str) – angular units: ‘rad’ [default], or ‘deg’
v (array_like(3)) – rotation axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.AngleAxis(θ, v)
is an SE(3) rotation defined by a rotation ofθ
about the vectorv
.\[\begin{split}\mbox{if}\,\, \theta \left\{ \begin{array}{ll} = 0 & \mbox{return identity matrix}\\ \ne 0 & \mbox{v must have a finite length} \end{array} \right.\end{split}\]- Seealso:
- classmethod CopyFrom(T, check=True)[source]
Create an SE(3) from a 4x4 numpy array that is passed by value.
- Parameters:
T (ndarray(4, 4)) – homogeneous transformation
check (bool, optional) – check rotation validity, defaults to True
- Raises:
ValueError – bad rotation matrix, bad transformation matrix
- Returns:
SE(3) matrix representing that transformation
- Return type:
SE3 instance
- classmethod Delta(d)[source]
Create SE(3) from differential motion
- Parameters:
d (array_like(6)) – differential motion
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.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 for Python, Section 3.1, P. Corke, Springer 2023.
- Seealso:
delta()
delta2tr()
- SymPy:
supported
- 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 Eul(*angles, unit='rad')[source]
Create an SE(3) pure rotation from Euler angles
- Parameters:
𝚪 (3 floats, array_like(3) or ndarray(N,3)) – Euler angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Eul(𝚪)
is an SE(3) rotation defined by a 3-vector of Euler angles \(\Gamma=(\phi, \theta, \psi)\) which correspond to consecutive rotations about the Z, Y, Z axes respectively.
If
𝚪
is an Nx3 matrix then the result is a sequence of rotations each defined by Euler angles corresponding to the rows of𝚪
.SE3.Eul(φ, θ, ψ)
as above but the angles are provided as three scalars.
Example:
>>> from spatialmath import SE3 >>> SE3.Eul(0.1, 0.2, 0.3) SE3(array([[ 0.9021, -0.3836, 0.1977, 0. ], [ 0.3875, 0.9216, 0.0198, 0. ], [-0.1898, 0.0587, 0.9801, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.Eul([0.1, 0.2, 0.3]) SE3(array([[ 0.9021, -0.3836, 0.1977, 0. ], [ 0.3875, 0.9216, 0.0198, 0. ], [-0.1898, 0.0587, 0.9801, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.Eul(10, 20, 30, unit="deg") SE3(array([[ 0.7146, -0.6131, 0.3368, 0. ], [ 0.6337, 0.7713, 0.0594, 0. ], [-0.2962, 0.171 , 0.9397, 0. ], [ 0. , 0. , 0. , 1. ]]))
- classmethod EulerVec(w)[source]
Construct a new SE(3) pure rotation matrix from an Euler rotation vector
- Parameters:
ω (array_like(3)) – rotation axis
- Returns:
SE(3) rotation
- Return type:
SE3 instance
SE3.EulerVec(ω)
is a unit quaternion that describes the 3D rotation defined by a rotation of \(\theta = \lVert \omega \rVert\) about the unit 3-vector \(\omega / \lVert \omega \rVert\).Example:
>>> from spatialmath import SE3 >>> SE3.EulerVec([0.5,0,0]) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.8776, -0.4794, 0. ], [ 0. , 0.4794, 0.8776, 0. ], [ 0. , 0. , 0. , 1. ]]))
Note
\(\theta = 0\) the result in an identity matrix, otherwise
V
must have a finite length, ie. \(|V| > 0\).- Seealso:
- classmethod Exp(S, check=True)[source]
Create an SE(3) matrix from se(3)
- Parameters:
S (ndarray(6), ndarray(4,4)) – Lie algebra se(3) matrix
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Exp(S)
is an SE(3) rotation defined by its Lie algebra which is a 4x4 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)
- classmethod OA(o, a)[source]
Create an SE(3) pure rotation from two vectors
- Parameters:
o (array_like(3)) – 3-vector parallel to Y- axis
a (array_like(3)) – 3-vector parallel to the Z-axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.OA(o, a)
is an SE(3) rotation defined in terms of vectorso
anda
respectively 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 \(\mathbf{R} = [n, o, a]\) and \(n = o \times a\).Note
The
a
vector is the only guaranteed to have the same direction in the resulting rotation matrixo
anda
do not have to be unit-length, they are normalizedo
anda
do not have to be orthogonal, so long as they are not parallelo
is adjusted to be orthogonal toa
.
Example:
>>> from spatialmath import SE3 >>> SE3.OA([1, 0, 0], [0, 0, -1]) SE3(array([[ 0., 1., 0., 0.], [ 1., 0., 0., 0.], [ 0., 0., -1., 0.], [ 0., 0., 0., 1.]]))
- Seealso:
- classmethod RPY(*angles, unit='rad', order='zyx')[source]
Create an SE(3) pure rotation from roll-pitch-yaw angles
- Parameters:
𝚪 (3 floats, array_like(3) or ndarray(N,3)) – roll-pitch-yaw angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
order (str) – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.RPY(𝚪)
is an SE(3) rotation defined by a 3-vector of roll, pitch, yaw angles \(\Gamma=(r, p, y)\) which correspond to successive rotations about the axes specified byorder
:'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. This is the 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. This is the 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. This is the convention for a camera with z-axis parallel to the optical axis and x-axis parallel to the pixel rows.
If
𝚪
is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles corresponding to the rows of𝚪
.SE3.RPY(⍺, β, 𝛾)
as above but the angles are provided as three scalars.
Example:
>>> from spatialmath import SE3 >>> SE3.RPY(0.1, 0.2, 0.3) SE3(array([[ 0.9363, -0.2751, 0.2184, 0. ], [ 0.2896, 0.9564, -0.037 , 0. ], [-0.1987, 0.0978, 0.9752, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.RPY([0.1, 0.2, 0.3]) SE3(array([[ 0.9363, -0.2751, 0.2184, 0. ], [ 0.2896, 0.9564, -0.037 , 0. ], [-0.1987, 0.0978, 0.9752, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.RPY(0.1, 0.2, 0.3, order='xyz') SE3(array([[ 0.9752, -0.0978, 0.1987, 0. ], [ 0.1538, 0.9447, -0.2896, 0. ], [-0.1593, 0.313 , 0.9363, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.RPY(10, 20, 30, unit='deg') SE3(array([[ 0.8138, -0.441 , 0.3785, 0. ], [ 0.4698, 0.8826, 0.018 , 0. ], [-0.342 , 0.1632, 0.9254, 0. ], [ 0. , 0. , 0. , 1. ]]))
- classmethod RTvec(rvec, tvec)[source]
Construct a new SE(3) from OpenCV-style rotation and translation vectors
- Parameters:
rvec (ArrayLike3) – rotation as exponential coordinates
tvec (ArrayLike3) – translation vector
- Returns:
An SE(3) instance
- Return type:
SE3 instance
Many OpenCV functions (such as pose estimation) return pose as two 3-vectors: a rotation vector using exponential coordinates and a translation vector. This method combines them into an SE(3) instance.
- Seealso:
- classmethod Rand(N=1, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), theta_range=None, unit='rad')[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]
theta_range (
Union
[List
,Tuple
[float
,float
],ndarray
[Any
,dtype
[TypeVar
(ScalarType
, bound=generic
, covariant=True)]],None
]) – angular magnitude range [min,max], defaults to None -> [0,pi].unit (str) – angular units: ‘rad’ [default], or ‘deg’
N (int) – number of random transforms
- Returns:
SE(3) 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.
Example:
>>> from spatialmath import SE3 >>> SE3.Rand(2) SE3([ array([[-0.0996, -0.2704, 0.9576, -0.4312], [ 0.6104, 0.7434, 0.2735, 0.7751], [-0.7858, 0.6118, 0.091 , -0.5214], [ 0. , 0. , 0. , 1. ]]), array([[ 0.2253, -0.3794, -0.8974, 0.3533], [ 0.8095, 0.5855, -0.0443, 0.5428], [ 0.5422, -0.7165, 0.439 , -0.832 ], [ 0. , 0. , 0. , 1. ]]) ])
- Seealso:
Rand()
- classmethod RotatedVector(v1, v2, tol=20)
Construct a new SO(3) from a vector and its rotated image
- Parameters:
v1 (array_like(3)) – initial vector
v2 (array_like(3)) – vector after rotation
tol (float) – tolerance for singularity in units of eps, defaults to 20
- Returns:
SO(3) rotation
- Return type:
SO3
instance
SO3.RotatedVector(v1, v2)
is an SO(3) rotation defined in terms of two vectors. The rotation takes vectorv1
tov2
.>>> from spatialmath import SO3 >>> v1 = [1, 2, 3] >>> v2 = SO3.Eul(0.3, 0.4, 0.5) * v1 >>> print(v2) [[0.3842] [2.4579] [2.7948]] >>> R = SO3.RotatedVector(v1, v2) >>> print(R) 0.9857 -0.1131 -0.1251 0.1282 0.9844 0.1203 0.1095 -0.1346 0.9848 >>> print(R * v1) [[0.3842] [2.4579] [2.7948]]
Note
The vectors do not have to be unit-length.
- classmethod Rt(R, t=None, check=True)[source]
Create an SE(3) from rotation and translation
- Parameters:
R (SO3 or ndarray(3,3)) – rotation
t (array_like(3)) – translation
check (bool, optional) – check rotation validity, defaults to True
- Raises:
ValueError – bad rotation matrix
- Returns:
SE(3) matrix
- Return type:
SE3 instance
- classmethod Rx(theta, unit='rad', t=None)[source]
Create anSE(3) pure rotation about the X-axis
- Parameters:
θ (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (3-element array-like) – translation, optional
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Rx(θ)
is an SE(3) rotation of θ radians about the x-axisSE3.Rx(θ, "deg")
as above but θ is in degreesSE3.Rx(θ, t=T)
as above but also sets the translational component
If
θ
is an array then the result is a sequence of rotations defined by consecutive elements.Note
The translation option only works for the scalar θ case.
Example:
>>> from spatialmath import SE3 >>> SE3.Rx(0.3) SE3(array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.Rx([0.3, 0.4]) SE3([ array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]]), array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9211, -0.3894, 0. ], [ 0. , 0.3894, 0.9211, 0. ], [ 0. , 0. , 0. , 1. ]]) ])
- Seealso:
- SymPy:
supported
- classmethod Ry(theta, unit='rad', t=None)[source]
Create an SE(3) pure rotation about the Y-axis
- Parameters:
θ (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (3-element array-like) – translation, optional
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Ry(θ)
is an SO(3) rotation of θ radians about the y-axisSE3.Ry(θ, "deg")
as above but θ is in degreesSE3.Ry(θ, t=T)
as above but also sets the translational component
If
θ
is an array then the result is a sequence of rotations defined by consecutive elements.Note
The translation option only works for the scalar θ case.
Example:
>>> from spatialmath import SE3 >>> SE3.Ry(0.3) SE3(array([[ 0.9553, 0. , 0.2955, 0. ], [ 0. , 1. , 0. , 0. ], [-0.2955, 0. , 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.Ry([0.3, 0.4]) SE3([ array([[ 0.9553, 0. , 0.2955, 0. ], [ 0. , 1. , 0. , 0. ], [-0.2955, 0. , 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]]), array([[ 0.9211, 0. , 0.3894, 0. ], [ 0. , 1. , 0. , 0. ], [-0.3894, 0. , 0.9211, 0. ], [ 0. , 0. , 0. , 1. ]]) ])
- Seealso:
- SymPy:
supported
- classmethod Rz(theta, unit='rad', t=None)[source]
Create an SE(3) pure rotation about the Z-axis
- Parameters:
θ (float) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
t (3-element array-like) – translation, optional
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Rz(θ)
is an SO(3) rotation of θ radians about the z-axisSE3.Rz(θ, "deg")
as above but θ is in degreesSE3.Rz(θ, t=T)
as above but also sets the translational component
If
θ
is an array then the result is a sequence of rotations defined by consecutive elements.Note
The translation option only works for the scalar θ case.
Example:
>>> from spatialmath import SE3 >>> SE3.Rz(0.3) SE3(array([[ 0.9553, -0.2955, 0. , 0. ], [ 0.2955, 0.9553, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]])) >>> SE3.Rz([0.3, 0.4]) SE3([ array([[ 0.9553, -0.2955, 0. , 0. ], [ 0.2955, 0.9553, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]), array([[ 0.9211, -0.3894, 0. , 0. ], [ 0.3894, 0.9211, 0. , 0. ], [ 0. , 0. , 1. , 0. ], [ 0. , 0. , 0. , 1. ]]) ])
- Seealso:
- SymPy:
supported
- classmethod Trans(x, y=None, z=None)[source]
Create SE(3) from translation vector
- Parameters:
x (float or array_like(3)) – x-coordinate or translation vector
y (float, optional) – y-coordinate, defaults to None
z (float, optional) – z-coordinate, defaults to None
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Trans(x, y, z)
is an SE(3) representing pure translation.SE3.Trans([x, y, z])
as above, but translation is given as an array.SE3.Trans(t)
wheret
is Nx3 then create an SE3 object with N elements whose translation is defined by the rows oft
.
- classmethod TwoVectors(x=None, y=None, z=None)
Construct a new SO(3) from any two vectors
- Parameters:
x (str, array_like(3), optional) – new x-axis, defaults to None
y (str, array_like(3), optional) – new y-axis, defaults to None
z (str, array_like(3), optional) – new z-axis, defaults to None
- Return type:
Self
Create a rotation by defining the direction of two of the new axes in terms of the old axes. Axes are denoted by strings
"x"
,"y"
,"z"
,"-x"
,"-y"
,"-z"
.The directions can also be specified by 3-element vectors. If the vectors are not orthogonal, they will orthogonalized w.r.t. the first available dimension. I.e. if x is available, it will be normalized and the remaining vector will be orthogonalized w.r.t. x, else, y will be normalized and z will be orthogonalized w.r.t. y.
To create a rotation where the new frame has its x-axis in -z-direction of the previous frame, and its z-axis in the x-direction of the previous frame is:
>>> SO3.TwoVectors(x='-z', z='x')
- classmethod Tx(x)[source]
Create an SE(3) translation along the X-axis
- Parameters:
x (float) – translation distance along the X-axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Tx(x) is an SE(3) translation of
x
along the x-axisExample:
>>> from spatialmath import SE3 >>> SE3.Tx(2) SE3(array([[1., 0., 0., 2.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> SE3.Tx([2,3]) SE3([ array([[1., 0., 0., 2.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), array([[1., 0., 0., 3.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) ])
- Seealso:
- SymPy:
supported
- classmethod Ty(y)[source]
Create an SE(3) translation along the Y-axis
- Parameters:
y (float) – translation distance along the Y-axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Ty(y) is an SE(3) translation of ``y` along the y-axis
Example:
>>> from spatialmath import SE3 >>> SE3.Ty(2) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 2.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> SE3.Ty([2,3]) SE3([ array([[1., 0., 0., 0.], [0., 1., 0., 2.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), array([[1., 0., 0., 0.], [0., 1., 0., 3.], [0., 0., 1., 0.], [0., 0., 0., 1.]]) ])
- Seealso:
- SymPy:
supported
- classmethod Tz(z)[source]
Create an SE(3) translation along the Z-axis
- Parameters:
z (float) – translation distance along the Z-axis
- Returns:
SE(3) matrix
- Return type:
SE3 instance
SE3.Tz(z) is an SE(3) translation of
z
along the z-axisExample:
>>> from spatialmath import SE3 >>> SE3.Tz(2) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 2.], [0., 0., 0., 1.]])) >>> SE3.Tz([2,3]) SE3([ array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 2.], [0., 0., 0., 1.]]), array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 3.], [0., 0., 0., 1.]]) ])
- Seealso:
- SymPy:
supported
- UnitQuaternion()
SO3 as a unit quaternion instance
- Returns:
a unit quaternion representation
- Return type:
UnitQuaternion instance
R.UnitQuaternion()
is anUnitQuaternion
instance representing the same rotation as the SO3 rotationR
.Example:
>>> from spatialmath import SO3 >>> SO3.Rz(0.3).UnitQuaternion() UnitQuaternion(array([0.9888, 0. , 0. , 0.1494]))
- __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, z=None, *, check=True)[source]
Construct new SE(3) object
- Return type:
SE3 instance
There are multiple call signatures that return an
SE3
instance with one or more values.SE3()
null motion, value is the identity matrix.SE3(x, y, z)
is a pure translation of (x,y,z)SE3(T)
whereT
is a 4x4 Numpy array representing an SE(3) matrix. Ifcheck
isTrue
check the matrix belongs to SE(3).SE3([T1, T2, ... TN])
hasN
values given by the elementsTi
each of which is a 4x4 NumPy array representing an SE(3) matrix. Ifcheck
isTrue
check the matrix belongs to SE(3).SE3(X)
whereX
is: -SE3
is a copy ofX
-SO3
is the rotation ofX
with zero translation -SE2
is the z-axis rotation and x- and y-axis translation ofX
SE3([X1, X2, ... XN])
hasN
values given by the elementsXi
each of which is an SE3 instance.
- SymPy:
supported
- __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()
- angdist(other, metric=6)[source]
Angular distance metric between poses
- Parameters:
other (SE3 instance) – second rotation
metric (int) – metric, default is 6
- Raises:
TypeError – if other is not an SE3
- Returns:
angle in radians
- Return type:
float or ndarray
T1.angdist(T2)
is the geodesic norm, or geodesic distance between the rotational parts of the two poses.Several metrics are supported, the first 5 are computed after conversion to unit quaternions.
Metric
Details
0
\(1 - | \q_1 \bullet \q_2 | \in [0, 1]\)
1
\(\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]\)
2
\(\cos^{-1} | \q_1 \bullet \q_2 | \in [0, \pi/2]\)
3
\(2 \tan^{-1} \| \q_1 - \q_2\| / \|\q_1 + \q_2\| \in [0, \pi/2]\)
4
\(\cos^{-1} \left( 2 (\q_1 \bullet \q_2)^2 - 1\right) \in [0, 1]\)
5
\(\|I - \mat{R}_1 \mat{R}_2^T\| \in [0, 2]\)
6
\(\|\log \mat{R}_1 \mat{R}_2^T\| \in [0, \pi]\)
Example:
>>> from spatialmath import SE3 >>> T1 = SE3.Rx(0.3) >>> T2 = SE3.Ry(0.3) >>> print(T1.angdist(T1)) 0.0 >>> print(T1.angdist(T2)) 0.4234654354756045
Note
metrics 1, 2, 4 can throw ValueError “math domain error” due to numeric errors which push the argument of
acos()
marginally outside its domain [0, 1].metrics 2 and 3 are equivalent, but 3 is more robust
- Seealso:
UnitQuaternion.angdist()
- angvec(unit='rad')
SO(3) or SE(3) as angle and rotation vector
- Parameters:
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
\((\theta, \hat{\bf v})\)
- Return type:
float or ndarray(3)
x.angvec()
is a tuple \((\theta, v)\) containing the rotation angle and a rotation axis.By default the angle is in radians but can be changed setting unit=’deg’.
Note
If the input is SE(3) the translation component is ignored.
Example:
>>> from spatialmath import SO3 >>> R = SO3.Rx(0.3) >>> R.angvec() (0.3, array([1., 0., 0.]))
- Seealso:
eulervec()
AngVec()
angvec()
AngVec()
,angvec2r()
- 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 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:
- 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.
- 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]]
- delta(X2=None)[source]
Infinitesimal difference of SE(3) values
- Returns:
differential motion vector
- Return type:
ndarray(6)
X1.delta(X2)
is the differential motion (6x1) corresponding to infinitesimal motion (in theX1
frame) from poseX1
toX2
.The vector \(d = [\delta_x, \delta_y, \delta_z, \theta_x, \theta_y, \theta_z]\) represents infinitesimal translation and rotation.
Example:
>>> from spatialmath import SE3 >>> x1 = SE3.Rx(0.3) >>> x2 = SE3.Rx(0.3001) >>> x1.delta(x2) array([0. , 0. , 0. , 0.0001, 0. , 0. ])
Note
the displacement is only an approximation to the motion, and assumes that
X1
~X2
.can be considered as an approximation to the effect of spatial velocity over a a time interval, ie. the average spatial velocity multiplied by time.
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- Seealso:
- 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
- eul(unit='rad', flip=False)
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:
ndarray(3,), ndarray(n,3)
x.eul
is the Euler angle representation of the rotation. Euler angles are a 3-vector \((\phi, \theta, \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=(3,N)
- eulervec()
SO(3) or SE(3) as Euler vector (exponential coordinates)
- Returns:
\(\theta \hat{\bf v}\)
- Return type:
ndarray(3)
x.eulervec()
is the Euler vector (or exponential coordinates) which is related to angle-axis notation and is the product of the rotation angle and the rotation axis.Example:
>>> from spatialmath import SO3 >>> R = SO3.Rx(0.3) >>> R.eulervec() array([0.3, 0. , 0. ])
Note
If the input is SE(3) the translation component is ignored.
- Seealso:
angvec()
angvec2r()
- 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 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(3)
- Returns:
inverse
- Return type:
SE3 instance
Efficiently compute the inverse of each of the SE(3) values taking into account the matrix structure.
\[\begin{split}T = \left[ \begin{array}{cc} \mat{R} & \vec{t} \\ 0 & 1 \end{array} \right], \mat{T}^{-1} = \left[ \begin{array}{cc} \mat{R}^T & -\mat{R}^T \vec{t} \\ 0 & 1 \end{array} \right]`\end{split}\]Example:
>>> from spatialmath import SE3 >>> x = SE3(1,2,3) >>> x.inv() SE3(array([[ 1., 0., 0., -1.], [ 0., 1., 0., -2.], [ 0., 0., 1., -3.], [ 0., 0., 0., 1.]]))
- Seealso:
- SymPy:
supported
- 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 a valid SE(3)
- Parameters:
x (numpy.ndarray) – matrix to test
- Returns:
True
if the matrix is 4x4 and a valid element of SE(3), ie. it is a valid homogeneous transformation matrix.- Return type:
bool
- Seealso:
- jacob()[source]
Velocity transform for SE(3)
- Returns:
Jacobian matrix
- Return type:
ndarray(6,6)
SE3.jacob()
is the 6x6 Jacobian that maps spatial velocity or differential motion from frame {B} to frame {A} where the pose of {B} relative to {A} is represented by the homogeneous transform T = \({}^A {\bf T}_B\).Note
To map from frame {A} to frame {B} use the transpose of this matrix.
Use this method to map velocities between the robot end-effector frame and the base frames.
Warning
Do not use this method to map velocities between two frames on the same rigid-body.
- Seealso:
SE3.Ad, Twist.ad,
tr2jac()
- Reference:
Robotics, Vision & Control for Python, Section 3.1, P. Corke, Springer 2023.
- SymPy:
supported
- 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
- mean(tol=20)
Mean of a set of rotations
- Parameters:
tol (float, optional) – iteration tolerance in units of eps, defaults to 20
- Returns:
the mean rotation
- Return type:
SO3
instance.
Computes the Karcher mean of the set of rotations within the SO(3) instance.
- References:
**Hartley, Trumpf** - “Rotation Averaging” - IJCV 2011, Algorithm 1, page 15.
- 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
- Return type:
None
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
- 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(*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]
- 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 asfmt='{:.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 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
- rpy(unit='rad', 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:
ndarray(3,), ndarray(n,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 byorder
:'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.
If len(x) is:
1, return an ndarray with shape=(3,)
N>1, return ndarray with shape=(3,N)
- rtvec()[source]
Convert to OpenCV-style rotation and translation vectors
- Returns:
rotation and translation vectors
- Return type:
ndarray(3), ndarray(3)
Many OpenCV functions accept pose as two 3-vectors: a rotation vector using exponential coordinates and a translation vector. This method combines them into an SE(3) instance.
- Seealso:
- 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()
- twist()[source]
SE(3) as twist
- Returns:
equivalent rigid-body motion as a twist vector
- Return type:
Twist3 instance
Example:
>>> from spatialmath import SE3 >>> x = SE3(1,2,3) >>> x.twist() Twist3([1, 2, 3, 0, 0, 0])
- Seealso:
- yaw_SE2(order='zyx')[source]
Create SE(2) from SE(3) yaw angle.
- Parameters:
order (str) – angle sequence order, default to ‘zyx’
- Returns:
SE(2) with same rotation as the yaw angle using the roll-pitch-yaw convention, and translation along the roll-pitch axes.
- Return type:
SE2 instance
Roll-pitch-yaw corresponds 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.
- 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 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: SO3Array
SO(3) or SE(3) as rotation matrix
- Returns:
rotational component
- Return type:
ndarray(3,3)
x.R
is the rotation matrix component ofx
as an array with shape (3,3). Iflen(x) > 1
, return an array with shape=(N,3,3).Warning
The i’th rotation matrix is
x[i,:,:]
or simplyx[i]
. This is different to the MATLAB version where the i’th rotation matrix isx(:,:,i)
.Example:
>>> from spatialmath import SO3 >>> x = SO3.Rx(0.3) >>> x.R array([[ 1. , 0. , 0. ], [ 0. , 0.9553, -0.2955], [ 0. , 0.2955, 0.9553]])
- SymPy:
supported
- property a: ndarray[Any, dtype[floating]]
Approach vector of SO(3) or SE(3)
- Returns:
approach vector
- Return type:
ndarray(3)
This is the third column of the rotation submatrix, sometimes called the approach vector. It is parallel to the z-axis of the frame defined by this pose.
- 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 n: ndarray[Any, dtype[floating]]
Normal vector of SO(3) or SE(3)
- Returns:
normal vector
- Return type:
ndarray(3)
This is the first column of the rotation submatrix, sometimes called the normal vector. It is parallel to the x-axis of the frame defined by this pose.
- property o: ndarray[Any, dtype[floating]]
Orientation vector of SO(3) or SE(3)
- Returns:
orientation vector
- Return type:
ndarray(3)
This is the second column of the rotation submatrix, sometimes called the orientation vector. It is parallel to the y-axis of the frame defined by this pose.
- property shape: Tuple[int, int]
Shape of the object’s internal matrix representation
- Returns:
(4,4)
- Return type:
tuple
Each value within the
SE3
instance is a NumPy array of this shape.
- property t: ndarray[Any, dtype[floating]]
Translational component of SE(3)
- Returns:
translational component of SE(3)
- Return type:
x.t
is the translational component ofx
as an array with shape (3,). Iflen(x) > 1
, return an array with shape=(N,3).Example:
>>> from spatialmath import SE3 >>> x = SE3(1,2,3) >>> x.t array([1., 2., 3.]) >>> x = SE3([ SE3(1,2,3), SE3(4,5,6)]) >>> x.t array([[1., 2., 3.], [4., 5., 6.]])
- SymPy:
supported
- property x: float
First element of translational component of SE(3)
- Returns:
first element of translational component of SE(3)
- Return type:
float
If
len(v) > 1
, return an array with shape=(N,).Example:
>>> from spatialmath import SE3 >>> v = SE3(1,2,3) >>> v.x 1.0 >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)]) >>> v.x array([1., 4.])
- SymPy:
supported
- property y: float
Second element of translational component of SE(3)
- Returns:
second element of translational component of SE(3)
- Return type:
float
If
len(v) > 1
, return an array with shape=(N,).Example:
>>> from spatialmath import SE3 >>> v = SE3(1,2,3) >>> v.y 2.0 >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)]) >>> v.y array([2., 5.])
- SymPy:
supported
- property z: float
Third element of translational component of SE(3)
- Returns:
third element of translational component of SE(3)
- Return type:
float
If
len(v) > 1
, return an array with shape=(N,).Example:
>>> from spatialmath import SE3 >>> v = SE3(1,2,3) >>> v.z 3.0 >>> v = SE3([ SE3(1,2,3), SE3(4,5,6)]) >>> v.z array([3., 6.])
- SymPy:
supported