# Inverse Kinematics

The Robotics Toolbox supports an extensive set of numerical inverse kinematics (IK) tools and we will demonstrate the different ways these IK tools can be interacted with in this document.

For a tutorial on numerical IK, see here.

Within the Toolbox, we have two sets of solvers: solvers written in C++ and solvers written in Python. However, within all of our solvers there are several common arguments:

Tep

Tep represent the desired end-effector pose.

A note on the semantics of the above variable:

• T represents an SE(3) (a homogeneous tranformation matrix in 3 dimensions, a 4x4 matrix)

• e is short for end-effector referring to the end of the kinematic chain

• p is short for prime or desired

• Since there is no letter to the left of the T, the world or base reference frame is implied

Therefore, Tep refers to the desired end-effector pose in the base robot frame represented as an SE(3).

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

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

These solvers are written in high performance C++ and wrapped in Python methods. The methods are made available within the ETS and Robot classes. Being written in C++, these solvers are extraordinarily fast and typically take 30 to 90 µs. However, these solvers are hard to extend or modify.

These methods have been written purely for speed so they do not contain the niceties of the Python alternative. For example, if you give the incorrect length for the q0 vector, you could end up with a seg-fault or other undetermined behaviour. Therefore, when using these methods it is very important that you understand each of the parameters and the parameters passed are of the correct type and length.

The C++ solvers return a tuple with the following members:

Element

Type

Description

q

ndarray

The joint coordinates of the solution. Note that these will not be valid if failed to find a solution

success

bool

True if a valid solution was found

iterations

int

How many iterations were performed

searches

int

How many searches were performed

residual

float

The final error value from the cost function

The C++ solvers can be identified as methods which start with ik_.

ETS C++ IK Methods

 ik_LM(Tep[, q0, ilimit, slimit, tol, mask, ...]) Fast levenberg-Marquadt Numerical Inverse Kinematics Solver ik_GN(Tep[, q0, ilimit, slimit, tol, mask, ...]) Fast numerical inverse kinematics by Gauss-Newton optimization ik_NR(Tep[, q0, ilimit, slimit, tol, mask, ...]) Fast numerical inverse kinematics using Newton-Raphson optimization

Robot C++ IK Methods

 ik_LM(Tep[, end, start, q0, ilimit, slimit, ...]) Fast levenberg-Marquadt 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 one of the fast IK solvers available within the Robot class.

>>> import roboticstoolbox as rtb
>>> # Make a Panda robot
>>> panda = rtb.models.Panda()
>>> # Make a goal pose
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> # Solve the IK problem
>>> panda.ik_LM(Tep)
(array([ 0.5007, -0.3356, -0.4457, -2.1972, -0.1636,  1.9881,  0.9144]), 1, 16, 2, 9.976484070621097e-11)


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

>>> import roboticstoolbox as rtb
>>> # Make a Panda robot
>>> panda = rtb.models.Panda()
>>> # Get the ETS
>>> ets = panda.ets()
>>> # Make a goal pose
>>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> # Solve the IK problem
>>> ets.ik_LM(Tep)
(array([ 0.5007, -0.3356, -0.4457, -2.1972, -0.1636,  1.9881,  0.9144]), 1, 16, 2, 9.976484070621097e-11)


## Python Solvers

These solvers are Python classes which extend the abstract base class IKSolver and the solve() method returns an IKSolution. These solvers are slow and will typically take 100 - 1000 ms. However, these solvers are easy to extend and modify.

The Abstract Base Class

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

The Solution DataClass

The IKSolution is a dataclasses.dataclass instance with the following members.

Element

Type

Description

q

ndarray

The joint coordinates of the solution. Note that these will not be valid if failed to find a solution

success

bool

True if a valid solution was found

iterations

int

How many iterations were performed

searches

int

How many searches were performed

residual

float

The final error value from the cost function

reason

str

The reason the IK problem failed if applicable

The Implemented IK Solvers

These solvers can be identified as a Class starting with IK_.

Example

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
>>> # Make a Panda robot
>>> panda = rtb.models.Panda()
>>> # Get the ETS of the Panda
>>> ets = panda.ets()
>>> # Make an IK solver
>>> solver = rtb.IK_LM()
>>> # Make a goal pose
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> # Solve the IK problem
>>> solver.solve(ets, Tep)
IKSolution(q=array([-2.006 ,  0.5697,  2.1978, -2.174 , -0.5032,  1.8822,  1.1792]), success=True, iterations=10, searches=1, residual=2.3488721776519882e-07, reason='Success')


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(Tep[, q0, ilimit, slimit, tol, ...]) Levemberg-Marquadt Numerical Inverse Kinematics Solver ikine_QP(Tep[, q0, ilimit, slimit, tol, ...]) Quadratic Programming Numerical Inverse Kinematics Solver ikine_GN(Tep[, q0, ilimit, slimit, tol, ...]) Gauss-Newton Numerical Inverse Kinematics Solver ikine_NR(Tep[, q0, ilimit, slimit, tol, ...]) Newton-Raphson Numerical Inverse Kinematics Solver

Robot Python IK Methods

 ikine_LM(Tep[, end, start, q0, ilimit, ...]) Levenberg-Marquadt 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

Example

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

>>> import roboticstoolbox as rtb
>>> # Make a Panda robot
>>> panda = rtb.models.Panda()
>>> # Make a goal pose
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> # Solve the IK problem
>>> panda.ikine_LM(Tep)
IKSolution(q=array([-0.617 , -0.3565,  0.5461, -2.1954,  0.2088,  1.9805,  0.6208]), success=True, iterations=10, searches=1, residual=3.31895442979262e-07, reason='Success')


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

>>> import roboticstoolbox as rtb
>>> # Make a Panda robot
>>> panda = rtb.models.Panda()
>>> # Get the ETS
>>> ets = panda.ets()
>>> # Make a goal pose
>>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> # Solve the IK problem
>>> ets.ikine_LM(Tep)
IKSolution(q=array([-1.396 , -0.8222,  1.0591, -2.1449,  0.7496,  1.7232,  0.2073]), success=True, iterations=11, searches=1, residual=7.265959113355526e-10, reason='Success')