- Drastically increase nonlinear constraint violation cost (I think)
- Make state & input costs much smaller than constraint costs
- Add condition to stop simulation if nonlinear constraint
is not satisfied (doesn't seem to work for going off track)
- Create separate nonlinear constraints for each state to check
if it is on track and if it doesn't intersect each obstacle
- Change indexing of nonlinear constraints