eHQP

The eHQP solver (eHQP.h) is based on the work from Flacco et. al: Prioritized Multi-Task Motion Control of Redundant Robots under Hard Joint Constraints (paper).

The eHQP solver permits to resolve hierarchical QP problems with equality only constraints, without considering the \(\mathbf{c}\) term in the cost function. Hierarchies are considered though null-space projection.

Singularities are also checked and avoided thorugh damped pseudo-inverse. Solution of the problem is provided by the use of Eigen::JacobiSVD (webpage).