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

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