ETS for 3D space

An ET is one elementary \(\SE{3}\) transform: a rotation about, or a translation along, the x-, y- or z-axis. The angle or distance is either a constant or, if it is left out, a variable, which makes the transform a joint. An ETS is a sequence of them. The two-dimensional equivalents are on the ETS for 2D space page.

We can use ETS to describes a planar arm that moves in 3D space.

>>> from roboticstoolbox import ET
>>> ET.Rz()                      # a revolute joint
ET.Rz()
>>> ET.tx(0.5)                   # a constant translation
ET.tx(param=0.5)
>>> ET.Ry(90, 'deg')             # a constant rotation
ET.Ry(param=1.5707963267948966)
>>> ET.tz(flip=True, qlim=[0, 1])  # a prismatic joint, flipped, with limits
ET.tz(flip=True, qlim=array([0., 1.]))
>>> e = ET.Rz() * ET.tx(1) * ET.Ry() * ET.tz()
>>> e.structure                  # the joint types
'RRP'
>>> e.n, e.m                     # joints, and transforms
(3, 4)

ET: Elementary Transform

Creating an elementary transform

The constructors Rx, Ry, Rz, tx, ty and tz take the same arguments. The first, param, is what makes the transform a constant or a joint.

Call	Result
ET.Rz()	a joint, its value is supplied at evaluation time
ET.Rz(0.3)	a constant, the angle in radians
ET.Rz(30, 'deg')	a constant, the angle in degrees (rotations only)
ET.tx(0.5)	a constant, the distance in the units of the robot
ET.Rz(flip=True)	a joint that moves in the opposite sense
ET.Rz(jindex=2)	a joint with an explicit index in the joint vector of a robot
ET.Rz(qlim=[-1, 1])	a joint with joint limits
ET.Rz("q2")	a joint described by a string, see Joint descriptors
ET.SE3(T)	a constant, an arbitrary \(\SE{3}\) transform

The arguments are:

param

the angle (radians) or distance of a constant transform, left out for a joint, or a string that names a joint, see Joint descriptors

unit

'rad' (default) or 'deg', the unit of the angle of a constant rotation

flip

if True the joint moves in the opposite sense, q is negated

jindex

the index of this joint in the joint vector of a robot, by default the joints are numbered in the order they appear

qlim

the joint limits, a 2-vector [lower, upper]

>>> from roboticstoolbox import ET
>>> ET.Rz()                         # a joint
ET.Rz()
>>> ET.Rz(0.3)                      # a constant, radians
ET.Rz(param=0.3)
>>> ET.Rz(30, 'deg')                # a constant, degrees
ET.Rz(param=0.5235987755982988)
>>> ET.tz(flip=True, qlim=[0, 1])   # a flipped prismatic joint with limits
ET.tz(flip=True, qlim=array([0., 1.]))
>>> ET.Rz(jindex=2).jindex          # an explicit joint index
2
>>> from spatialmath import SE3
>>> ET.SE3(SE3(1, 2, 3))            # a constant arbitrary transform
ET.SE3(T=array([[1., 0., 0., 1.],
       [0., 1., 0., 2.],
       [0., 0., 1., 3.],
       [0., 0., 0., 1.]]))

Rx	Pure rotation about the x-axis
Ry	Pure rotation about the y-axis
Rz	Pure rotation about the z-axis
tx	Pure translation along the x-axis
ty	Pure translation along the y-axis
tz	Pure translation along the z-axis
SE3	A static SE3

Information

kind	The transform type and axis
axis	The transform type and axis
ax	The Cartesian axis this transform acts along/about
param	Get the transform constant
isjoint	Test if ET is a joint
isflip	Test if ET joint is flipped
isrotation	Test if ET is a rotation
istranslation	Test if ET is a translation
iselementary	Test if ET is an elementary transform
jindex	Get ET joint index
qlim	Joint limits

Kinematics

A	Evaluate an elementary transformation
inv	Inverse of ET
s	Motion subspace of the joint

Joint descriptors

The parameter of a transform can also be a string that names a joint. This is convenient when a robot is written down from a datasheet or a URDF, where joints have names such as q1, theta2 or A3. The string is parsed once, when the transform is created, into the joint index, whether the joint is flipped, and the name that is displayed.

>>> from roboticstoolbox import ET
>>> e = ET.Rz("q2")              # the number is the joint index
>>> e, e.jindex, e.isflip
(ET.Rz(jindex=2), 2, False)
>>> e = ET.Rz("-q1")             # a leading minus flips the joint
>>> e, e.jindex, e.isflip
(ET.Rz(jindex=1, flip=True), 1, True)
>>> e = ET.Rz("theta2")          # any name, the first number is the index
>>> e, e.jindex
(ET.Rz(jindex=2), 2)
>>> e = ET.Rz("A1_")             # trailing underscore: counts from one
>>> e, e.jindex
(ET.Rz(jindex=0), 0)
>>> ET.Rz("joint").jindex        # no number, numbered automatically
>>> ET.Rz("1.5")                 # a number, it is a constant
ET.Rz(param=1.5)

A whole robot can be written with names that match its documentation.

>>> from roboticstoolbox import ET
>>> e = ET.tz(0.4) * ET.Rz("A1_") * ET.Ry("-A2_") * ET.tx(0.5) * ET.Ry("A3_")
>>> e
[ET.tz(param=0.4), ET.Rz(jindex=0), ET.Ry(jindex=1, flip=True), ET.tx(param=0.5), ET.Ry(jindex=2)]
>>> e.jindices
array([0, 1, 2])
>>> e.structure
'RRR'

The rules are:

• the first number in the string is the joint index, so "q0" is joint 0 and "theta2" is joint 2; "q(3)" and "θ_3" are joint 3

• a leading - (or the Unicode minus sign −) negates the joint variable, flip, so "-q1" is joint 1, flipped. A leading + is ignored

• joint indices count from zero. If the names count from one, as "A1" to "A6" of a Kuka arm do, end the name with an underscore: "A1_" is joint 0, and "-A1_" is joint 0, flipped. The underscore is not part of the name that is displayed

• a string with no number, such as "joint", is a variable joint that is numbered automatically

• a string that is a number, such as "1.5", is a constant

• jindex and flip must not also be given, ET.Rz("q1", jindex=2) raises ValueError

The name is shown when the transform is displayed, Rz(A1).

Note

A robot built from names "A1" to "A6", without the underscores, has joint indices 1 to 6, and is rejected as the index 6 is out of range for 6 joints.

Changed in version 1.5.0: A trailing underscore, and the Unicode minus sign, are accepted.

Details

class ET(**kwargs)

A(q=0.0)

Evaluate an elementary transformation

Parameters:

q (float | None) – Is used if this ET is variable (a joint)

Returns:

The SE(3) matrix value of the ET

Return type:

ndarray

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.A()
array([[1., 0., 0., 1.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.],
       [0., 0., 0., 1.]])
>>> e = ET.tx()
>>> e.A(0.7)
array([[1. , 0. , 0. , 0.7],
       [0. , 1. , 0. , 0. ],
       [0. , 0. , 1. , 0. ],
       [0. , 0. , 0. , 1. ]])

property s: ndarray

Motion subspace of the joint

Returns:

the 6-vector \((v, \omega)\) that is the unit twist of the transform: a unit translation or a unit rotation about the axis

classmethod Rx(param=None, unit='rad', *, eta=None, **kwargs)

Pure rotation about the x-axis

Parameters:

• param (float | None | str) – rotation about the x-axis, or a joint name such as "q2", see Joint descriptors

• unit (str) – angular unit, “rad” [default] or “deg”

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.Rx(param) is an elementary rotation about the x-axis by a constant angle

• ET.Rx() is an elementary rotation about the x-axis by a variable angle, i.e. a revolute robot joint. j or flip can be set in this case.

See also

ET() isrotation()

SymPy:

supported

classmethod Ry(param=None, unit='rad', *, eta=None, **kwargs)

Pure rotation about the y-axis

Parameters:

• param (float | None | str) – rotation about the y-axis, or a joint name such as "q2", see Joint descriptors

• unit (str) – angular unit, “rad” [default] or “deg”

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.Ry(param) is an elementary rotation about the y-axis by a constant angle

• ET.Ry() is an elementary rotation about the y-axis by a variable angle, i.e. a revolute robot joint. j or flip can be set in this case.

See also

ET() isrotation()

SymPy:

supported

classmethod Rz(param=None, unit='rad', *, eta=None, **kwargs)

Pure rotation about the z-axis

Parameters:

• param (float | None | str) – rotation about the z-axis, or a joint name such as "q2", see Joint descriptors

• unit (str) – angular unit, “rad” [default] or “deg”

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.Rz(param) is an elementary rotation about the z-axis by a constant angle

• ET.Rz() is an elementary rotation about the z-axis by a variable angle, i.e. a revolute robot joint. j or flip can be set in this case.

