Porting and cleaning up WBIToolbox-Controllers

[gabrielenava]

Controllers to be ported: (see https://github.com/robotology-playground/WBI-Toolbox-controllers/tree/master/controllers)

• torqueBalancingYoga
• boundedTorqueControl
• torqueBalancingOnSeesaw
• torqueBalancingStandUp
• wholeBodyImpedance
• utilites
• tests on the real robots (iCubGenova02 and iCubGenova04)

[gabrielenava]

Guidelines to follow: https://github.com/loc2/element_qp-reactive-control/issues/21 and https://github.com/loc2/element_qp-reactive-control/issues/18

[gabrielenava]

Cleanup of the file for loading Yoga++ initial values is required, see https://github.com/loc2/lab-events-demos/issues/23#issuecomment-355575988

[gabrielenava]

Yoga controller has been ported and tested in simulation with this commit: bd91502

[gabrielenava]

Same comment of mex-wholebodymodel also for the seesaw controller. Does it make sense to port it now in the new repo? It is not used, and there are no plans to use it soon. Anyway, it will be always kept inside the legacy repo, and in my private fork. To conclude, I think only yoga, impedanceController and standup should be ported to the new repo.

[gabrielenava]

Ported completed for the standup controller in simulation with this commit: efd8740

[gabrielenava]

Impedance control ported and tested in simulation: e6b987f

[gabrielenava]

Added utilities (IMU, FT debug) with 80cd096

[gabrielenava]

New standup code working on icubgenova02: 3be6c0e

[gabrielenava]

config files added for icubgenova02 for all controllers: e8079bb

[gabrielenava]

Performing tests on the iCubGenova02 with @anqingd @GiulioRomualdi

[gabrielenava]

Only tests on iCubGenova04 missing.

[gabrielenava]

Closing, even if the tests on icubGenova04 are still missing we are using the new controllers as default on icubGenova02 without any issue.