initial working planner, but doens't detect collisions