Changes made to fix issue of robot overshooting goal. The planner now
plans based on the robot's motion state at the next control cycle.