See also

ET() isrotation()

SymPy:

supported

classmethod tx(param=None, *, eta=None, **kwargs)

Pure translation along the x-axis

Parameters:

• param (float | None | str) – translation distance along the x-axis, or a joint name such as "q2", see Joint descriptors

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.tx(param) is an elementary translation along the x-axis by a distance constant

• ET.tx() is an elementary translation along the x-axis by a variable distance, i.e. a prismatic robot joint. j or flip can be set in this case.

See also

ET() istranslation()

SymPy:

supported

classmethod ty(param=None, *, eta=None, **kwargs)

Pure translation along the y-axis

Parameters:

• param (float | None | str) – translation distance along the y-axis, or a joint name such as "q2", see Joint descriptors

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.ty(param) is an elementary translation along the y-axis by a distance constant

• ET.ty() is an elementary translation along the y-axis by a variable distance, i.e. a prismatic robot joint. j or flip can be set in this case.

See also

ET() istranslation()

SymPy:

supported

classmethod tz(param=None, *, eta=None, **kwargs)

Pure translation along the z-axis

Parameters:

• param (float | None | str) – translation distance along the z-axis

• j – Explicit joint number within the robot

• flip – Joint moves in opposite direction

Returns:

An elementary transform

Return type:

ET

• ET.tz(param) is an elementary translation along the z-axis by a distance constant

• ET.tz() is an elementary translation along the z-axis by a variable distance, i.e. a prismatic robot joint. j or flip can be set in this case.

See also

ET() istranslation()

SymPy:

supported

classmethod SE3(T, **kwargs)

A static SE3

Parameters:

T (ndarray | SE3) – The SE3 transformation matrix

Returns:

An elementary transform

Return type:

ET

See also

ET() istranslation()

SymPy:

supported

property ax: str | None

The Cartesian axis this transform acts along/about

Returns:

"x", "y", or "z" for an elementary transform, otherwise None (e.g. ET2’s rotation, which has no axis letter, or a compound/arbitrary SE3/SE2 transform)

Return type:

str or None

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.ax
'x'
>>> e = ET.Rx(90, 'deg')
>>> e.ax
'x'

property axis: str

The transform type and axis

Deprecated since version 1.4.0: Use kind instead. axis is kept as an alias and will not be repurposed to mean something else in a future release.

Returns:

The transform type and axis

Return type:

str

property eta: float | Symbol | None

Get the transform constant

Deprecated since version 1.4.0: eta (η) is the name used in the original Elementary Transform Sequence paper; kept as a permanent alias for param, which is otherwise identical.

Returns:

The constant value if set

Return type:

float or Sym or None

Setter accepts:

float | Symbol

inv()

Inverse of ET

Returns:

Inverse of the ET

Return type:

Any

The inverse of a given ET.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz(2.5)
>>> print(e)
Rz(143.2°)
>>> print(e.inv())
Rz(-143.2°)

property iselementary: bool

Test if ET is an elementary transform

Returns:

True if an elementary transform

Return type:

bool

Notes

• ET’s may not actually be “elementary”, it can be a complex

  mix of rotations and translations.

See also

compile()

property isflip: bool

Test if ET joint is flipped

Returns:

True if joint is flipped

Return type:

bool

A flipped joint uses the negative of the joint variable, ie. it rotates or moves in the opposite direction.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx()
>>> e.A(1)
array([[1., 0., 0., 1.],
       [0., 1., 0., 0.],
       [0., 0., 1., 0.],
       [0., 0., 0., 1.]])
>>> eflip = ET.tx(flip=True)
>>> eflip.A(1)
array([[ 1.,  0.,  0., -1.],
       [ 0.,  1.,  0.,  0.],
       [ 0.,  0.,  1.,  0.],
       [ 0.,  0.,  0.,  1.]])

property isjoint: bool

Test if ET is a joint

Returns:

True if a joint

Return type:

bool

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.isjoint
False
>>> e = ET.tx()
>>> e.isjoint
True

property isrotation: bool

Test if ET is a rotation

Returns:

True if a rotation

Return type:

bool

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.isrotation
False
>>> e = ET.Rx()
>>> e.isrotation
True

property istranslation: bool

Test if ET is a translation

Returns:

True if a translation

Return type:

bool

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.istranslation
True
>>> e = ET.Rx()
>>> e.istranslation
False

property jindex: int | None

Get ET joint index

Returns:

The assigned joint index

Return type:

int or None

Allows an ET to be associated with a numbered joint in a robot.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx()
>>> print(e)
tx(q)
>>> e = ET.tx(jindex=3)
>>> print(e)
tx(q3)
>>> print(e.jindex)
3

Setter accepts:

Any

property kind: str

The transform type and axis

Returns:

The transform type and axis, e.g. "Rx", "tx", "SE3"

Return type:

str

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.kind
'tx'
>>> e = ET.Rx(90, 'deg')
>>> e.kind
'Rx'

property param: float | Symbol | None

Get the transform constant

Returns:

The constant value if set

Return type:

float or Sym or None

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1)
>>> e.param
1.0
>>> e = ET.Rx(90, 'deg')
>>> e.param
1.5707963267948966
>>> e = ET.ty()
>>> e.param

Notes

• If the value was given in degrees it will be converted and

  stored internally in radians

• Historically called eta (η), after the notation used in the

  original Elementary Transform Sequence paper (Haviland & Corke, “Manipulator Differential Kinematics”). eta is kept as a deprecated alias below.

Setter accepts:

float | Symbol

property qlim: ndarray | None

Joint limits

Returns:

the joint limits as a 2-vector [lower, upper], or None if the transform has no limits

Setter accepts:

ndarray | Sequence[float] | None

ETS: Elementary Transform Sequence

An ETS is a list-like object. It supports len, indexing and slicing, iteration, and the list methods append, extend, pop, remove, reverse, index and count. Multiplying elementary transforms, or sequences, gives a new sequence.

Information

n	Number of joints
m	Number of transforms
structure	Joint structure string
joints	Get a list of the variable ETs with this ETS
joint_idx	Get index of joint transforms
jindices	Get an array of joint indices
jindex_set	Get set of joint indices
qlim	Get/Set Joint limits
random_q	Generate a random valid joint configuration

Modification

insert	Insert value
inv	Inverse of ETS
fixjoint	Return an independent ETS with the specified joint held constant.
split	Split ETS into link segments
swap	Swap two transforms in the ETS
merge	Merge two transforms in the ETS
compile	Compile an ETS

Kinematics

fkine	Forward kinematics
eval	Forward kinematics (returns raw ndarray)
jacob0	Manipulator geometric Jacobian in the base frame
jacobe	Manipulator geometric Jacobian in the end-effector frame
jacob0_analytical	Manipulator analytical Jacobian in the base frame
hessian0	Manipulator Hessian in the base frame
hessiane	Manipulator Hessian in the end-effector frame
partial_fkine0	Manipulator Forward Kinematics nth Partial Derivative
manipulability	Manipulability measure
jacobm	The manipulability Jacobian

Inverse kinematics

There are three ways to solve the numerical inverse kinematics problem for a sequence. All are described, with examples and the shared arguments, on the IK page. The first two are methods of the ETS, and a robot has the same methods, which call them on its own sequence.

Method	Poses	Notes
ik_LM(), ik_NR(), ik_GN()	one	fast, written in C++, not easy to extend
ikine_LM(), ikine_NR(), ikine_GN(), ikine_QP()	one, or a trajectory	methods that use the solver objects in the next row, and add the quadratic programming solver
IK_LM, IK_NR, IK_GN, IK_QP	one, or a trajectory	solver objects, written in Python so easy to extend, IK_LM().solve(ets, Tep), that take an ETS rather than a robot, convenient for comparing solvers on different robots

Each returns an IKSolution.

Other

plot	Plot the sequence as a robot
teach	Interactively explore the sequence as a robot

Details

class ETS(arg=None)

This class implements an elementary transform sequence (ETS) for 3D

An instance can contain an elementary transform (ET) or an elementary transform sequence (ETS). It has list-like properties by subclassing UserList, which means we can perform indexing, slicing pop, insert, as well as using it as an iterator over its values.

• ETS() an empty ETS list

• ETS(et) an ETS containing a single ET

• ETS([et0, et1, et2]) an ETS consisting of three ET’s

Parameters:

arg (list[ETS | ET] | list[ET] | list[ETS] | ET | ETS | None) – list of ETs or a single ET to initialise the ETS

Examples

>>> from roboticstoolbox import ETS, ET
>>> e = ET.Rz(0.3) # a single ET, rotation about z
>>> ets1 = ETS(e)
>>> len(ets1)
1
>>> ets2 = ET.Rz(0.3) * ET.tx(2) # an ETS
>>> len(ets2)                    # of length 2
2
>>> ets2[1]                      # an ET sliced from the ETS
ET.tx(param=2.0)

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

