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

Delete the old code that was solving motion planning problems to do conflict resolution instead of just doing path tracking

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

Planning with regular optimization kind of works. Update guide path each iteration and do some unintelligent goal re-assignment.

Added code from other repo to find and resolve discrete subproblems. Added place grid and get starts and goals to traj opt db resolver