se(3) twist
- class Twist3(arg=None, w=None, check=True)[source]
Bases:
BaseTwist3D twist class
A Twist class holds the parameters of a twist, a representation of a 3D rigid body transformation which is the unique elements of the Lie algebra se(3) of the corresponding SE(3) matrix.
A twist can be used in two ways which share the same 6-vector, the same \([\vec{v}, \omega]\) layout and the same algebra:
as a displacement, where
exp(S)is the pose reached by following the screw motion andexp(S * theta)scales it.Sis often a unit twist, seeunit(). This is the usage in Robotics, Vision & Control and in Modern Robotics.as a velocity, where
Shas units of m/s and rad/s andexp(S * dt)is the pose change over a timedt. This is the meaning ofTwistin ROS. The typed counterpart, which also has acceleration, force and momentum, isSpatialVelocity.
Properties that describe the screw axis, such as
pitch,poleandline(), are unchanged by scaling the twist so apply to both. Those that depend on its magnitude, such astheta, are an angle in the first usage and a rate in the second.- References:
Robotics, Vision & Control for Python, Section 2.3.2.3, P. Corke, Springer 2023.
Modern Robotics, Lynch & Park, Cambridge 2017
Note
Compared to Lynch & Park this module implements twist vectors with the translational components first, followed by rotational components, ie. \([\vec{v}, \omega]\).
- classmethod Alloc(n=1)
Construct an instance with N default values (BasePoseList superclass method)
- Parameters:
n (
Optional[int]) – Number of values, defaults to 1- Return type:
Self- Returns:
pose instance with
ndefault values
X.Alloc(N)creates an instance of the pose classXwithNdefault values, ie.len(X)will beN.Xcan 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,SE3it is an identity matrix, for a twist classTwist2orTwist3it is a zero vector, for aUnitQuaternionorQuaternionit is a zero vector.Example:
>>> x = X.Alloc(10) >>> len(x) 10
where
Xis any of the SMTB classes.
- classmethod Empty()
Construct an empty instance (BasePoseList superclass method)
- Return type:
Self- Returns:
pose instance with zero values
Example:
>>> x = X.Empty() >>> len(x) 0
where
Xis any of the SMTB classes.
- classmethod RPY(*pos, **kwargs)[source]
Create a new 3D twist from roll-pitch-yaw angles
- Parameters:
𝚪 (array_like or numpy.ndarray with shape=(N,3)) – roll-pitch-yaw angles
unit (str) – angular units: ‘rad’ [default], or ‘deg’
order (str) – rotation order: ‘zyx’ [default], ‘xyz’, or ‘yxz’
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.RPY(𝚪)is a 3D 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𝚪.Twist3.RPY(⍺, β, 𝛾)as above but the angles are provided as three scalars.
Foo bar!
Example:
>>> from spatialmath import Twist3 >>> Twist3.RPY(0.1, 0.2, 0.3) Twist3([0, 0, 0, 0.068925, 0.21323, 0.28875]) >>> Twist3.RPY([0.1, 0.2, 0.3]) Twist3([0, 0, 0, 0.068925, 0.21323, 0.28875]) >>> Twist3.RPY(0.1, 0.2, 0.3, order='xyz') Twist3([0, 0, 0, 0.30875, 0.18343, 0.12892]) >>> Twist3.RPY(10, 20, 30, unit='deg') Twist3([0, 0, 0, 0.077525, 0.38485, 0.48648])
- Seealso:
RPY()- SymPy:
supported
- classmethod Rand(*, xrange=(-1, 1), yrange=(-1, 1), zrange=(-1, 1), N=1)[source]
Create a new random 3D twist
- Parameters:
xrange (2-element sequence, optional) – x-axis range [min,max], defaults to [-1, 1]
yrange (2-element sequence, optional) – y-axis range [min,max], defaults to [-1, 1]
zrange (2-element sequence, optional) – z-axis range [min,max], defaults to [-1, 1]
N (int) – number of random transforms
- Returns:
3D twist
- Return type:
Twist3 instance
Return a Twist3 instance with random rotation and translation.
Twist3.Rand()is a random se(3) twist.Twist3.Rand(N=N)is a Twist3 object containing a sequence of N random twists.
Example:
>>> from spatialmath import Twist3 >>> Twist3.Rand(N=2) Twist3([ [-0.52105, 0.40921, 1.498, 3.0693, -0.26608, 0.45241], [1.3623, -0.45062, -0.33081, -1.5331, 0.0076993, -1.0719] ])
- Seealso:
- classmethod Rx(theta, unit='rad')[source]
Create a new 3D twist for pure rotation about the X-axis
- Parameters:
θ (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Rx(θ)is an se(3) rotation of θ radians about the x-axisTwist3.Rx(θ, "deg")as above but θ is in degrees
If
θis an array then the result is a sequence of rotations defined by consecutive elements.Example:
- Seealso:
trotx()- SymPy:
supported
- classmethod Ry(theta, unit='rad', t=None)[source]
Create a new 3D twist for pure rotation about the Y-axis
- Parameters:
θ (float) – rotation angle about X-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Ry(θ)is an se(3) rotation of θ radians about the y-axisTwist3.Ry(θ, "deg")as above but θ is in degrees
If
θis an array then the result is a sequence of rotations defined by consecutive elements.Example:
- Seealso:
troty()- SymPy:
supported
- classmethod Rz(theta, unit='rad', t=None)[source]
Create a new 3D twist for pure rotation about the Z-axis
- Parameters:
θ (float) – rotation angle about Z-axis
unit (str) – angular units: ‘rad’ [default], or ‘deg’
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Rz(θ)is an se(3) rotation of θ radians about the z-axisTwist3.Rz(θ, "deg")as above but θ is in degrees
If
θis an array then the result is a sequence of rotations defined by consecutive elements.Example:
- Seealso:
trotz()- SymPy:
supported
- classmethod Tx(x)[source]
Create a new 3D twist for pure translation along the X-axis
- Parameters:
x (float) – translation distance along the X-axis
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Tx(x)is an se(3) translation ofxalong the x-axisExample:
- Seealso:
transl()- SymPy:
supported
- classmethod Ty(y)[source]
Create a new 3D twist for pure translation along the Y-axis
- Parameters:
y (float) – translation distance along the Y-axis
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Ty(y)is an se(3) translation ofyalong the y-axisExample:
- Seealso:
transl()- SymPy:
supported
- classmethod Tz(z)[source]
Create a new 3D twist for pure translation along the Z-axis
- Parameters:
z (float) – translation distance along the Z-axis
- Returns:
3D twist vector
- Return type:
Twist3 instance
Twist3.Tz(z)is an se(3) translation ofzalong the z-axisExample:
- Seealso:
transl()- SymPy:
supported
- classmethod UnitPrismatic(a)[source]
Construct a new 3D unit prismatic twist
- Parameters:
a (array_like(3)) – Twist axis or line of action
- Returns:
a prismatic twist
- Return type:
Twist instance
A prismatic twist with a line of action in the z-direction would be:
>>> from spatialmath import Twist3 >>> Twist3.UnitPrismatic([0, 0, 1]) Twist3([0, 0, 1, 0, 0, 0])
- classmethod UnitRevolute(a, q, pitch=None)[source]
Construct a new 3D rotational unit twist
- Parameters:
a (array_like(3)) – Twist axis or line of action
q (array_like(3)) – Point on the line of action
p (float, optional) – pitch, defaults to None
- Returns:
a rotational or helical twist
- Return type:
Twist instance
A revolute twist with a line of action in the z-direction and passing through (1, 2, 0) would be:
>>> from spatialmath import Twist3 >>> Twist3.UnitRevolute([0, 0, 1], [1, 2, 0]) Twist3([2, -1, -0, 0, 0, 1])
- static isvalid(v, check=True)[source]
Test if matrix is valid twist
- Parameters:
v (ndarray) – array to test
- Returns:
Whether the value is a 6-vector or a valid 4x4 se(3) element
- Return type:
bool
A twist can be represented by a 6-vector or a 4x4 skew symmetric matrix, for example:
>>> from spatialmath import Twist3 >>> from spatialmath.base import skewa >>> import numpy as np >>> Twist3.isvalid([1, 2, 3, 4, 5, 6]) True >>> a = skewa([1, 2, 3, 4, 5, 6]) >>> a array([[ 0., -6., 5., 1.], [ 6., 0., -4., 2.], [-5., 4., 0., 3.], [ 0., 0., 0., 0.]]) >>> Twist3.isvalid(a) True >>> Twist3.isvalid(np.random.rand(4,4)) False
- Ad()[source]
Adjoint of 3D twist
- Returns:
adjoint matrix
- Return type:
ndarray(6,6)
S.Ad()is the 6x6 adjoint matrix of the corresponding homogeneous transformation.For a twist representing motion from frame {B} to {A}, the adjoint will transform a twist relative to frame {A} to one relative to frame {B}.
Example:
>>> from spatialmath import Twist3 >>> S = Twist3.Rx(0.3) >>> S.Ad() array([[ 1. , 0. , 0. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. , 0. , 0. ], [ 0. , 0.2955, 0.9553, 0. , 0. , 0. ], [ 0. , 0. , 0. , 1. , 0. , 0. ], [ 0. , 0. , 0. , 0. , 0.9553, -0.2955], [ 0. , 0. , 0. , 0. , 0.2955, 0.9553]])
Note
Equivalent to, but faster than,
self.SE3().Ad()– computes the adjoint directly from the twist’s exponential without constructing an intermediateSE3instance.- Seealso:
- SE3(theta=1, unit='rad')[source]
Convert 3D twist to SE(3) matrix
- Returns:
an SE(3) representation
- Return type:
SE3 instance
S.SE3()is an SE3 object representing the homogeneous transformation equivalent to the Twist3. This is the exponentiation of the twist vector.Example:
- Seealso:
- __eq__(right)
Overloaded
==operator (superclass method)- Returns:
Equality of two operands
- Return type:
bool or list of bool
S1 == S2is True ifS1` is elementwise equal to ``S2.Example:
- Seealso:
- __init__(arg=None, w=None, check=True)[source]
Construct a new 3D twist object
Twist3()is a Twist3 instance representing null motion – the identity twistTwist3(S)is a Twist3 instance from an array-like (6,)Twist3(v, w)is a Twist3 instance from a momentv(3,) and directionw(3,)Twist3([S1, S2, ... SN])where eachSiis a numpy array (6,)Twist3(X)is a Twist3 instance with the same value asX, ie. a copyTwist3([X1, X2, ... XN])where each Xi is a Twist3 instance, is a Twist3 instance containing N motions
- __mul__(right)[source]
Overloaded
*operator- Parameters:
left – left multiplicand
right – right multiplicand
- Returns:
product
- Raises:
ValueError
Twist composition or scaling:
X * Ycompounds the twistsXandYX * sperforms elementwise multiplication of the elements ofXbyss * Xperforms elementwise multiplication of the elements ofXbys
Multiplicands
Product
left
right
type
operation
Twist3
Twist3
Twist3
product of exponentials
Twist3
scalar
Twist3
element-wise product
scalar
Twist3
Twist3
element-wise product
Twist3
SE3
Twist3
exponential x SE3
Twist3
SpatialVector
SpatialVector
adjoint/coadjoint product
Note
scalar x Twist is handled by
__rmul__scalar multiplication is commutative but the result is not a group operation so the result will be a matrix
Twist3 * SpatialVector(velocity, acceleration, force, momentum) is handled by falling back to__rmul__().Any other input combination raises a
TypeError.
For pose composition the
leftandrightoperands may be a sequencelen(left)
len(right)
len
operation
1
1
1
prod = left * right1
M
M
prod[i] = left * right[i]N
1
M
prod[i] = left[i] * rightM
M
M
prod[i] = left[i] * right[i]
- __ne__(right)
Overloaded
!=operator (superclass method)- Return type:
bool
S1 == S2is True ifS1is not elementwise equal toS2.Example:
- Seealso:
- __truediv__(right)
- ad()[source]
Logarithm of adjoint of 3D twist
- Returns:
logarithm of adjoint matrix
- Return type:
ndarray(6,6)
S.ad()is the 6x6 logarithm of the adjoint matrix of the corresponding homogeneous transformation.For a twist representing motion from frame {B} to {A}, the adjoint will transform a twist relative to frame {A} to one relative to frame {B}.
Example:
Note
An alternative approach to computing the adjoint is to exponentiate this 6x6 matrix.
- 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
Xis any of the SMTB classes.
- clear() None -- remove all items from S
- exp(theta=1, unit='rad')[source]
Exponentiate a 3D twist
- Parameters:
theta (float, optional) – rotation magnitude, defaults to 1
units (str, optional) – rotational units, defaults to ‘rad’
- Returns:
SE(3) matrix
- Return type:
SE3 instance
X.exp()is the homogeneous transformation equivalent to the twist, \(e^{[S]}\)X.exp(θ)as above but with a rotation ofθabout the twist axis, \(e^{ heta[S]}\)
If
len(X)==1andlen(θ)==Nthen the resulting SE3 object hasNvalues equivalent to the twist \(e^{ heta_i[S]}\).If
len(X)==Nandlen(θ)==1then the resulting SE3 object hasNvalues equivalent to the twist \(e^{ heta[S_i]}\).If
len(X)==Nandlen(θ)==Nthen the resulting SE3 object hasNvalues equivalent to the twist \(e^{ heta_i[S_i]}\).Example:
>>> from spatialmath import SE3, Twist3 >>> T = SE3(1, 2, 3) * SE3.Rx(0.3) >>> S = Twist3(T) >>> S.exp(0) SE3(array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]])) >>> S.exp(1) SE3(array([[ 1. , 0. , 0. , 1. ], [ 0. , 0.9553, -0.2955, 2. ], [ 0. , 0.2955, 0.9553, 3. ], [ 0. , 0. , 0. , 1. ]]))
Note
For the second form, the twist must, if rotational, have a unit rotational component.
- Seealso:
spatialmath.smb.trexp()
- 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
Xis any of the SMTB classes.
- insert(i, item)
Insert a value to an instance (BasePoseList superclass method)
- Parameters:
i (
int) – element to insert value beforeitem (
BasePoseList) – 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
Xis any of the SMTB classes.Note
If
iis beyond the end of the list, the item is appended to the list
- inv()
Inverse of Twist (superclass method)
- Returns:
inverse
- Return type:
Twist instance
Compute the inverse of each of the values within the twist instance. The inverse is the negative of the twist vector.
Example:
>>> from spatialmath import Twist3 >>> S = Twist3(SE3.Rand()) >>> S Twist3([-0.19529, -1.0559, 0.049327, -1.127, -2.1798, 0.82902]) >>> S.inv() Twist3([0.19529, 1.0559, -0.049327, 1.127, 2.1798, -0.82902]) >>> S * S.inv() Twist3([1.3878e-16, -2.2204e-16, -5.5511e-17, -2.2874e-17, -5.5511e-17, -1.6719e-17])
- line()[source]
Line of action of 3D twist as a Plucker line
- Returns:
the 3D line of action
- Return type:
Line instance
X.line()is a Plucker object representing the line of the twist axis.Example:
- pop(i=-1)
Pop value from an instance (BasePoseList superclass method)
- Parameters:
i (
Optional[int]) – item in the list to pop, default is last- Returns:
the popped value
- Return type:
Self- 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
Xis any of the SMTB classes.
- prod()
Product of twists (superclass method)
For a twist instance with N values return the matrix product of those elements \(\prod_i=0^{N-1} S_i\).
Example:
- reverse()
S.reverse() – reverse IN PLACE
- skewa()[source]
Convert 3D twist to se(3)
- Returns:
An se(3) matrix
- Return type:
ndarray(4,4)
X.skewa()is the twist as a 4x4 augmented skew-symmetric matrix belonging to the group se(3). This is the Lie algebra of the corresponding SE(3) element.Example:
>>> from spatialmath import Twist3, base >>> S = Twist3.Rx(0.3) >>> se = S.skewa() >>> se array([[ 0. , -0. , 0. , 0. ], [ 0. , 0. , -0.3, 0. ], [-0. , 0.3, 0. , 0. ], [ 0. , 0. , 0. , 0. ]]) >>> base.trexp(se) array([[ 1. , 0. , 0. , 0. ], [ 0. , 0.9553, -0.2955, 0. ], [ 0. , 0.2955, 0.9553, 0. ], [ 0. , 0. , 0. , 1. ]])
- unit()[source]
Unit twist
S.unit()is a Twist3 object representing a unit twist aligned with the TwistS.
Example:
- property A: Union[List[NDArray], NDArray]
Array value of an instance (BasePoseList superclass method)
- Returns:
NumPy array value of this instance
- Return type:
ndarray
X.Ais 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
Dimension of the object’s group
- Returns:
dimension
- Return type:
int
Dimension of the group is 3 for
Twist3and corresponds to the dimension of the space (3D in this case) to which these rigid-body motions apply.Example:
- property S
Twist as a vector (superclass property)
- Returns:
Twist vector
- Return type:
ndarray(N)
X.Sis a 3-vector if X is aTwist2instance, and a 6-vector if X is aTwist3instance.
Note
the vector is the unique elements of the se(N) representation.
the vector is sometimes referred to as the twist coordinate vector.
if
len(X)> 1 then return a list of vectors.
- property isprismatic
Test for prismatic twist (superclass property)
- Returns:
Whether twist is purely prismatic
- Return type:
bool
A prismatic twist has \(\vec{\omega} = 0\).
Example:
>>> from spatialmath import Twist3 >>> x = Twist3.UnitPrismatic([1,2,3]) >>> x.isprismatic True >>> x = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> x.isprismatic False
- property isrevolute
Test for revolute twist (superclass property)
- Returns:
Whether twist is purely revolute
- Return type:
bool
A revolute twist has \(\vec{v} = 0\).
Example:
>>> from spatialmath import Twist3 >>> x = Twist3.UnitPrismatic([1,2,3]) >>> x.isrevolute False >>> x = Twist3.UnitRevolute([1,2,3], [0,0,0]) >>> x.isrevolute True
- property isunit
Test for unit twist (superclass property)
- Returns:
Whether twist is a unit-twist
- Return type:
bool
A unit twist is one with a norm of 1, ie. \(\| S \| = 1\).
Example:
>>> from spatialmath import Twist3 >>> S = Twist3([1,2,3,4,5,6]) >>> S.isunit False >>> S = Twist3.UnitRevolute([1,2,3], [4,5,6]) >>> S.isunit False
- property pitch
Pitch of a 3D twist
- Returns:
the pitch of the twist
- Return type:
float
X.pitch()is the pitch of the twist as a scalar in units of distance per radian.If we consider the twist as a screw, this is the distance of translation along the screw axis for a one radian rotation about the screw axis.
Example:
- property pole
Pole of a 3D twist
- Returns:
the pole of the twist
- Return type:
ndarray(3)
X.poleis a point on the twist axis. For a pure translation this point is at infinity.Example:
- property shape
Shape of the object’s internal array representation
- Returns:
(6,)
- Return type:
tuple
- property theta
Twist angle (superclass method)
- Returns:
magnitude of rotation (1x1) about the twist axis in radians
- Return type:
float
- property v
Moment vector of twist
- Returns:
Moment vector
- Return type:
ndarray(3)
X.vis a 3-vector representing the moment vector of the twist.Example:
- property w
Direction vector of twist
- Returns:
Direction vector
- Return type:
ndarray(3)
X.wis a 3-vector representing the direction vector of the twist.Example: