Work in progress from before break
Successfully add the equivalent of solve_eqp1 in trajopt