Draw sphere in goal location before planning