P-step can run optimization now. Need to check the solution. Will also add constraint on the time interval between consecutive knot points; such constraint are second order cone constraint with additional variables