rx ry rz tx ty tz

fixjoint(j, value, unit='rad')

Return an independent ETS with the specified joint held constant.

j is the zero-based position among joint ETs, independent of their jindex values. Remaining joints retain explicit global jindices; automatically indexed joints are renumbered to fill the gap. Joint limits and flip flags are preserved. The original is not modified.

Parameters:

• j (int) – index of the joint to fix

• value (float) – fixed joint coordinate, in radians/degrees or metres

• unit (str) – angular unit for rotational joints, “rad” [default] or “deg”

Return type:

ETS

Returns:

ETS with one fewer joint

Raises:

ValueError – for an invalid joint position or angular unit

compile()

Compile an ETS

Returns:

optimised ETS

Return type:

ETS

Perform constant folding for faster evaluation. Consecutive constant ETs are compounded, leading to a constant ET which is denoted by SE3 when displayed.

Examples

>>> import roboticstoolbox as rtb
>>> robot = rtb.models.ETS.Panda()
>>> ets = robot.ets()
>>> ets
[ET.tz(param=0.333), ET.Rz(jindex=0), ET.Rx(param=-1.5707963267948966), ET.Rz(jindex=1), ET.Rx(param=1.5707963267948966), ET.tz(param=0.316), ET.Rz(jindex=2), ET.tx(param=0.0825), ET.Rx(param=1.5707963267948966), ET.Rz(jindex=3), ET.tx(param=-0.0825), ET.Rx(param=-1.5707963267948966), ET.tz(param=0.384), ET.Rz(jindex=4), ET.Rx(param=1.5707963267948966), ET.Rz(jindex=5), ET.tx(param=0.088), ET.Rx(param=1.5707963267948966), ET.tz(param=0.107), ET.Rz(jindex=6), ET.tz(param=0.10300000000000001), ET.Rz(param=-0.7853981633974483)]
>>> ets.compile()
[ET.SE3(T=array([[1.   , 0.   , 0.   , 0.   ],
       [0.   , 1.   , 0.   , 0.   ],
       [0.   , 0.   , 1.   , 0.333],
       [0.   , 0.   , 0.   , 1.   ]])), ET.Rz(jindex=0), ET.SE3(T=array([[ 1.,  0.,  0.,  0.],
       [ 0.,  0.,  1.,  0.],
       [ 0., -1.,  0.,  0.],
       [ 0.,  0.,  0.,  1.]])), ET.Rz(jindex=1), ET.SE3(T=array([[ 1.   ,  0.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   , -1.   , -0.316],
       [ 0.   ,  1.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  1.   ]])), ET.Rz(jindex=2), ET.SE3(T=array([[ 1.    ,  0.    ,  0.    ,  0.0825],
       [ 0.    ,  0.    , -1.    ,  0.    ],
       [ 0.    ,  1.    ,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  1.    ]])), ET.Rz(jindex=3), ET.SE3(T=array([[ 1.    ,  0.    ,  0.    , -0.0825],
       [ 0.    ,  0.    ,  1.    ,  0.384 ],
       [ 0.    , -1.    ,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  1.    ]])), ET.Rz(jindex=4), ET.SE3(T=array([[ 1.,  0.,  0.,  0.],
       [ 0.,  0., -1.,  0.],
       [ 0.,  1.,  0.,  0.],
       [ 0.,  0.,  0.,  1.]])), ET.Rz(jindex=5), ET.SE3(T=array([[ 1.   ,  0.   ,  0.   ,  0.088],
       [ 0.   ,  0.   , -1.   , -0.107],
       [ 0.   ,  1.   ,  0.   ,  0.   ],
       [ 0.   ,  0.   ,  0.   ,  1.   ]])), ET.Rz(jindex=6), ET.SE3(T=array([[ 0.7071,  0.7071,  0.    ,  0.    ],
       [-0.7071,  0.7071,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  1.    ,  0.103 ],
       [ 0.    ,  0.    ,  0.    ,  1.    ]]))]

See also

isconstant

insert(index, value)

Insert value

Parameters:

• index (int) – position to insert at

• value (ET | ETS) – the elementary transform or sequence to insert

Return type:

None

Inserts an ET or ETS into the ET sequence. The inserted ET is at position index; an ETS is expanded and inserted element by element.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1)
>>> f = ET.Ry()
>>> e.insert(2, f)
>>> e
[ET.Rz(jindex=0), ET.tx(param=1.0), ET.Ry(), ET.Rz(jindex=1), ET.tx(param=1.0)]

fkine(q=None, base=None, tool=None, include_base=True)

Forward kinematics

Parameters:

• q (ndarray | Sequence[float] | None) – Joint coordinates, optional for a sequence with no joints

• base (ndarray | SE3 | None) – a base transform applied before the ETS

• tool (ndarray | SE3 | None) – tool transform, optional

• include_base (bool) – set to True if the base transform should be considered

Returns:

the transformation matrix representing the pose of the end-effector

Return type:

SE3

T = ets.fkine(q) evaluates forward kinematics for the ets at joint configuration q.

Trajectory operation: If q has multiple rows (mxn), it is considered a trajectory and the result is an SE3 instance with m values.

Examples

The following example makes a panda robot object, gets the ets, and solves for the forward kinematics at the listed configuration.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
SE3(array([[ 0.995 , -0.    ,  0.0998,  0.484 ],
           [-0.    , -1.    ,  0.    ,  0.    ],
           [ 0.0998, -0.    , -0.995 ,  0.4126],
           [ 0.    ,  0.    ,  0.    ,  1.    ]]))

Notes

• A tool transform, if provided, is incorporated into the result.

• Works from the end-effector link to the base

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

eval(q=None, base=None, tool=None, include_base=True)

Forward kinematics (returns raw ndarray)

Parameters:

• q (ndarray | Sequence[float] | None) – Joint coordinates – either global (length max(self.jindices) + 1, addressed by each joint’s own jindex) or compact (length self.n, positionally ordered to match joints()). These only differ when this ETS is one branch of a larger, branched robot – e.g. it’s what ikine_LM/ik_LM return when solving for just this ETS. For a whole, unbranched robot the two coincide.

• base (ndarray | SE3 | None) – a base transform applied before the ETS

• tool (ndarray | SE3 | None) – tool transform, optional

• include_base (bool) – set to True if the base transform should be considered

Raises:

ValueError – if q’s length matches neither interpretation

Returns:

the transformation matrix representing the pose of the end-effector

Return type:

ndarray

T = ets.eval(q) evaluates forward kinematics for the ets at joint configuration q.

Trajectory operation: If q has multiple rows (mxn), it is considered a trajectory and the result is a 3d array with m planes.

Examples

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> panda.eval([0, -0.3, 0, -2.2, 0, 2, 0.7854])
array([[ 0.995 , -0.    ,  0.0998,  0.484 ],
       [-0.    , -1.    , -0.    , -0.    ],
       [ 0.0998, -0.    , -0.995 ,  0.4126],
       [ 0.    ,  0.    ,  0.    ,  1.    ]])

Notes

• A tool transform, if provided, is incorporated into the result.

• Works from the end-effector link to the base

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

jacob0(q, tool=None)

Manipulator geometric Jacobian in the base frame

Parameters:

• q (ndarray | Sequence[float]) – Joint coordinate vector

• tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of ETS

Returns:

Manipulator Jacobian in the base frame

Return type:

ndarray

robot.jacob0(q) is the manipulator Jacobian matrix which maps joint velocity to end-effector spatial velocity expressed in the base frame.

End-effector spatial velocity \(\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T\) is related to joint velocity by \({}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}\).

Examples

The following example makes a Puma560 robot object, and solves for the base-frame Jacobian at the zero joint angle configuration

