Numerical inverse kinematics

The numerical solvers are methods of the ETS class, and a Robot has methods of the same names that call the ETS of the robot. The ikine_XX methods are written in Python and the ik_XX methods in C++, where XX is the algorithm, for example LM. The C++ methods have limits that the Python ones do not. Each solver adds arguments of its own, so check its documentation for the complete list.

Common arguments

Besides Tep the numerical solvers share these arguments.

Arguments common to the numerical solvers
Argument	Meaning
`q0`	initial joint coordinates, a vector (`n`) for the first search or a matrix (`slimit`, `n`) with one row for each search
`ilimit`	the number of iterations allowed in one search
`slimit`	the number of searches allowed before the problem is deemed unsolvable
`tol`	the error tolerance for a solution to count as successful
`mask`	six weights, for translation and rotation about X, Y and Z, of how much each Cartesian degree of freedom matters; 0 ignores it
`joint_limits`	if true, reject solutions that violate the joint limits

The sections below describe each argument in detail.

ilimit

The ilimit specifies how many iterations are allowed within a single search. After ilimit is reached, either, a new attempt is made or the IK solution has failed depending on slimit

slimit

The slimit specifies how many searches are allowed before the problem is deemed unsolvable. The maximum number of iterations allowed is therefore ilimit x slimit. By having slimit > 1, a global search is performed. Since finding a solution with numerical IK heavily depends on the initial choice of q0, performing a global search where slimit >> 1 will provide a far greater chance of success.

q0

q0 is the initial joint coordinate vector. If q0 is 1 dimensional (, n), then q0 is only used for the first attempt, after which a new random valid initial joint coordinate vector will be generated. If q0 is 2 dimensional (slimit, n), then the next vector within q0 will be used for the next search.

tol

tol sets the error tolerance before the solution is deemed successful. The error is typically set by some quadratic error function

\[E = \frac{1}{2} \vec{e}^{\top} \mat{W}_e \vec{e}\]

where \(\vec{e} \in \mathbb{R}^6\) is the angle-axis error, and \(\mat{W}_e\) assigns weights to Cartesian degrees-of-freedom

mask

mask is a (,6) array that sets \(\mat{W}_e\) in error equation above. The vector has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value can be 0 (for ignore) or above to assign a priority relative to other Cartesian DoF.

For the case where the manipulator has fewer than 6 DoF the solution space has more dimensions than can be spanned by the manipulator joint coordinates.

In this case we use the mask option where the mask vector specifies the Cartesian DOF that will be ignored in reaching a solution. The number of non-zero elements must equal the number of manipulator DOF.

For example when using a 3 DOF manipulator tool orientation might be unimportant, in which case use the option mask=[1, 1, 1, 0, 0, 0].

joint_limits

setting joint_limits = True will reject solutions with joint limit violations. Note that finding a solution with valid joint coordinates is likely to take longer than without.

After convergence, prismatic joint coordinates are left unchanged. Revolute coordinates already within their limits are preserved, including limits outside the principal interval of \([-\pi, \pi]\). Other revolute coordinates are reduced to the principal angle and, if necessary, shifted by whole turns towards their limits. If no equivalent angle is within the limits, the solution is rejected when joint_limits = True. Setting joint_limits = False disables this rejection; it does not change a prismatic distance into an angle.

Others

There are other arguments which may be unique to the solver, so check the documentation of the solver you wish to use for a complete list and explanation of arguments.

C++ Solvers

The ik_XX methods are written in high performance C++ and wrapped in Python. They are extraordinarily fast, typically about 10 µs, but are hard to extend or modify. There are ik_LM, ik_GN and ik_NR, but no ik_QP.

These methods have been written purely for speed so they do not contain the niceties of the Python alternative. They handle a single pose only, Tep must not be a trajectory, and they give no failure reason, which is always empty. The lengths of q0 (n) and of mask (6) are checked, and a wrong length raises ValueError, but the other arguments are passed to the C++ code as they are, so it is important that you understand each of the parameters and that they are of the correct type. To solve for a trajectory use the Python solvers.

When the C++ solvers cannot be used

The ik_XX methods need the compiled C++ extension and raise RuntimeError if it cannot be used. This is so:

in the pure-Python wheel and in the browser under Pyodide/JupyterLite, which have no compiled code
if the sequence has a SymPy parameter, a symbol or expression as the parameter of a transform, even one with a numeric value such as sympy.pi/2
if the sequence has a joint with a nonzero offset, which is what merging a constant into a joint produces

Everything else in the Toolbox, such as fkine and jacob0, falls back to Python in these cases, but the C++ solvers do not. Use the ikine_XX methods, which work in all of them as long as every parameter has a numeric value, are not much harder to call, and take the same arguments with the exceptions noted in their documentation.

ETS C++ IK Methods

`ik_LM`	Fast Levenberg-Marquardt numerical inverse kinematics solver
`ik_GN`	Fast numerical inverse kinematics by Gauss-Newton optimisation
`ik_NR`	Fast numerical inverse kinematics using Newton-Raphson optimisation

Robot C++ IK Methods

