IK_QP - Numerical IK with Quadratic Programming

class IK_QP(name='IK Solver', ilimit=30, slimit=100, tol=1e-06, mask=None, joint_limits=True, seed=None, kj=0.01, ks=1.0, kq=0.0, km=0.0, ps=0.0, pi=0.3, **kwargs)[source]

Bases: IKSolver

Quadratic Programming Numerical Inverse Kinematics Solver

A class which provides functionality to perform numerical inverse kinematics (IK) using a quadratic programming approach. See step method for mathematical description.

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

Parameters:

name (str) – The name of the IK algorithm
ilimit (int) – How many iterations are allowed within a search before a new search is started
slimit (int) – How many searches are allowed before being deemed unsuccessful
tol (float) – Maximum allowed residual error E, where \(E = \tfrac{1}{2} \vec{e}^\top \mat{W}_e \vec{e}\) is a quadratic form in the 6-vector angle-axis pose error \(\vec{e}\) (see error()). Because E is quadratic, tol does not bound the linear-scale position/ orientation error directly — with the default unit weighting, components of \(\vec{e}\) are only guaranteed to be within roughly \(\sqrt{2 \cdot \text{tol}}\) (e.g. tol=1e-6 guarantees pose error on the order of 1e-3, not 1e-6). Pick tol accordingly if you need a specific linear-scale accuracy
mask (ndarray | Sequence[float] | None) – A 6 vector which assigns weights to Cartesian degrees-of-freedom error priority
joint_limits (bool) – Reject solutions with joint limit violations
seed (int | None) – A seed for the private RNG used to generate random joint coordinate vectors
verbose – If True, print the progress of each search: the error and joint coordinates at each iteration, and why a converged solution was rejected
kj (float) – A gain for joint velocity norm minimisation
ks (float) – A gain which adjusts the cost of slack (intentional error)
kq (float) – The gain for joint limit avoidance. Setting to 0.0 will remove this completely from the solution
km (float) – The gain for maximisation. Setting to 0.0 will remove this completely from the solution
ps (float) – The minimum angle/distance (in radians or metres) in which the joint is allowed to approach to its limit
pi (ndarray | Sequence[float] | float) – The influence angle/distance (in radians or metres) in null space motion becomes active

Raises:

ImportError – If the package qpsolvers is not installed, see the note above

Example:

The following example gets the ``ets`` of a ``panda`` robot object, instantiates
the `IK_QP` solver class using default parameters, makes a goal pose ``Tep``,
and then solves for the joint coordinates which result in the pose ``Tep``
using the ``solve`` method.

>>> import roboticstoolbox as rtb
>>> panda = rtb.models.Panda().ets()
>>> solver = rtb.IK_QP()
!! [RUNBLOCK-ERROR] robot/IK.py:docstring of roboticstoolbox.robot.IK.IK_QP:71
    Install them using 'pip install roboticstoolbox-python[qp]'
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> solver.solve(panda, Tep)
!! [RUNBLOCK-ERROR] robot/IK.py:docstring of roboticstoolbox.robot.IK.IK_QP:71
    NameError: name 'solver' is not defined

Notes

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

References

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

`step`(ets, Tep, q)	Performs a single iteration of the QP optimisation method
`solve`(ets, Tep[, q0])	Solves the IK problem
`error`(Te, Tep)	Calculates the error between Te and Tep

`_random_q`(ets[, i])	Generate a random valid joint configuration using a private RNG
`_check_jl`(ets, q)	Checks if the joints are within their respective limits