>>> import roboticstoolbox as rtb
>>> puma = rtb.models.Puma560().ets()
>>> puma.jacob0([0, 0, 0, 0, 0, 0])
array([[ 0.1295, -0.4854, -0.4854, -0.    , -0.0533,  0.    ],
       [ 0.4318,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
       [-0.    ,  0.4318,  0.    ,  0.    ,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
       [ 0.    , -1.    , -1.    ,  0.    , -1.    ,  0.    ],
       [ 1.    ,  0.    ,  0.    ,  1.    ,  0.    ,  1.    ]])

Notes

• This is the geometric Jacobian as described in texts by

  Corke, Spong etal., Siciliano etal. The end-effector velocity is described in terms of translational and angular velocity, not a velocity twist as per the text by Lynch & Park.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

jacobe(q, tool=None)

Manipulator geometric Jacobian in the end-effector frame

Parameters:

• q (ndarray | Sequence[float]) – Joint coordinate vector

• tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of ETS

Returns:

Manipulator Jacobian in the end-effector frame

Return type:

ndarray

robot.jacobe(q) is the manipulator Jacobian matrix which maps joint velocity to end-effector spatial velocity expressed in the end-effector frame.

End-effector spatial velocity \(\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T\) is related to joint velocity by \({}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}\).

Examples

The following example makes a Puma560 robot object, and solves for the end-effector frame Jacobian at the zero joint angle configuration

>>> import roboticstoolbox as rtb
>>> puma = rtb.models.Puma560().ets()
>>> puma.jacobe([0, 0, 0, 0, 0, 0])
array([[ 0.1295, -0.4854, -0.4854, -0.    , -0.0533,  0.    ],
       [ 0.4318,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
       [-0.    ,  0.4318,  0.    ,  0.    ,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
       [ 0.    , -1.    , -1.    ,  0.    , -1.    ,  0.    ],
       [ 1.    ,  0.    ,  0.    ,  1.    ,  0.    ,  1.    ]])

Notes

• This is the geometric Jacobian as described in texts by

  Corke, Spong etal., Siciliano etal. The end-effector velocity is described in terms of translational and angular velocity, not a velocity twist as per the text by Lynch & Park.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

hessian0(q=None, J0=None, tool=None)

Manipulator Hessian in the base frame

Parameters:

• q (ndarray | Sequence[float] | None) – joint angles (optional if J0 supplied)

• J0 (ndarray | None) – the manipulator Jacobian in the base frame (optional if q supplied)

• tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of ETS

Returns:

The manipulator Hessian in the base frame

Return type:

ndarray

This method computes the manipulator Hessian in the base frame. If we take the time derivative of the differential kinematic relationship

\[\begin{split}\nu &= \mat{J}(\vec{q}) \dvec{q} \\ \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}\end{split}\]

where

\[\dmat{J} = \mat{H} \dvec{q}\]

and \(\mat{H} \in \mathbb{R}^{6\times n \times n}\) is the Hessian tensor.

The elements of the Hessian are

\[\mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}\]

where \(u = \{t_x, t_y, t_z, r_x, r_y, r_z\}\) are the elements of the spatial velocity vector.

Similarly, we can write

\[\mat{J}_{i,j} = \frac{d u_i}{d q_j}\]

Examples

The following example makes a Panda robot object, and solves for the base frame Hessian at the given joint angle configuration

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> panda.hessian0([0, -0.3, 0, -2.2, 0, 2, 0.7854])
array([[[-0.484 , -0.    , -0.486 , -0.    , -0.1547,  0.    , -0.    ],
        [ 0.    ,  0.0796,  0.    ,  0.2466,  0.    ,  0.2006,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    , -1.    , -0.    ,  1.    ,  0.    ,  1.    ,  0.    ],
        [ 0.    , -0.    , -0.2955, -0.    ,  0.9463, -0.    ,  0.0998],
        [ 0.    ,  0.    , -0.    , -0.    ,  0.    , -0.    ,  0.    ]],

       [[-0.    , -0.484 , -0.    ,  0.4986, -0.    ,  0.1086,  0.    ],
        [ 0.0796,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    , -0.0796, -0.    , -0.2466, -0.    , -0.2006, -0.    ],
        [ 0.    ,  0.    ,  0.9553,  0.    , -0.3233,  0.    , -0.995 ],
        [ 0.    ,  0.    , -0.    , -0.    ,  0.    , -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.2955,  0.    , -0.9463,  0.    , -0.0998]],

       [[-0.486 , -0.    , -0.4643, -0.    , -0.1478,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.383 ,  0.    ,  0.2237,  0.    ],
        [ 0.    , -0.    , -0.1436, -0.    , -0.0457, -0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.9553,  0.    ,  0.9553,  0.    ],
        [ 0.    ,  0.    ,  0.    , -0.    ,  0.8085, -0.    , -0.1987],
        [ 0.    ,  0.    ,  0.    ,  0.2955,  0.    ,  0.2955,  0.    ]],

       [[-0.    ,  0.4986, -0.    , -0.4986,  0.    , -0.1086, -0.    ],
        [ 0.2466,  0.    ,  0.383 ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    , -0.2466, -0.    ,  0.2466,  0.    ,  0.2006,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.3233,  0.    ,  0.995 ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.9463,  0.    ,  0.0998]],

       [[-0.1547, -0.    , -0.1478,  0.    ,  0.05  , -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.1676, -0.    ],
        [ 0.    , -0.    , -0.0457,  0.    ,  0.1464,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.3233,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.    ,  0.9093],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.9463, -0.    ]],

       [[ 0.    ,  0.1086,  0.    , -0.1086, -0.    , -0.1086, -0.    ],
        [ 0.2006,  0.    ,  0.2237,  0.    , -0.1676,  0.    ,  0.    ],
        [ 0.    , -0.2006, -0.    ,  0.2006,  0.    ,  0.2006,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.995 ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.0998]],

       [[-0.    ,  0.    ,  0.    , -0.    ,  0.    , -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    , -0.    ,  0.    , -0.    ],
        [ 0.    , -0.    , -0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ]]])

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

hessiane(q=None, Je=None, tool=None)

Manipulator Hessian in the end-effector frame

Parameters:

• q (ndarray | Sequence[float] | None) – joint angles (optional if Je supplied)

• Je (ndarray | None) – the manipulator Jacobian in the end-effector frame (optional if q supplied)

• tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of ETS

Returns:

The manipulator Hessian in the end-effector frame

Return type:

ndarray

This method computes the manipulator Hessian in the end-effector frame. If we take the time derivative of the differential kinematic relationship

\[\begin{split}\nu &= \mat{J}(\vec{q}) \dvec{q} \\ \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q}\end{split}\]

where

\[\dmat{J} = \mat{H} \dvec{q}\]

and \(\mat{H} \in \mathbb{R}^{6\times n \times n}\) is the Hessian tensor.

The elements of the Hessian are

\[\mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k}\]

where \(u = \{t_x, t_y, t_z, r_x, r_y, r_z\}\) are the elements of the spatial velocity vector.

Similarly, we can write

\[\mat{J}_{i,j} = \frac{d u_i}{d q_j}\]

Examples

The following example makes a Panda robot object, and solves for the end-effector frame Hessian at the given joint angle configuration

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> panda.hessiane([0, -0.3, 0, -2.2, 0, 2, 0.7854])
array([[[-0.4816, -0.    , -0.4835, -0.    , -0.1539, -0.    ,  0.    ],
        [ 0.    , -0.0796,  0.    , -0.2466,  0.    , -0.2006,  0.    ],
        [-0.0483, -0.    , -0.0485, -0.    , -0.0154, -0.    ,  0.    ],
        [ 0.    , -0.995 ,  0.    ,  0.995 , -0.    ,  0.995 , -0.    ],
        [ 0.    ,  0.    ,  0.2955, -0.    , -0.9463, -0.    , -0.0998],
        [ 0.    , -0.0998,  0.    ,  0.0998, -0.    ,  0.0998,  0.    ]],

       [[-0.    , -0.4896, -0.    ,  0.4715, -0.    ,  0.088 ,  0.    ],
        [-0.0796,  0.    ,  0.    , -0.    ,  0.    , -0.    ,  0.    ],
        [-0.    ,  0.0309,  0.    ,  0.2952,  0.    ,  0.2104, -0.    ],
        [ 0.    ,  0.    ,  0.9801,  0.    , -0.4161,  0.    , -1.    ],
        [ 0.    ,  0.    , -0.    , -0.    ,  0.    , -0.    ,  0.    ],
        [ 0.    ,  0.    , -0.1987, -0.    ,  0.9093, -0.    ,  0.    ]],

       [[-0.4835, -0.    , -0.4763, -0.    , -0.1516, -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    , -0.383 ,  0.    , -0.2237,  0.    ],
        [-0.0485,  0.    ,  0.0965, -0.    ,  0.0307, -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.9801, -0.    ,  0.9801,  0.    ],
        [ 0.    ,  0.    ,  0.    , -0.    , -0.8085, -0.    ,  0.1987],
        [ 0.    ,  0.    ,  0.    , -0.1987, -0.    , -0.1987, -0.    ]],

       [[-0.    ,  0.4715, -0.    , -0.4715,  0.    , -0.088 ,  0.    ],
        [-0.2466, -0.    , -0.383 ,  0.    , -0.    ,  0.    , -0.    ],
        [-0.    ,  0.2952, -0.    , -0.2952, -0.    , -0.2104,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.4161,  0.    ,  1.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    , -0.    ,  0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    , -0.9093,  0.    ,  0.    ]],

       [[-0.1539, -0.    , -0.1516,  0.    ,  0.0644,  0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    , -0.    , -0.    ,  0.1676, -0.    ],
        [-0.0154,  0.    ,  0.0307, -0.    , -0.1407, -0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.4161, -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.9093],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.9093,  0.    ]],

       [[-0.    ,  0.088 , -0.    , -0.088 ,  0.    , -0.088 ,  0.    ],
        [-0.2006, -0.    , -0.2237,  0.    ,  0.1676,  0.    , -0.    ],
        [-0.    ,  0.2104, -0.    , -0.2104, -0.    , -0.2104,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  1.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    , -0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ]],

       [[ 0.    ,  0.    ,  0.    ,  0.    , -0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    , -0.    , -0.    , -0.    , -0.    ],
        [ 0.    , -0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ],
        [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ]]])

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

