Robot.ikine_QP

Robot.ikine_QP(Tep, end=None, start=None, 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, tool=None, **kwargs)

Quadratic Programming Numerical Inverse Kinematics Solver

Parameters:

Tep (ndarray | SE3) – The desired end-effector pose or pose trajectory
end (str | Link | Gripper | None) – the link considered as the end-effector
start (str | Link | Gripper | None) – the link considered as the base frame, defaults to the robots’s base frame
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) – 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
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
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 | float) – The influence angle/distance (in radians or metres) in null space motion becomes active
tool (ndarray | SE3 | None) – a static tool transformation matrix to apply to the end of end; defaults to the robot’s own self.tool if not given

Raises:

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

Returns:

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

Return type:

Any

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

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

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 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()
>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854])
>>> panda.ikine_QP(Tep)
IKSolution: q=[1.203, 1.597, -2.252, -2.071, 1.321, 1.029, -0.13], success=True, iterations=283, searches=10, residual=6.8e-07

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.

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