Optimal trajectory planning for nonlinear systems: robust and constrained solution