MPC 02: Current support position is added to decision variables.
Less general swing trajectory generation.