Adds a linear segment to the path from the lastPose to the endPose. The heading will be interpolated linearly between the start and end poses.