ETS.ikine_QP

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

Quadratic Programming Numerical Inverse Kinematics Solver

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

See the Inverse Kinematics Docs Page for more details and for a tutorial on numerical IK, see here.

Parameters:

• Tep (Union[ndarray, SE3]) – The desired end-effector pose

• q0 (Union[ndarray, List[float], Tuple[float], Set[float], None]) – The initial joint coordinate vector

• 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 (Union[ndarray, List[float], Tuple[float], Set[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 (Optional[int]) – A seed for the private RNG used to generate random joint coordinate vectors

• kj – A gain for joint velocity norm minimisation

• ks – 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 (Union[ndarray, float]) – The influence angle/distance (in radians or metres) in null space motion becomes active

Raises:

ImportError – If the package qpsolvers is not installed

Synopsis

Each iteration uses the following approach

$${\mathit{q}}_{k+1}={\mathit{q}}_k+\dot{\mathit{q}}.$$

where the QP is defined as

$$\begin{array}{rl}\underset{x}{min}{f}_o(\mathit{x})& =\frac{1}{2}{\mathit{x}}^{\mathrm{\top }}\mathcal{Q}\mathit{x}+{\mathcal{C}}^{\mathrm{\top }}\mathit{x},\\ \text{subject to}\mathcal{J}\mathit{x}& =\mathit{\nu },\\ \mathcal{A}\mathit{x}& \le \mathcal{B},\\ {\mathit{x}}^{-}& \le \mathit{x}\le {\mathit{x}}^{+}\end{array}$$

with

$$\begin{array}{rl}\mathit{x}& =\left(\begin{array}{c}\dot{\mathit{q}}\\ \mathit{\delta }\end{array}\right)\in {\mathbb{R}}^{(n+6)}\\ \mathcal{Q}& =\left(\begin{array}{cc}{\lambda }_q{\mathbf{1}}_n& {\mathbf{0}}_{6\times 6}\\ {\mathbf{0}}_{n\times n}& {\lambda }_{\delta }{\mathbf{1}}_6\end{array}\right)\in {\mathbb{R}}^{(n+6)\times (n+6)}\\ \mathcal{J}& =\left(\begin{array}{cc}\mathbf{J}(\mathit{q})& {\mathbf{1}}_6\end{array}\right)\in {\mathbb{R}}^{6\times (n+6)}\\ \mathcal{C}& =\left(\begin{array}{c}{\mathbf{J}}_m\\ {\mathbf{0}}_{\mathbf{6}\times \mathbf{1}}\end{array}\right)\in {\mathbb{R}}^{(n+6)}\\ \mathcal{A}& =\left(\begin{array}{c}{\mathbf{1}}_{n\times n+6}\end{array}\right)\in {\mathbb{R}}^{(l+n)\times (n+6)}\\ \mathcal{B}& =\eta \left(\begin{array}{c}\frac{{\rho }_0-{\rho }_s}{{\rho }_i-{\rho }_s}\\ ⋮\\ \frac{{\rho }_n-{\rho }_s}{{\rho }_i-{\rho }_s}\end{array}\right)\in {\mathbb{R}}^n\\ {\mathit{x}}^{-,+}& =\left(\begin{array}{c}{\dot{\mathit{q}}}^{-,+}\\ {\mathit{\delta }}^{-,+}\end{array}\right)\in {\mathbb{R}}^{(n+6)},\end{array}$$

where $\mathit{\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, ${\dot{\mathit{q}}}^{-,+}$ are the minimum and maximum joint velocities, and ${\dot{\mathit{\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.

ImportError: the package qpsolvers is required for this class. 
Install using 'pip install qpsolvers'

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

See also

IK_NR

An IK Solver class which implements the Newton-Raphson optimisation technique

ikine_LM

Implements the IK_LM class as a method within the ETS class

ikine_GN

Implements the IK_GN class as a method within the ETS class

ikine_NR

Implements the IK_NR class as a method within the ETS class

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