1. Add the back-off step for the alternation, to make sure the solution is strictly feasible.
2. Fix a bug in AngularMomentumCost on the F_idx
3. Change the quadratic cost in the NLP from penalizing compp to the actual comddot