Gait/Contact Planner runs on MPC thread and is thus delayed by solver time delays

[LouKordos]

Since the contact planner is running on the MPC thread and is not based on time, but on iterations instead, the gait planner is becoming increasingly out of sync with the expected gait. When the optimizer takes longer than dt seconds, the contact planner will still assume exactly dt seconds have passed and thus generate a delayed gait compared to the "real" time and even the previous gait given to the MPC.

The solution for this is to use a time-based contact planner and move the planning (and lift-off position updates) outside the MPC thread. This means the MPC will simply call the functions when it needs them and because absolute time values are used for getting the gait phase, delays are ignored.

[LouKordos]

Green is time-based function, yellow is a previous run based on MPC iterations. The delay is clearly visible here.