Improve computation time of HQP solver

[andreadelprete]

Currently the only QP solver implemented is eiquadprog, which is already rather efficient, but there's room for improvement. In particular, the current version of the solver allocates dynamic memory at each call to the solver, so it should be easy to decrease computation time by pre-allocating all the necessary memory in an initialization phase.

[andreadelprete]

In the last commit 8788221 I added two improved versions of eiquadprog, neither of which allocates dynamic memory during resolution (verified using Eigen function set_is_malloc_allowed). The only difference between them is that RtEiquadprog uses matrices with size known at compile time, whereas EiquadprogFast does not. In both solvers I optimized a bit the code using some Eigen tricks such as noalias, or solveInPlace. Finally, both new solvers have also more documentation and provide more information to the interested user, such as number of iterations, number of active inequalities, maximum number of iterations and proper status flag in case the problem could not be solved.

[andreadelprete]

Thiese are the results I got solving random QP on HRP-2's computer (but in a non-real-time thread). More or less we gained 15% of the computation time. The real-time solver is only slightly faster than the "fast" solver. The size of the problems has been chosen to match the size of the QP we typically have to solve in our inverse-dynamics balance controller.

1000 tests with 60 variables, 18 equalities, 40 inequalities
*** PROFILING RESULTS [ms] (min - avg - max - nSamples) ***
Eiquadprog ................... 0.651 0.704 0.870 1000
Eiquadprog Fast ............ 0.563 0.605 0.810 1000
Eiquadprog Real Time .... 0.543 0.592 0.712 1000

*** STATISTICS (min - avg - max - nSamples) ***
active inequalities .... 16.0 19.8 26.0 1000
solver iterations ........ 17.0 21.5 29.0 1000

[andreadelprete]

FYI: I also tried to warm start the algorithm (see macro USE_WARM_START in eiquadprog_fast.h) but I didn't manage to make it work properly (it only worked in the trivial case where the active set didn't change). Then I decided to stop working on it, but maybe it's just a matter of fixing a simple bug to make it work, in case someone is interested in finishing the work (the code should be quite self-explaining now because I added plenty of comments).

[andreadelprete]

I've just discovered a new open-source QP solver, which is supposed to be faster than eiquadprog, so we may give it a try by integrating it inside TSID. Anybody feels like doing it?

[andreadelprete]

Since this issue has been inactive for a while now I think we can close it.