jacob0_analytical(q, representation='rpy/xyz', tool=None)

Manipulator analytical Jacobian in the base frame

Parameters:

• q (ndarray | Sequence[float]) – joint coordinate vector

• representation (str) – angular representation

• tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of ETS

Returns:

Manipulator Jacobian in the base frame

Return type:

Any

ets.jacob0_analytical(q) is the manipulator Jacobian matrix which maps joint velocity to end-effector spatial velocity expressed in the base frame. End-effector spatial velocity \(\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T\) is related to joint velocity by \({}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}\).

representation	rotational representation
'rpy/xyz'	RPY angular rates in XYZ order
'rpy/zyx'	RPY angular rates in ZYX order
'eul'	Euler angular rates in ZYZ order
'exp'	exponential coordinate rates

Examples

Makes a robot object and computes the analytic Jacobian for the given joint configuration

>>> import roboticstoolbox as rtb
>>> puma = rtb.models.ETS.Puma560().ets()
>>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0])
array([[ 0.15  ,  0.8636,  0.4318,  0.    ,  0.    ,  0.    ],
       [ 0.2203,  0.    ,  0.    ,  0.2   ,  0.    ,  0.2   ],
       [ 0.    , -0.2203, -0.2   ,  0.    , -0.2   ,  0.    ],
       [ 1.    ,  0.    ,  0.    ,  1.    ,  0.    ,  1.    ],
       [ 0.    ,  1.    ,  1.    ,  0.    ,  1.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  0.    ,  0.    ,  0.    ]])

jacobm(q)

The manipulability Jacobian

Parameters:

q (ndarray | Sequence[float]) – joint angles/configuration of the robot

Returns:

The manipulability Jacobian

Return type:

ndarray

This measure relates the rate of change of the manipulability to the joint velocities of the robot. Yoshikawa’s manipulability measure

\[m(\vec{q}) = \sqrt{\mat{J}(\vec{q}) \mat{J}(\vec{q})^T}\]

This method returns its Jacobian with respect to configuration

\[\frac{\partial m(\vec{q})}{\partial \vec{q}}\]

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

manipulability(q, method='yoshikawa', axes='all')

Manipulability measure

Parameters:

• q (Any) – joint coordinates (trajectory as matrix(m,n))

• method (Literal['yoshikawa', 'minsingular', 'invcondition']) – method to use, "yoshikawa" (default), "invcondition", or "minsingular"

• axes (Union[Literal['all', 'trans', 'rot'], list[bool]]) – task space axes to consider: "all" [default], "trans", or "rot"

Returns:

the manipulability metric

Return type:

Any

manipulability(q) is the scalar manipulability index for the ets at the joint configuration q. It indicates dexterity, that is, how well conditioned the ets is for motion with respect to the 6 degrees of Cartesian motion. The value is zero if the ets is at a singularity.

Various measures are supported:

Measure	Description
"yoshikawa"	volume of the velocity ellipsoid, distance from singularity [Yoshikawa85]
"invcondition"	inverse condition number of Jacobian, isotropy of the velocity ellipsoid [Klein87]
"minsingular"	minimum singular value of the Jacobian, distance from singularity [Klein87]

Trajectory operation:

If q is a matrix (m,n) then the result (m,) is a vector of manipulability indices for each joint configuration specified by a row of q.

Notes

• Invokes the jacob0 method of the robot if J is not passed

• The “all” option includes rotational and translational dexterity, but this involves adding different units. It can be more useful to look at the translational and rotational manipulability separately.

• Examples in the RVC book (1st edition) can be replicated by using the “all” option

References

[Yoshikawa85]

Manipulability of Robotic Mechanisms. Yoshikawa T., The International Journal of Robotics Research. 1985;4(2):3-9. doi:10.1177/027836498500400201

[Klein87] (1,2)

Dexterity Measures for the Design and Control of Kinematically Redundant Manipulators. Klein CA, Blaho BE. The International Journal of Robotics Research. 1987;6(2):72-83. doi:10.1177/027836498700600206

• Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7.

Changed in version 1.0.4: Removed ‘both’ option for axes, added a custom list option.

partial_fkine0(q, n)

Manipulator Forward Kinematics nth Partial Derivative

Parameters:

• q (ndarray | Sequence[float]) – joint angles/configuration of the robot

• n (int) – the order of derivative (must be >= 3)

Return type:

ndarray

Returns:

The nth partial derivative of the forward kinematics

This method computes the nth derivative of the forward kinematics where n is greater than or equal to 3. This is an extension of the differential kinematics where the Jacobian is the first partial derivative and the Hessian is the second.

Examples

The following example makes a Panda robot object, and solves for the base-effector frame 4th derivative of the forward kinematics at the given joint angle configuration

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> dT = panda.partial_fkine0([0, -0.3, 0, -2.2, 0, 2, 0.7854], n=4)
>>> dT.shape
(7, 7, 7, 6, 7)

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

ik_LM(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, k=1.0, method='chan')

Fast Levenberg-Marquardt numerical inverse kinematics solver

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose

• q0 (ndarray | None) – the initial joint coordinate vector

• ilimit (int) – maximum iterations allowed per search

• slimit (int) – maximum search attempts before failure

• tol (float) – maximum allowed residual error E

• mask (ndarray | None) – a 6-vector weighting Cartesian DoF error priority

• joint_limits (bool) – reject solutions with joint limit violations

• k (float) – gain value for the damping matrix Wn

• method (Literal['chan', 'wampler', 'sugihara']) – one of "chan" (default), "sugihara" or "wampler"

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches and residual error value (reason is always empty – this fast C++ solver doesn’t produce a granular failure reason string, unlike ikine_LM())

Return type:

IKSolution

Warning

This method requires the compiled C++ extension. It raises RuntimeError if that extension is unavailable, e.g. in a pure-Python build/wheel or under Pyodide/JupyterLite. Use ikine_LM() instead in those environments.

A method which provides functionality to perform numerical inverse kinematics (IK) using the Levenberg-Marquardt method. This is a fast solver implemented in C++.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

The operation is defined by the choice of the method kwarg.

The step is defined as

\[\begin{split}\vec{q}_{k+1} &= \vec{q}_k + \left( \mat{A}_k \right)^{-1} \bf{g}_k \\ % \mat{A}_k &= {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \ {\mat{J}(\vec{q}_k)} + \mat{W}_n\end{split}\]

where \(\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})\) is a diagonal damping matrix. The damping matrix ensures that \(\mat{A}_k\) is non-singular and positive definite. The performance of the LM method largely depends on the choice of \(\mat{W}_n\).

Chan’s Method

Chan proposed

\[\mat{W}_n = λ E_k \mat{1}_n\]

where λ is a constant which reportedly does not have much influence on performance. Use the kwarg k to adjust the weighting term λ.

Sugihara’s Method

Sugihara proposed

\[\mat{W}_n = E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n)\]

where \(\hat{\vec{w}}_n \in \mathbb{R}^n\), \(\hat{w}_{n_i} = l^2 \sim 0.01 l^2\), and \(l\) is the length of a typical link within the manipulator. We provide the variable k as a kwarg to adjust the value of \(w_n\).

Wampler’s Method

Wampler proposed \(\vec{w_n}\) to be a constant. This is set through the k kwarg.

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_LM method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_LM(Tep)
IKSolution: q=[2.493, 0.3636, -2.569, -2.195, 0.2218, 1.978, 0.6104], success=True, iterations=22, searches=2, residual=1.81e-08

Notes

The value for the k kwarg will depend on the method chosen and the arm you are using. Use the following as a rough guide chan, k = 1.0 - 0.01, wampler, k = 0.01 - 0.0001, and sugihara, k = 0.1 - 0.0001

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ik_NR() ik_GN() ikine_LM()

Changed in version 1.0.4: Merged the Levenberg-Marquardt IK solvers into the ik_LM method

ik_NR(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, pinv=True, pinv_damping=0.0)

Fast numerical inverse kinematics using Newton-Raphson optimisation

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose (a single pose only, for a trajectory use ikine_NR())

• q0 (ndarray | None) – initial joint configuration (random valid configuration if not supplied)

• ilimit (int) – maximum number of iterations per search

• slimit (int) – maximum number of search attempts

• tol (float) – final error tolerance

• mask (ndarray | None) – a 6-vector weighting end-effector error priority (XYZ translation, XYZ rotation)

• joint_limits (bool) – reject solutions with invalid joint configurations

