1. SNOPT can solve the NLP in around 1 minute.
2. Add functions in FixedFootYawCoMPlanning to add constraints on CoM, H0 and comdot, rather than adding these constraints separately in each step.
3. The infeasibility appears again in the bilinear alternation