Adds a linear segment to the path from the lastPose the the endPose. The heading will be interpolated tangent to the direction of travel along the segment, meaning the robot will always face the direction it is moving in.