Planner still needs debugging

there might be still problems with collision checking! ;(