• pinv (int) – use the pseudo-inverse instead of the normal matrix inverse

• pinv_damping (float) – damping factor for the pseudo-inverse

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches and residual error value (reason is always empty – this fast C++ solver doesn’t produce a granular failure reason string, unlike ikine_NR())

Return type:

IKSolution

Warning

This method requires the compiled C++ extension. It raises RuntimeError if that extension is unavailable, e.g. in a pure-Python build/wheel or under Pyodide/JupyterLite. Use ikine_NR() instead in those environments.

sol = ets.ik_NR(Tep) are the joint coordinates (n) corresponding to the robot end-effector pose Tep which is an SE3 or ndarray object. This method can be used for robots with any number of degrees of freedom. This is a fast solver implemented in C++.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

When using this method with redundant robots (>6 DoF), pinv must be set to True.

If success == 0 the q values will be valid numbers, but the solution will be in error. The amount of error is indicated by the residual.

Each iteration uses the Newton-Raphson optimisation method

\[\vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k\]

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ik_NR method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ik_NR(Tep)
IKSolution: q=[-1.598, 1.026, 2.072, -2.122, -0.9157, 1.568, 1.479], success=True, iterations=16, searches=1, residual=1.46e-11

Notes

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ik_LM() ik_GN() ikine_NR()

ik_GN(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, pinv=True, pinv_damping=0.0)

Fast numerical inverse kinematics by Gauss-Newton optimisation

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose (a single pose only, for a trajectory use ikine_GN())

• q0 (ndarray | None) – initial joint configuration (random valid configuration if not supplied)

• ilimit (int) – maximum number of iterations per search

• slimit (int) – maximum number of search attempts

• tol (float) – final error tolerance

• mask (ndarray | None) – a 6-vector weighting end-effector error priority (XYZ translation, XYZ rotation)

• joint_limits (bool) – reject solutions with invalid joint configurations

• pinv (int) – use the pseudo-inverse instead of the normal matrix inverse

• pinv_damping (float) – damping factor for the pseudo-inverse

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches and residual error value (reason is always empty – this fast C++ solver doesn’t produce a granular failure reason string, unlike ikine_GN())

Return type:

IKSolution

sol = ets.ik_GN(Tep) are the joint coordinates (n) corresponding to the robot end-effector pose Tep which is an SE3 or ndarray object. This method can be used for robots with any number of degrees of freedom. This is a fast solver implemented in C++.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

When using this method with redundant robots (>6 DoF), pinv must be set to True.

If success == 0 the q values will be valid numbers, but the solution will be in error. The amount of error is indicated by the residual.

Each iteration uses the Gauss-Newton optimisation method

\[\begin{split}\vec{q}_{k+1} &= \vec{q}_k + \left( {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \ {\mat{J}(\vec{q}_k)} \right)^{-1} \bf{g}_k \\ \bf{g}_k &= {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \vec{e}_k\end{split}\]

where \(\mat{J} = {^0\mat{J}}\) is the base-frame manipulator Jacobian. If \(\mat{J}(\vec{q}_k)\) is non-singular, and \(\mat{W}_e = \mat{1}_n\), then the above provides the pseudoinverse solution. However, if \(\mat{J}(\vec{q}_k)\) is singular, the above can not be computed and the GN solution is infeasible.

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_GN method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ik_GN(Tep)
IKSolution: q=[-0.733, -0.3847, 0.6438, -2.193, 0.2593, 1.97, 0.581], success=True, iterations=349, searches=22, residual=6.84e-09

Notes

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ik_LM() ik_NR() ikine_GN()

ikine_LM(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, k=1.0, method='chan', kq=0.0, km=0.0, ps=0.0, pi=0.3, **kwargs)

Levenberg-Marquardt numerical inverse kinematics solver

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose or pose trajectory

• q0 (ndarray | Sequence[float] | None) – the initial joint coordinates, a vector (n,), or a matrix (m, n) whose rows are the starting points of the first m searches (any further searches start from random valid coordinates)

• ilimit (int) – maximum iterations allowed per search

• slimit (int) – maximum search attempts before failure

• tol (float) – maximum allowed residual error E

• mask (ndarray | Sequence[float] | None) – a 6-vector weighting Cartesian DoF error priority

• joint_limits (bool) – reject solutions with joint limit violations

• seed (int | None) – seed for the RNG used to generate random joint configurations

• k (float) – gain value for the damping matrix Wn

• method (Literal['chan', 'wampler', 'sugihara']) – one of "chan" (default), "sugihara" or "wampler"

• kq (float) – gain for joint limit avoidance (0.0 disables)

• km (float) – gain for manipulability maximisation (0.0 disables)

• ps (float) – minimum joint approach distance to limit (radians or metres)

• pi (ndarray | float) – null-space influence distance (radians or metres)

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches, residual error value, and reason string if applicable

Return type:

Any

If Tep is a trajectory (an SE3 with N poses, or an array with shape (N, 4, 4)) each pose is solved independently from the same q0, so consecutive solutions are not guaranteed to be on the same IK branch. q has shape (N, n), success is True only if every pose succeeded, and residual is the maximum over the poses. See solve().

Changed in version 1.4.5: For a trajectory, residual is the maximum over the poses, it was the minimum.

A method which provides functionality to perform numerical inverse kinematics (IK) using the Levenberg-Marquardt method.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

The operation is defined by the choice of the method kwarg.

The step is defined as

\[\begin{split}\vec{q}_{k+1} &= \vec{q}_k + \left( \mat{A}_k \right)^{-1} \bf{g}_k \\ % \mat{A}_k &= {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \ {\mat{J}(\vec{q}_k)} + \mat{W}_n\end{split}\]

where \(\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})\) is a diagonal damping matrix. The damping matrix ensures that \(\mat{A}_k\) is non-singular and positive definite. The performance of the LM method largely depends on the choice of \(\mat{W}_n\).

Chan’s Method

Chan proposed

\[\mat{W}_n = λ E_k \mat{1}_n\]

where λ is a constant which reportedly does not have much influence on performance. Use the kwarg k to adjust the weighting term λ.

Sugihara’s Method

Sugihara proposed

\[\mat{W}_n = E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n)\]

where \(\hat{\vec{w}}_n \in \mathbb{R}^n\), \(\hat{w}_{n_i} = l^2 \sim 0.01 l^2\), and \(l\) is the length of a typical link within the manipulator. We provide the variable k as a kwarg to adjust the value of \(w_n\).

Wampler’s Method

Wampler proposed \(\vec{w_n}\) to be a constant. This is set through the k kwarg.

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_LM method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_LM(Tep)
IKSolution: q=[-0.7567, -0.3911, 0.6634, -2.192, 0.2702, 1.967, 0.5725], success=True, iterations=19, searches=1, residual=2.37e-11

Notes

The value for the k kwarg will depend on the method chosen and the arm you are using. Use the following as a rough guide chan, k = 1.0 - 0.01, wampler, k = 0.01 - 0.0001, and sugihara, k = 0.1 - 0.0001

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ikine_NR() ikine_GN() ikine_QP() ik_LM()

Changed in version 1.0.4: Added the Levenberg-Marquardt IK solver method on the ETS class

ikine_NR(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, pinv=False, kq=0.0, km=0.0, ps=0.0, pi=0.3, **kwargs)

Newton-Raphson numerical inverse kinematics solver

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose or pose trajectory

• q0 (ndarray | Sequence[float] | None) – the initial joint coordinates, a vector (n,), or a matrix (m, n) whose rows are the starting points of the first m searches (any further searches start from random valid coordinates)

• ilimit (int) – maximum iterations allowed per search

• slimit (int) – maximum search attempts before failure

• tol (float) – maximum allowed residual error E

• mask (ndarray | Sequence[float] | None) – a 6-vector weighting Cartesian DoF error priority

• joint_limits (bool) – reject solutions with joint limit violations

• seed (int | None) – seed for the RNG used to generate random joint configurations

• pinv (bool) – use the pseudo-inverse in the step method instead of the normal inverse

• kq (float) – gain for joint limit avoidance (0.0 disables)

• km (float) – gain for manipulability maximisation (0.0 disables)

• ps (float) – minimum joint approach distance to limit (radians or metres)

• pi (ndarray | float) – null-space influence distance (radians or metres)

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches, residual error value, and reason string if applicable

Return type:

Any

If Tep is a trajectory (an SE3 with N poses, or an array with shape (N, 4, 4)) each pose is solved independently from the same q0, so consecutive solutions are not guaranteed to be on the same IK branch. q has shape (N, n), success is True only if every pose succeeded, and residual is the maximum over the poses. See solve().

Changed in version 1.4.5: For a trajectory, residual is the maximum over the poses, it was the minimum.

A method which provides functionality to perform numerical inverse kinematics (IK) using the Newton-Raphson method.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

