ETS for 3D space 

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)[source]

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.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)[source]

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.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)[source]

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

SymPy:: supported

classmethod ty(param=None, *, eta=None, **kwargs)[source]

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

SymPy:: supported

classmethod tz(param=None, *, eta=None, **kwargs)[source]

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

SymPy:: supported

classmethod SE3(T, **kwargs)[source]

A static SE3

Parameters:: T (ndarray | SE3) – The SE3 transformation matrix
Returns:: An elementary transform
Return type:: ET

See also

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 offset: float | Symbol

Offset of a joint

Returns:: the offset, zero unless one has been set
Raises:: TypeError – on set, if the transform is not a joint

A joint transforms by $\pm q + a$: the joint variable, negated if the joint is flipped, plus the offset $a$. This is the offset of a Denavit-Hartenberg link, and it is what remains of a constant transform of the same kind that is merged into a joint, see merge(). The joint limits qlim are limits on $q$, not on $\pm q + a$.

The offset may be symbolic. It is zero if it is exactly zero or, for a rotation, a whole number of turns of a symbolic angle.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz()
>>> e.offset = 0.5
>>> print(e)
Rz(q + 28.65°)
>>> e.A(0.1)
array([[ 0.8253, -0.5646,  0.    ,  0.    ],
       [ 0.5646,  0.8253,  0.    ,  0.    ],
       [ 0.    ,  0.    ,  1.    ,  0.    ],
       [ 0.    ,  0.    ,  0.    ,  1.    ]])
>>> print(e.inv())
Rz(q - 28.65°)

Note

A joint with an offset is evaluated by the Python code, not the compiled code, so it is slower. ik_LM, ik_NR and ik_GN, which need the compiled code, do not accept it.

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 adjacent transforms, in place
`swapped`	Swap two adjacent transforms, returning a copy
`merge`	Merge two adjacent transforms into one, in place
`merged`	Merge two adjacent transforms, returning a copy
`replace`	Replace a transform by an equivalent one on another axis, in place
`replaced`	Replace a transform by an equivalent one on another axis, returning a copy
`compile`	Compile an ETS

List operations

A sequence is a list of elementary transforms, so it has the methods of a list. Those that change it do so in place.

