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

Inheritance diagram of spatialmath.pose3d.SE3
Ad()[source]

Adjoint of SE(3)

Returns:

adjoint matrix

Return type:

ndarray(6,6)

SE3.Ad is the 6x6 adjoint matrix

If 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 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 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 vector v.

Deprecated since version 0.9.8: Use AngleAxis() instead.

Seealso:

angvec(), EulerVec(), angvec2r()

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 vector v.

\[\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:

angvec(), EulerVec(), angvec2r()

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.    ]]))
Seealso:

eul(), eul2r()

SymPy:

supported

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:

AngVec(), angvec2tr()

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)

Seealso:

trexp(), skew()

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 vectors o and a 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 matrix

  • o and a do not have to be unit-length, they are normalized

  • o and a do not have to be orthogonal, so long as they are not parallel o is adjusted to be orthogonal to a.

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:

oa2r()

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 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. 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.    ]]))
Seealso:

rpy(), rpy2r()

SymPy:

supported

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:

rtvec()

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 vector v1 to v2.

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

  • SE3.Rx(θ, "deg") as above but θ is in degrees

  • SE3.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:

trotx()

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

  • SE3.Ry(θ, "deg") as above but θ is in degrees

  • SE3.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:

troty()

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

  • SE3.Rz(θ, "deg") as above but θ is in degrees

  • SE3.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:

trotz()

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) where t is Nx3 then create an SE3 object with N elements whose translation is defined by the rows of t.

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

Example:

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

transl()

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:

transl()

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

Example:

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

transl()

SymPy:

supported

UnitQuaternion()

SO3 as a unit quaternion instance

Returns:

a unit quaternion representation

Return type:

UnitQuaternion instance

R.UnitQuaternion() is an UnitQuaternion instance representing the same rotation as the SO3 rotation R.

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

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

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

  3. scalar + Pose is handled by __radd__()

  4. Addition is commutative

  5. 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) where T is a 4x4 Numpy array representing an SE(3) matrix. If check is True check the matrix belongs to SE(3).

  • SE3([T1, T2, ... TN]) has N values given by the elements Ti each of which is a 4x4 NumPy array representing an SE(3) matrix. If check is True check the matrix belongs to SE(3).

  • SE3(X) where X is: - SE3 is a copy of X - SO3 is the rotation of X with zero translation - SE2 is the z-axis rotation and x- and y-axis translation of

    X

  • SE3([X1, X2, ... XN]) has N values given by the elements Xi 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 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

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

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

  3. Scalar x Pose is handled by __rmul__`

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

  5. Any other input combinations result in a ValueError.

For pose composition 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

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

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

  3. scalar - Pose is handled by __rsub__()

  4. Any other input combinations result in a ValueError.

For pose 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

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

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

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

  4. Any other input combinations result in a ValueError.

For pose composition 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 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.

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 the X1 frame) from pose X1 to X2.

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:

tr2delta()

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

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)

Seealso:

Eul(), tr2eul()

SymPy:

not supported

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

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

trinv()

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:

ishom()

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:

trlog2(),

trlog()

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

Normalize pose (superclass method)

Returns:

pose

Return type:

SO2, SE2, SO3, SE3 instance

  • X.norm() is an equivalent pose object but the rotational matrix part of all values has been adjusted to ensure it is a proper orthogonal matrix rotation.

Example:

>>> x = SE3()
>>> y = x.norm()
>>> y
SE3(array([[1., 0., 0., 0.],
           [0., 1., 0., 0.],
           [0., 0., 1., 0.],
           [0., 0., 0., 1.]]))

Notes:

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

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

Seealso:

trnorm(), trnorm2()

plot(*args, **kwargs)

Plot pose object as a coordinate frame (superclass method)

Parameters:

**kwargs – plotting options

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)

_images/3d_pose_SE3-1.png
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

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

If len(x) is:

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

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

Seealso:

RPY(), tr2rpy()

SymPy:

not supported

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:

rtvec()

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

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:

spatialmath.twist.Twist3()

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

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

Returns:

rotational component

Return type:

ndarray(3,3)

x.R is the rotation matrix component of x as an array with shape (3,3). If len(x) > 1, return an array with shape=(N,3,3).

Warning

The i’th rotation matrix is x[i,:,:] or simply x[i]. This is different to the MATLAB version where the i’th rotation matrix is x(:,:,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)

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

numpy.ndarray

x.t is the translational component of x as an array with shape (3,). If len(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