When using this method with redundant robots (>6 DoF), pinv must be set to True.

Each iteration uses the Newton-Raphson optimisation method

\[\vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k\]

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_NR method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_NR(Tep)
IKSolution: q=[0.7567, 1.192, 2.47, -0.7633, 2.79, 0.3284, -2.66], success=False, reason=iteration and search limit reached, 100 numpy.LinAlgError encountered, iterations=100, searches=100, residual=3.75

Notes

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ikine_LM() ikine_GN() ikine_QP() ik_NR()

Changed in version 1.0.4: Added the Newton-Raphson IK solver method on the ETS class

ikine_GN(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, pinv=False, kq=0.0, km=0.0, ps=0.0, pi=0.3, **kwargs)

Gauss-Newton numerical inverse kinematics solver

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose or pose trajectory

• q0 (ndarray | Sequence[float] | None) – the initial joint coordinates, a vector (n,), or a matrix (m, n) whose rows are the starting points of the first m searches (any further searches start from random valid coordinates)

• ilimit (int) – maximum iterations allowed per search

• slimit (int) – maximum search attempts before failure

• tol (float) – maximum allowed residual error E

• mask (ndarray | Sequence[float] | None) – a 6-vector weighting Cartesian DoF error priority

• joint_limits (bool) – reject solutions with joint limit violations

• seed (int | None) – seed for the RNG used to generate random joint configurations

• pinv (bool) – use the pseudo-inverse in the step method instead of the normal inverse

• kq (float) – gain for joint limit avoidance (0.0 disables)

• km (float) – gain for manipulability maximisation (0.0 disables)

• ps (float) – minimum joint approach distance to limit (radians or metres)

• pi (ndarray | float) – null-space influence distance (radians or metres)

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches, residual error value, and reason string if applicable

Return type:

Any

If Tep is a trajectory (an SE3 with N poses, or an array with shape (N, 4, 4)) each pose is solved independently from the same q0, so consecutive solutions are not guaranteed to be on the same IK branch. q has shape (N, n), success is True only if every pose succeeded, and residual is the maximum over the poses. See solve().

Changed in version 1.4.5: For a trajectory, residual is the maximum over the poses, it was the minimum.

A method which provides functionality to perform numerical inverse kinematics (IK) using the Gauss-Newton method.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

When using this method with redundant robots (>6 DoF), pinv must be set to True.

Each iteration uses the Gauss-Newton optimisation method

\[\begin{split}\vec{q}_{k+1} &= \vec{q}_k + \left( {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \ {\mat{J}(\vec{q}_k)} \right)^{-1} \bf{g}_k \\ \bf{g}_k &= {\mat{J}(\vec{q}_k)}^\top \mat{W}_e \vec{e}_k\end{split}\]

where \(\mat{J} = {^0\mat{J}}\) is the base-frame manipulator Jacobian. If \(\mat{J}(\vec{q}_k)\) is non-singular, and \(\mat{W}_e = \mat{1}_n\), then the above provides the pseudoinverse solution. However, if \(\mat{J}(\vec{q}_k)\) is singular, the above can not be computed and the GN solution is infeasible.

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_GN method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_GN(Tep)
IKSolution: q=[-0.07258, 0.5624, -2.317, -0.7923, -1.749, 0.429, -1.806], success=False, reason=iteration and search limit reached, 100 numpy.LinAlgError encountered, iterations=100, searches=100, residual=1.99

Notes

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ikine_LM() ikine_NR() ikine_QP() ik_GN()

Changed in version 1.0.4: Added the Gauss-Newton IK solver method on the ETS class

ikine_QP(Tep, q0=None, ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, kj=1.0, ks=1.0, kq=0.0, km=0.0, ps=0.0, pi=0.3, **kwargs)

Quadratic programming numerical inverse kinematics solver

Parameters:

• Tep (ndarray | SE3) – the desired end-effector pose or pose trajectory

• q0 (ndarray | Sequence[float] | None) – the initial joint coordinates, a vector (n,), or a matrix (m, n) whose rows are the starting points of the first m searches (any further searches start from random valid coordinates)

• ilimit (int) – maximum iterations allowed per search

• slimit (int) – maximum search attempts before failure

• tol (float) – maximum allowed residual error E

• mask (ndarray | Sequence[float] | None) – a 6-vector weighting Cartesian DoF error priority

• joint_limits (bool) – reject solutions with joint limit violations

• seed (int | None) – seed for the RNG used to generate random joint configurations

• kj (float) – gain for joint velocity norm minimisation

• ks (float) – gain adjusting the cost of slack (intentional error)

• kq (float) – gain for joint limit avoidance (0.0 disables)

• km (float) – gain for manipulability maximisation (0.0 disables)

• ps (float) – minimum joint approach distance to limit (radians or metres)

• pi (ndarray | float) – null-space influence distance (radians or metres)

Returns:

an IKSolution containing joint coordinates q, success flag, iterations, searches, residual error value, and reason string if applicable

Return type:

Any

If Tep is a trajectory (an SE3 with N poses, or an array with shape (N, 4, 4)) each pose is solved independently from the same q0, so consecutive solutions are not guaranteed to be on the same IK branch. q has shape (N, n), success is True only if every pose succeeded, and residual is the maximum over the poses. See solve().

Changed in version 1.4.5: For a trajectory, residual is the maximum over the poses, it was the minimum.

Raises:

ImportError – if the package qpsolvers is not installed

A method that provides functionality to perform numerical inverse kinematics (IK) using a quadratic programming approach.

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

Each iteration uses the following approach

\[\vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}.\]

where the QP is defined as

\[\begin{split}\min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\ \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu}, \\ \mathcal{A} \vec{x} &\leq \mathcal{B}, \\ \vec{x}^- &\leq \vec{x} \leq \vec{x}^+\end{split}\]

with

\[\begin{split}\vec{x} &= \begin{pmatrix} \dvec{q} \\ \vec{\delta} \end{pmatrix} \in \mathbb{R}^{(n+6)} \\ \mathcal{Q} &= \begin{pmatrix} \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6} \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\ \mathcal{J} &= \begin{pmatrix} \mat{J}(\vec{q}) & \mat{1}_{6} \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\ \mathcal{C} &= \begin{pmatrix} \mat{J}_m \\ \bf{0}_{6 \times 1} \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\ \mathcal{A} &= \begin{pmatrix} \mat{1}_{n \times n + 6} \\ \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\ \mathcal{B} &= \eta \begin{pmatrix} \frac{\rho_0 - \rho_s} {\rho_i - \rho_s} \\ \vdots \\ \frac{\rho_n - \rho_s} {\rho_i - \rho_s} \end{pmatrix} \in \mathbb{R}^{n} \\ \vec{x}^{-, +} &= \begin{pmatrix} \dvec{q}^{-, +} \\ \vec{\delta}^{-, +} \end{pmatrix} \in \mathbb{R}^{(n+6)},\end{split}\]

where \(\vec{\delta} \in \mathbb{R}^6\) is the slack vector, \(\lambda_\delta \in \mathbb{R}^+\) is a gain term which adjusts the cost of the norm of the slack vector in the optimiser, \(\dvec{q}^{-,+}\) are the minimum and maximum joint velocities, and \(\dvec{\delta}^{-,+}\) are the minimum and maximum slack velocities.

Examples

The following example gets the ets of a panda robot object, makes a goal pose Tep, and then solves for the joint coordinates which result in the pose Tep using the ikine_QP method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_QP(Tep)
!! [RUNBLOCK-ERROR] ets/ETS.py:docstring of roboticstoolbox.ets.ETS.ETS.ikine_QP:1430
    Install using 'pip install qpsolvers'

Notes

When using this method, the initial joint coordinates \(q_0\), should correspond to a non-singular manipulator pose, since it uses the manipulator Jacobian.

This class supports null-space motion to assist with maximising manipulability and avoiding joint limits. These are enabled by setting kq and km to non-zero values.

References

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part I: Kinematics, Velocity, and Applications.” arXiv preprint arXiv:2207.01796 (2022).

• J. Haviland, and P. Corke. “Manipulator Differential Kinematics Part II: Acceleration and Advanced Applications.” arXiv preprint arXiv:2207.01794 (2022).

See also

ikine_LM() ikine_NR() ikine_GN()

Changed in version 1.0.4: Added the Quadratic Programming IK solver method on the ETS class

inv()

Inverse of ETS

The inverse of a given ETS. It is computed as the inverse of the individual ETs in the reverse order.

\[(\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} )\]

Return type:

Self

Returns:

Inverse of the ETS

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz(jindex=2) * ET.tx(1) * ET.Rx(jindex=3,flip=True) * ET.tx(1)
>>> print(e)
Rz(q2) ⊕ tx(1) ⊕ Rx(-q3) ⊕ tx(1)
>>> print(e.inv())
tx(-1) ⊕ Rx(q3) ⊕ tx(-1) ⊕ Rz(-q2)

