Fix cartesian path

 fix for when compute cartesian path adds the first waypoint of an individual segment multiple times, once as the endpoint for the previous segment and once as the start point for the current segment.