Fixed path planning bug for point-robots.

For point-robots, the resultant tether for a shortest path would cut
through obstacles because the last tether point was always discarded
when leaving from an obstacle vertex.