Method	Does
`append()`	adds an elementary transform to the end
`extend()`	adds the transforms of another sequence to the end
`pop`	removes and returns the transform at an index, by default the last
`remove()`	removes the first transform that is equal to the given one
`reverse()`	reverses the order of the transforms, which is not the inverse of the sequence, see `inv()`
`clear`	removes all the transforms
`count`	the number of transforms equal to the given one
`index`	the index of the first transform equal to the given one

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; they need the compiled extension, so not in the browser, and no SymPy parameters or joint offsets, see C++ Solvers
`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)[source]

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')[source]

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

A flipped joint can be fixed in degrees, without modifying the original sequence. This also works on the kinematics of a robot:

>>> from roboticstoolbox import ET, models
>>> ets = ET.Rz(flip=True) * ET.tx(1) * ET.Ry()
>>> fixed = ets.fixjoint(0, 90, unit="deg")
>>> fixed.n, ets.n
(1, 2)
>>> (1, 2)
(1, 2)
>>> robot = models.ETS.Panda()
>>> reduced = robot.ets().fixjoint(2, 0)
>>> reduced.n == robot.n - 1
True
>>> True
True

Added in version 1.5.0.

replace(i, axis)[source]

Replace a transform by an equivalent one on another axis, in place

Parameters:

i (int) – index of the transform
axis (str) – the new axis, "x", "y" or "z"

Raises:

IndexError – if i is not in the sequence
ValueError – if axis is not "x", "y" or "z", or the transform has no axis, such as an arbitrary SE3

Return type:

The transform at index i becomes three, which together mean the same thing: a constant quarter turn $R$, the transform moved to the new axis, and the inverse of $R$. This changes the sequence it is called on and returns None; use replaced() to get a modified copy and leave the original alone. If the transform is already on the axis nothing is changed.

The quarter turn is the one that carries the new axis onto the old, and it is about the third axis, so there is only one choice, and the transform keeps its sign: a joint is not flipped and its offset is unchanged. These are the substitutions of Corke’s method for assigning Denavit-Hartenberg parameters, for example

Rx(q) = Ry(90°) ⊕ Rz(q) ⊕ Ry(-90°)
Ry(q) = Rx(-90°) ⊕ Rz(q) ⊕ Rx(90°)
tx(d) = Ry(90°) ⊕ tz(d) ⊕ Ry(-90°)

The transform may be a translation or a rotation, a joint or a constant. A joint keeps its jindex, qlim and offset. The quarter turns are exact, pi/2 from SymPy, if the transform has a symbolic parameter or offset, and floating point otherwise.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1) * ET.Ry() * ET.tx(2)
>>> print(e)
tx(1) ⊕ Ry(q) ⊕ tx(2)
>>> e.replace(1, "z")
>>> print(e)
tx(1) ⊕ Rx(-90°) ⊕ Rz(q) ⊕ Rx(90°) ⊕ tx(2)
>>> e = ET.tx(1) * ET.ty(2) * ET.tx(3)
>>> e.replace(1, "z")
>>> print(e)
tx(1) ⊕ Rx(-90°) ⊕ tz(2) ⊕ Rx(90°) ⊕ tx(3)

With symbolic values:

>>> from roboticstoolbox import ET
>>> from spatialmath.base import symbol
>>> L = symbol("L")
>>> e = ET.Rz() * ET.tx(L) * ET.Rx()
>>> e.replace(1, "z")
>>> print(e)
Rz(q0) ⊕ Ry(pi/2) ⊕ tz(L) ⊕ Ry(-pi/2) ⊕ Rx(q1)

Added in version 1.5.0.

See also

replaced() swap()

replaced(i, axis)[source]

Replace a transform by an equivalent one on another axis, returning a copy

Parameters:

i (int) – index of the transform
axis (str) – the new axis, "x", "y" or "z"

Return type:

ETS

Returns:

a copy of the sequence with the transform at i replaced

Raises:

IndexError – if i is not in the sequence
ValueError – if axis is not "x", "y" or "z", or the transform has no axis, such as an arbitrary SE3

The sequence it is called on is not changed. See replace() for what the transform is replaced by.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.tx(1) * ET.Ry() * ET.tx(2)
>>> print(e.replaced(1, "z"))
tx(1) ⊕ Rx(-90°) ⊕ Rz(q) ⊕ Rx(90°) ⊕ tx(2)
>>> print(e)
tx(1) ⊕ Ry(q) ⊕ tx(2)

Added in version 1.5.0.

See also

replace() swapped()

compile()[source]

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)[source]

Insert value

Parameters:

index (int) – position to insert at
value (ET | ETS) – the elementary transform or sequence to insert

Return type:

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)[source]

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)[source]

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:

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)[source]

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:

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)[source]

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:

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)[source]

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:

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)[source]

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:

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)[source]

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:

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)[source]

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')[source]

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:

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)[source]

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:

ik_NR() ik_GN() ikine_LM()

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')[source]

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 needs the compiled C++ extension. It raises RuntimeError if the extension is unavailable, for example in a pure-Python wheel or under Pyodide/JupyterLite, and if the sequence has a SymPy parameter, even a numeric one such as sympy.pi/2, or a joint with an offset, which the compiled code cannot use. In all of these cases use ikine_LM(), which has none of these limits.

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=[1.136, -0.5709, -0.9438, -2.174, -0.5047, 1.882, 1.18], success=True, iterations=42, searches=4, residual=2.7e-07

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

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)[source]

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 needs the compiled C++ extension. It raises RuntimeError if the extension is unavailable, for example in a pure-Python wheel or under Pyodide/JupyterLite, and if the sequence has a SymPy parameter, even a numeric one such as sympy.pi/2, or a joint with an offset, which the compiled code cannot use. In all of these cases use ikine_NR(), which has none of these limits.

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)[source]

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

Warning

This method needs the compiled C++ extension. It raises RuntimeError if the extension is unavailable, for example in a pure-Python wheel or under Pyodide/JupyterLite, and if the sequence has a SymPy parameter, even a numeric one such as sympy.pi/2, or a joint with an offset, which the compiled code cannot use. In all of these cases use ikine_GN(), which has none of these limits.

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)[source]

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:

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.5.0: 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=[-2.477, 0.3678, 2.556, -2.195, -0.2298, 1.977, 0.9662], success=True, iterations=40, searches=4, residual=4.8e-07

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

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)[source]

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:

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.5.0: 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.1544, -0.2433, 0.9938, -1.254, -0.1537, 1.963, -2.606], success=False, reason=iteration and search limit reached, 100 numpy.LinAlgError encountered, iterations=100, searches=100, residual=2

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

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)[source]

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:

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.5.0: 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=[-1.5, 0.7954, -0.1998, -0.3236, -1.942, 3.078, 2.723], success=False, reason=iteration and search limit reached, 100 numpy.LinAlgError encountered, iterations=100, searches=100, residual=3.14

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

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)[source]

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:

ikine_LM() ikine_NR() ikine_GN()

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.5.0: For a trajectory, residual is the maximum over the poses, it was the minimum.

Raises:: ImportError – if the package qpsolvers is not installed, see the note below

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

Note

This needs the packages qpsolvers and quadprog, which are not installed with the Toolbox by default. Install them with the qp extra: pip install roboticstoolbox-python[qp].

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)
IKSolution: q=[1.212, -0.6262, -0.9884, -2.166, -0.565, 1.849, 1.226], success=True, iterations=139, searches=5, residual=1.98e-07

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

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

append(value): S.append(value) – append value to the end of the sequence

extend(values): S.extend(iterable) – extend sequence by appending elements from the iterable

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 adjacent transforms into one, in place

Parameters:

i (int) – index of the first transform

Raises:

IndexError – if i and i+1 are not both in the sequence
ValueError – if the transforms are not of the same kind, or both are joints

Return type:

Replaces the transforms at indices i and i+1 with a single transform. This changes the sequence it is called on and returns None; use merged() to get a modified copy and leave the original alone.

The two transforms must be of the same kind, for example two tx or two Rz. There are two cases:

Two constants become a constant whose parameter is the sum of the two.
A joint and a constant become the same joint, with the constant added to its offset, so Rz(q) * Rz(a) is Rz(q + a). The joint keeps its jindex, flip and qlim.

Two joints cannot be merged, since they are two variables.

The parameters and offsets may be numeric or symbolic, or one of each. If two constants add up to nothing the transform is removed altogether, rather than being left as a transform that does nothing. An offset that adds up to nothing is zero but the joint stays. The sum is nothing if it is

exactly zero, for example tx(x) * tx(-x) with x a SymPy symbol, or Rx(5) * Rx(-5)
for a rotation, a whole number of turns of a symbolic angle, for example Rz(pi/2) * Rz(3*pi/2) with pi from SymPy. A floating point sum such as Rz(pi/2) * Rz(3*pi/2) using math.pi is not exactly zero, so the rotation is kept.

Merging does not look at the neighbours of the result, so if removing a transform leaves two mergeable ones adjacent, call merge() again.

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)
>>> e = ET.tx(1) * ET.Rx(0.5) * ET.Rx(-0.5) * ET.tx(2)
>>> e.merge(1)
>>> print(e)
tx(1) ⊕ tx(2)

A constant merged into a joint becomes its offset, which may be symbolic:

>>> from roboticstoolbox import ET
>>> from spatialmath.base import symbol
>>> a = symbol("a")
>>> e = ET.tx(1) * ET.Rz() * ET.Rz(a) * ET.tx(2)
>>> e.merge(1)
>>> print(e)
tx(1) ⊕ Rz(q + a) ⊕ tx(2)
>>> e = ET.Rz(-a) * ET.Rz() * ET.Rz(a)
>>> e.merge(1)
>>> print(e)
Rz(-a) ⊕ Rz(q + a)
>>> e.merge(0)
>>> print(e)
Rz(q)

Changed in version 1.5.0: A joint can be merged with a constant, and a transform that adds up to nothing is removed.

See also

merged() swap()

merged(i)

Merge two adjacent transforms, returning a copy

Parameters:

i (int) – index of the first transform

Return type:

Self

Returns:

a copy of the sequence with transforms i and i+1 merged

Raises:

IndexError – if i and i+1 are not both in the sequence
ValueError – if the transforms are not of the same kind, or both are joints

The sequence it is called on is not changed. See merge() for which transforms can be merged and when the result is removed.

Examples

>>> from roboticstoolbox import ET
>>> e = ET.Rz() * ET.tx(1) * ET.tx(2) * ET.Rz(1)
>>> print(e.merged(1))
Rz(q) ⊕ tx(3) ⊕ Rz(57.3°)
>>> print(e)
Rz(q) ⊕ tx(1) ⊕ tx(2) ⊕ Rz(57.3°)

Added in version 1.5.0.

See also

merge() swapped()

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([-1.0622, -0.4925,  1.0927, -2.2199,  0.5729,  3.4896,  1.918 ])

remove(value): S.remove(value) – remove first occurrence of value. Raise ValueError if the value is not present.

reverse(): S.reverse() – reverse IN PLACE

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 adjacent transforms, in place

Parameters:

i (int) – index of the first transform

Raises:

IndexError – if i and i+1 are not both in the sequence
ValueError – if the two transforms cannot be swapped

Return type: