Analytical KKT Solutions for Equality-Constrained QP Problems

[simeon-ned]

For inverse kinematics problems without inequality constraints, we can significantly improve performance by implementing an analytical solution based on the Karush-Kuhn-Tucker (KKT) system. This approach would bypass the iterative OSQP solver for these specific cases, potentially offering substantial speed improvements.

The standard form of such a QP problem is:

$$\begin{align} \text{min} \quad &\frac{1}{2}v^T P v + c^T v\\ \text{subject to} \quad &Ev = d \end{align}$$

The analytical solution is given by solving the following linear system:

$$ \begin{bmatrix} P & E^T \\ E & 0 \end{bmatrix} \begin{bmatrix} v \\ \lambda \end{bmatrix} = \begin{bmatrix} -c \\ d \end{bmatrix} $$

Where $\lambda$ represents the Lagrange multipliers.

For the even simpler case where there are no equality constraints at all (only the objective function), the solution becomes even more straightforward:

$$v = -P^{-1}c$$

This is simply the solution to the unconstrained quadratic optimization problem, which can be computed directly by solving a linear system.

To implement this approach, we would need to modify the LocalIKSolver class to detect when a problem contains only equality constraints or no constraints at all, and then use a specialized solver method that constructs and solves the appropriate linear system directly. This would involve implementing the KKT system solver using JAX's linear algebra functions and adding a configuration option to enable/disable this optimization. Appropriate unit tests would also be needed to verify correctness and performance.

Direct solution of the linear system should be considerably faster than iterative methods for problems with only equality constraints or no constraints. For well-conditioned problems, the direct solution may offer better numerical stability.