Added formulation_06.
CoM and swing foot state feedback.