Add install commands for PotentialMethod and start adding debug outputs
Fix errors, works without obstacle and optimization
Move finishSolve() from DiffusingPlanner to PathPlanner, and add debug outputs