Fixed a bug: ALL robots were being passed into the discrete solver. Now only robots in conflict are passed in

When getting the nn for path tracking, take the closest one on the ref path that ocurrs the soonest and has not been visited already

Treat the optimization problem as a path tracking problem instead. Works both with and without the database as a conflict resolver