fixed test_planner visualization
Fixed discrete env coord wraparound issue
Fixed initial config for plan