`ik_LM`(Tep[, end, start, q0, ilimit, slimit, ...])	Fast Levenberg-Marquardt Numerical Inverse Kinematics Solver
`ik_GN`(Tep[, end, start, q0, ilimit, slimit, ...])	Fast numerical inverse kinematics by Gauss-Newton optimization
`ik_NR`(Tep[, end, start, q0, ilimit, slimit, ...])	Fast numerical inverse kinematics using Newton-Raphson optimization

In the following example, we create a Panda robot and use one of the fast IK solvers available within the Robot class.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()  # Make a Panda robot
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])  # Make a goal pose
>>> panda.ik_LM(Tep)  # Solve the IK problem
IKSolution: q=[0.5007, -0.3356, -0.4457, -2.197, -0.1636, 1.988, 0.9144], success=True, iterations=16, searches=2, residual=9.98e-11

In the following example, we create a Panda robot and then get the ETS representation. Subsequently, we use one of the fast IK solvers available within the ETS class.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()  # Make a Panda robot
>>> ets = panda.ets()  # Get the ETS
>>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])  # Make a goal pose
>>> ets.ik_LM(Tep)  # Solve the IK problem
IKSolution: q=[-1.778, 0.7841, 2.091, -2.15, -0.716, 1.75, 1.339], success=True, iterations=20, searches=1, residual=8.39e-08

Python Solvers

The ikine_XX methods are built on Python classes, one for each algorithm, which extend the abstract base class IKSolver, and the solve() method of each returns an IKSolution. They are slower than the C++ solvers, typically about 1 ms, but are easy to extend and modify. They also handle trajectories and give a failure reason.

The Abstract Base Class

The IKSolver provides basic functionality for performing numerical IK. Subclasses inherit from this class and must implement the solve() method. Additionally a subclass can redefine any other methods necessary such as error() to provide a custom error function.

The Solution DataClass

The IKSolution is a dataclasses.dataclass(), described at the top of this page.

Solving for a trajectory

If Tep is a trajectory of N poses, each pose is solved independently, starting from the same q0 (and the same random restarts) for every pose. Consecutive solutions are therefore not guaranteed to be on the same IK branch. The members of the returned IKSolution are:

q has shape (N, n), one row per pose
success is True only if every pose was solved
iterations and searches are summed over all poses
residual is the maximum over all poses, the worst case
reason is the reason given by the last pose that failed, if any

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

The Implemented IK Solvers

These solvers can be identified as classes with a name starting with IK_.

Class	Method	Notes
IK_LM - Levenberg-Marquardt Numerical IK	Levenberg-Marquardt	the solver used in the examples below
IK_GN - Gauss-Newton Numerical IK	Gauss-Newton	set `pinv=True` for a robot with more than 6 joints
IK_NR - Newton-Raphson Numerical IK	Newton-Raphson	set `pinv=True` for a robot with more than 6 joints
IK_QP - Numerical IK with Quadratic Programming	quadratic programming	needs the `qp` extra

Examples

In the following example, we create an IK Solver class and pass an ETS to it to solve the problem. This style may be preferable to experiments where you wish to compare the same solver on different robots.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()  # Make a Panda robot
>>> ets = panda.ets()  # Get the ETS of the Panda
>>> solver = rtb.IK_LM()  # Make an IK solver
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])  # Make a goal pose
>>> solver.solve(ets, Tep)  # Solve the IK problem
IKSolution: q=[0.1034, -0.3025, -0.09196, -2.203, -0.03145, 2.001, 0.8107], success=True, iterations=77, searches=6, residual=6.73e-07

Additionally, these class-based solvers have been implemented as methods within the ETS and Robot classes. The method names start with ikine_.

ETS Python IK Methods

`ikine_LM`	Levenberg-Marquardt numerical inverse kinematics solver
`ikine_QP`	Quadratic programming numerical inverse kinematics solver
`ikine_GN`	Gauss-Newton numerical inverse kinematics solver
`ikine_NR`	Newton-Raphson numerical inverse kinematics solver

Robot Python IK Methods

`ikine_LM`(Tep[, end, start, q0, ilimit, ...])	Levenberg-Marquardt Numerical Inverse Kinematics Solver
`ikine_QP`(Tep[, end, start, q0, ilimit, ...])	Quadratic Programming Numerical Inverse Kinematics Solver
`ikine_GN`(Tep[, end, start, q0, ilimit, ...])	Gauss-Newton Numerical Inverse Kinematics Solver
`ikine_NR`(Tep[, end, start, q0, ilimit, ...])	Newton-Raphson Numerical Inverse Kinematics Solver

Examples

In the following example, we create a Panda robot and use one of the IK solvers available within the Robot class. This style is far more convenient than the above example.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()  # Make a Panda robot
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])  # Make a goal pose
>>> panda.ikine_LM(Tep)  # Solve the IK problem
IKSolution: q=[1.477, 1.211, -2.1, -2.102, 1.052, 1.411, 0.004908], success=True, iterations=31, searches=3, residual=3.67e-08

In the following example, we create a Panda robot and then get the ETS representation. Subsequently, we use one of the IK solvers available within the ETS class.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda()  # Make a Panda robot
>>> ets = panda.ets()  # Get the ETS
>>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])  # Make a goal pose
>>> ets.ikine_LM(Tep)  # Solve the IK problem
IKSolution: q=[-1.187, 1.615, 2.262, -2.068, -1.337, 1.009, 1.703], success=True, iterations=41, searches=4, residual=3.33e-10