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