-Add capability to plan to poses close to robot path