Minor fixes.
Refactoring. Added CoM and feet states to robot structure. MPC_02 is broken.