Add debug variable in integrator to access initial base position
Print constraints' errors
- [SOLVER IMPROVEMENT] if previous QP's solution slightly violates inequalities (less than 1e-10) modify inequalities so that they are no longer violated; this ensures that the only QP that can be unfeasible is the first one
- [CHANGE VALUE]: for computing the joints acceleration limits use a timestep that is 50 times the controller's timestep (before it was 10 times); this should get a more conservative behavior
- []INTEGRATOR] In case of integration of the Equations of Motion, use the same positions and velocities in solver and integrator, so that error is (almost) zero
- [DEBUG] if first QP fails or its solution violates any constraint then try to solve a new QP assuming zero joints' velocities and enlarged joint acceleration limits (so that ddq=0 satisfies the inequality constraints)