Regularization unit-test and fixing a bug

In this PR I mainly worked on an unit-test for regularization. This test consists in comparing the Vx values for LM regularization against control cost one. Note that both have the same result under the assumption that u = 0. (no gradient).

This unit-test was requested in #36 (closed). When I developed this unit-test I found a bug in the update of the V terms (https://gepgitlab.laas.fr/loco-3d/cddp/blob/topic/unittest/cddp/solver/solver.py#L236 and https://gepgitlab.laas.fr/loco-3d/cddp/blob/topic/unittest/cddp/solver/solver.py#L239). Note that now they use Quu_r and Qux_r.

Furthermore, I cleaned up a lot the code. With this is intended to have an easier code to understand.