Support quadratic initial traj (not tested yet)