I've implemented the wrench distribution entity. It seems fine. This mostly fixes #63.
It will probably need a couple of additions for future use, but the current entity is fully functional.
I have a test to execute on Talos to check the results (test_dcmZmpControl_distribute). In this test, I can either compute the force distribution and use it (just to display it), or just compute it without using it.
Compared to the previous solution, the wrench distribution requires:
- an integer signal phase to know the support state (positive for LEFT, negative for RIGHT, zero for BOTH).
- in double support phase, a scalar parameter rho to determine the ratio between left and right.
I am a bit disappointed because the bounds on the CoP do not seem to be really effective. They work in the unit tests, but they do not prevent the robot from falling in simulation when asking it to move too much forward (even when I configure the surface to be really small). I will double check the implementation and try to play a little bit with the gains.