This commit introduces a bug (which I cannot find) that make the robot move jerkily as soon as the control starts. Moreover the trajectory generation seems wrong.
- add weights to penalize tangential forces and moments at the feet
- add drift compensation to avoid foot slipping in simulation
- fix bug: initial value of trajectory duration was not set
- TEMP: remove feedforward acceleration and velocity terms (in momentum and posture tasks) because they seem to be wrong
- TEMP: remove inequalities in force QP
- DEBUG: add computation of SoT using computationally expensive method
- move function to compute 6d error in wbiAbstractTask.cpp so that it can be used by the Constraint class too
- add function to nicely print joint values