Plan path independent from obstacles, then correction for obstacles