Notes

• It is essential to use explicit joint indices to account for

  the reversed order of the transforms.

jindex_set()

Get set of joint indices

Returns:

set of unique joint indices

Return type:

set[int]

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1)
>>> e.jindex_set()
{1, 2}

property jindices: ndarray

Get an array of joint indices

Returns:

array of unique joint indices

Return type:

ndarray

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1)
>>> e.jindices
array([1, 2, 1])

joint_idx()

Get index of joint transforms

Returns:

indices of transforms that are joints

Return type:

ndarray

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1)
>>> e.joint_idx()
array([0, 2])

joints()

Get a list of the variable ETs with this ETS

Returns:

list of ETs that are joints

Return type:

list[ETT]

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1)
>>> e.joints()
[ET.Rz(jindex=0), ET.Rz(jindex=1)]

property m: int

Number of transforms

Returns:

the number of transforms in the ETS

Return type:

int

Counts the number of transforms in the ETS.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rx() * ET.tx(1) * ET.tz()
>>> e.m
3

merge(i)

Merge two transforms in the ETS

Parameters:

i (int) – index of first transform

Return type:

None

Merges the two transforms at indices i and i+1. This is useful for reducing the number of transforms in an ETS.

Examples

>>> from roboticstoolbox import ET
>>> from math import pi
>>> e = ET.Rz() * ET.tx(1) * ET.tx(2) * ET.Rz(1)
>>> print(e)
Rz(q) ⊕ tx(1) ⊕ tx(2) ⊕ Rz(57.3°)
>>> e.merge(1)
>>> print(e)
Rz(q) ⊕ tx(3) ⊕ Rz(57.3°)
>>> e = ET.tx(1) * ET.Rx(0.1) * ET.Rx(pi/2) * ET.tx(2)
>>> print(e)
tx(1) ⊕ Rx(5.73°) ⊕ Rx(90°) ⊕ tx(2)
>>> e.merge(1)
>>> print(e)
tx(1) ⊕ Rx(95.73°) ⊕ tx(2)

Both transforms must be of the same kind, and at most one of them can be a joint.

property n: int

Number of joints

Returns:

the number of joints in the ETS

Return type:

int

Counts the number of joints in the ETS.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rx() * ET.tx(1) * ET.tz()
>>> e.n
2

See also

joints()

plot(*args, **kwargs)

Plot the sequence as a robot

The sequence is wrapped in a Robot (Robot2 for a planar sequence) and its plot method is called, which see for the arguments.

Return type:

None

property qlim: ndarray

Get/Set Joint limits

Limits are extracted from the link objects. If joints limits are not set for:

• a revolute joint [-𝜋. 𝜋] is returned

• a prismatic joint an exception is raised

Parameters:

new_qlim (ndarray(2,n)) – new joint limits to set

Returns:

array of joint limit values

Return type:

ndarray(2,n)

Raises:

ValueError – unset limits for a prismatic joint

Examples

>>> import roboticstoolbox as rtb
>>> robot = rtb.models.DH.Puma560()
>>> robot.qlim
array([[-2.7925, -0.7854, -3.927 , -1.9199, -1.7453, -4.6426],
       [ 2.7925,  3.927 ,  0.7854,  2.9671,  1.7453,  4.6426]])

Setter accepts:

ndarray | Sequence[float]

random_q(i=1)

Generate a random valid joint configuration

Parameters:

i (int) – number of configurations to generate

Returns:

random joint configuration

Return type:

ndarray

Generates a random q vector within the joint limits defined by self.qlim.

Examples

>>> import roboticstoolbox as rtb
>>> robot = rtb.models.Panda()
>>> ets = robot.ets()
>>> q = ets.random_q()
>>> q
array([-0.8414,  1.1288, -0.6403, -1.9503, -1.8938,  1.1227, -2.    ])

property structure: str

Joint structure string

A string comprising the characters ‘R’ or ‘P’ which indicate the types of joints in order from left to right.

Returns:

a string indicating the joint types

Return type:

str

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tz() * ET.tx(1) * ET.Rz() * ET.tx(1)
>>> e.structure
'PR'

swap(i)

Swap two transforms in the ETS

Parameters:

i (int) – index of first transform

Raises:

ValueError – if the two transforms do not commute

Return type:

None

Swaps the two transforms at indices i and i+1. This is useful for changing the order of commutative transforms in an ETS, and the kinematics of the sequence are unchanged. Two transforms commute if they are

• of the same kind, for example two translations along x

• both translations, along any axes

• a rotation and a translation along the axis of that rotation, for example Rz and tz

Anything else, such as a rotation and a translation along a different axis, does not commute and raises ValueError.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz() * ET.tx(1) * ET.ty(2) * ET.Rz(1)
>>> print(e)
Rz(q) ⊕ tx(1) ⊕ ty(2) ⊕ Rz(57.3°)
>>> e.swap(1)
>>> print(e)
Rz(q) ⊕ ty(2) ⊕ tx(1) ⊕ Rz(57.3°)
>>> e = ET.Rz() * ET.tz(1) * ET.Rx() * ET.Rz(1)
>>> e.swap(0)
>>> print(e)
tz(1) ⊕ Rz(q0) ⊕ Rx(q1) ⊕ Rz(57.3°)

teach(*args, **kwargs)

Interactively explore the sequence as a robot

The sequence is wrapped in a Robot (Robot2 for a planar sequence) and its teach method is called, which see for the arguments.

Return type:

None

split(method='last')

Split ETS into link segments

Parameters:

method (str) – one of "first", "last" (default), "dh", or "mdh".

Return type:

list[ETS]

Returns:

[base, *segments, gripper] – a list of length n_joints + 2. base/gripper are empty ETS when the method has no concept of one (e.g. "dh" never populates gripper, "mdh" never populates base).

Split an ETS into segments representing links, plus a base and gripper segment. Unpack with base, *segments, gripper = ets.split(method).

The behaviour depends on the method argument:

• "first": each link segment begins with a joint and continues upto, but not including the next joint. Any constant ET before the first joint are part of the base. There are no gripper ET, they are included in the last link segment.

• "last": each link segment ends with a joint and include all ET after the previous joint. Any constant ET after the last joint are part of the gripper. There are no base ET, they are included in the first link segment.

• "dh": similar to "first" but each segment is validated against the Denavit-Hartenberg convention: an ordered, 4-slot template Rz($θ_j$) tz($d_j$) tx($a_j$) Rx($α_j$), exactly one of Rz/tz being the segment’s joint and the rest optional (but present slots must occur in this order). Content between two joints that cannot be accounted for by the template raises ValueError. The base (content before the first joint’s template slots) is unvalidated and reported separately; trailing content past the last joint’s template is folded into the last segment.

• "mdh": similar to "last" but validated against the modified Denavit-Hartenberg convention, template tx($a_{j-1}$) Rx($α_{j-1}$) Rz($θ_j$) tz($d_j$), same rules as "dh" mirrored: the gripper (content past the last joint’s template slots) is unvalidated and reported separately; leading content before the first joint’s template is folded into the first segment.

>>> from roboticstoolbox import ET
>>> e = ET.tz(1) * ET.Rx("q1") * ET.tx(2) * ET.Ry("q2") * ET.ty(3) * ET.Rz("q3") * ET.tz(4)
>>> base, *segments, gripper = e.split("first")
>>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}")
Rx(q1) ⊕ tx(2)|Ry(q2) ⊕ ty(3)|Rz(q3) ⊕ tz(4) + base=tz(1), gripper=SE3()
>>> base, *segments, gripper = e.split("last")
>>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}")
tz(1) ⊕ Rx(q1)|tx(2) ⊕ Ry(q2)|ty(3) ⊕ Rz(q3) + base=SE3(), gripper=tz(4)
>>> e = ET.tz(1) * ET.Rz("q1") * ET.tx(2) * ET.Rx(90, 'deg') * ET.Rz("q2") * ET.tz(4) * ET.Rz(180, 'deg') * ET.tz("q3") * ET.Rz(270, 'deg') * ET.tz(4)
>>> base, *segments, gripper = e.split("dh")
>>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}")
Rz(q1) ⊕ tx(2) ⊕ Rx(90°)|Rz(q2) ⊕ tz(4)|Rz(180°) ⊕ tz(q3) ⊕ Rz(270°) ⊕ tz(4) + base=tz(1), gripper=SE3()
>>> base, *segments, gripper = e.split("mdh")
>>> print("|".join(str(_) for _ in segments), f"+ base={str(base)}, gripper={str(gripper)}")
tz(1) ⊕ Rz(q1)|tx(2) ⊕ Rx(90°) ⊕ Rz(q2) ⊕ tz(4)|Rz(180°) ⊕ tz(q3) + base=SE3(), gripper=Rz(270°) ⊕ tz(4)