Problem 695488 · medium · Robotics · Level 06 Estimation and Autonomy

Where Is the Carrot Now?

pure pursuit · lookahead point · line-circle intersection · quadratic equation · polyline

A tractor follows a planned row with pure pursuit. Every control cycle it needs its lookahead point: the first point on the path, from where it has got to onwards, that is exactly L metres from the tractor.

The path is a polyline: the list of points path, joined by straight segments. Segment i runs from path[i] to path[i + 1], and its points are A + t\,(B - A) for 0 \le t \le 1 (with A = path[i], B = path[i + 1]). A position along the path is a pair (i, t); positions are ordered by segment first, then by t.

The tractor stands at robot = (rx, ry), and its progress is the position progress = (k, s) on the path (worked out elsewhere, from where it was before). The lookahead point is the point with the smallest position (i, t) that is at or after (k, s) and lies at distance exactly L from the tractor. If no point of the path from (k, s) on is at distance L (the end of the path is inside the circle, or the tractor is far off the path), the lookahead point is the last point of the path.

Write lookahead_point(path, robot, L, progress) that returns the lookahead point as a tuple (x, y).

Examples

Input:  path = [(0, 0), (4, 0), (4, 4)], robot = (1, 0.5), L = 2, progress = (0, 0.25)
Output: (2.9364916731037085, 0.0)
Explanation: on segment 0 the points at distance 2 have t = -0.234 (off the segment)
and t = 0.734 (at or after 0.25): the point (2.9365, 0).

Input:  path = [(0, 0), (4, 0), (4, 4)], robot = (3, 0.2), L = 1.5, progress = (0, 0.75)
Output: (4.0, 1.3180339887498949)
Explanation: segment 0 crosses the circle only at t = 0.378, behind the progress.
Segment 1 crosses it at t = 0.3295.

Input:  path = [(0, 0), (4, 0), (4, 4)], robot = (4, 3.5), L = 1, progress = (1, 0.875)
Output: (4.0, 4.0)
Explanation: the rest of the path lies inside the circle: aim at its end.

Constraints

  • consecutive points of path are different; answers are compared with a tolerance of 1e-6

Goals

  • Intersect a circle around the robot with each segment of a path by solving a quadratic in $t$
  • Pick the first crossing at or after the robot's progress along the path
  • Fall back to the path's end when no crossing is left
Starting Python…