Implement ICentroidalMPC interface and add the StableCentroidalMPC class

[mebbaid]

This PR replaces the one in #703.

In this PR I will add the StableCentroidalMPC as derived class from a ICentroidalMPC together with keeping the original CentroidalMPC implementation unchanged.

The idea is also to add the ParametrizedCentroidalMPC derived class. However I will leave that to a separate PR till I fix the associated unit test issue and merge this one.

[mebbaid]

Put the PR back in draft in light of https://github.com/ami-iit/element_walking-with-payload/issues/59

[mebbaid]

For similar weights, the unit test produces the following results for tracking and angular momentum, which are very satisfactory to me. I will put this PR for review now so we agree on the details.

[mebbaid]

I am not sure why the valgrind CI check fails. Running the unitTest doing valgrind ./StableCentroidalMPCUnitTests in my machine i get the following output

===============================================================================
All tests passed (255 assertions in 1 test case)

==169640== 
==169640== HEAP SUMMARY:
==169640==     in use at exit: 15,319 bytes in 40 blocks
==169640==   total heap usage: 13,634,673 allocs, 13,634,633 frees, 368,146,061,381 bytes allocated
==169640== 
==169640== LEAK SUMMARY:
==169640==    definitely lost: 0 bytes in 0 blocks
==169640==    indirectly lost: 0 bytes in 0 blocks
==169640==      possibly lost: 0 bytes in 0 blocks
==169640==    still reachable: 15,319 bytes in 40 blocks
==169640==         suppressed: 0 bytes in 0 blocks
==169640== Rerun with --leak-check=full to see details of leaked memory
==169640== 
==169640== For lists of detected and suppressed errors, rerun with: -s
==169640== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)

which seems to indicate no memory errors. Any input on the CI failing for the memcheck would be helpful @GiulioRomualdi

[mebbaid]

As per suggestion, need to reduce the samples for the unit test.

[mebbaid]

I did some checks to verify that the CI is not failing for something more important in https://github.com/ami-iit/element_walking-with-payload/issues/53. The CI failure is incidental with no actual memory errors and not reproducible in local machines. Additionally changing the linear solver used (similar to what we do on the robot) significantly reduced computational time required.

[mebbaid]

PR is ready for review @GiulioRomualdi @S-Dafarra

[mebbaid]

The CI on ubuntu Debug is failing with

The following tests FAILED:
	112 - memcheck_ArucoDetectorTestUnitTests (Failed)
Errors while running CTest
Error: Process completed with exit code 8

which is not related to this PR i guess @GiulioRomualdi @S-Dafarra

Now this is not failing anymore. and PR ready for review.

[mebbaid]

We take the opportunity of putting this PR back in draft to test

• try to incorporate feet orientation from base estimator
• study more the effects of specific weights on the optimization problem feasibility as in https://github.com/ami-iit/element_walking-with-payload/issues/68#issuecomment-1932966873