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

A Tree That Grows Around the Boulders

RRT · sampling-based planning · tree · parent pointers · segment-circle collision

A Mars rover plans a drive across a boulder field with a rapidly-exploring random tree (RRT). To make the planner repeatable, its "random" points are given as a list samples. Positions are in metres; each boulder is a disc (cx, cy, r).

The tree starts with the single node start (node 0, no parent). If start is already within goal_radius of goal (distance <= goal_radius), the plan is just [start]. Otherwise take the samples in order; for each sample:

  1. Nearest. Find the tree node nearest to the sample (straight-line distance; on a tie, the node added earliest). If the sample is that node (distance 0), skip the sample.
  2. Steer. If the sample is within step of that node (distance <= step), the new point is the sample; otherwise it is the point step metres from the node toward the sample.
  3. Collide. If the segment from the node to the new point comes within or onto any boulder (the shortest distance from the boulder's centre to the segment is <= r), skip the sample: the tree does not change.
  4. Add the new point as a node whose parent is the nearest node.
  5. Goal. If the new node is within goal_radius of goal (<=), stop: return the path of nodes from start to the new node, following the parents.

If the samples run out first, return None. Return the path as a list of (x, y) tuples.

The setup provides scatter(n, seed, width, height): n sample points spread over the rectangle [0, w] \times [0, h], the same on every device. The larger tests use it. Press Run with plot(xs, ys) on your path to see the route.

Examples

Input:  start = (0, 0), goal = (3, 0), obstacles = [], samples = [(5, 0), (5, 0), (5, 1)],
        step = 1, goal_radius = 0.5
Output: [(0.0, 0.0), (1.0, 0.0), (2.0, 0.0), (2.948683298050514, 0.31622776601683794)]
Explanation: two steps along the x axis; the third grows from (2, 0) toward (5, 1)
and ends 0.32 m from the goal.

Input:  start = (0, 0), goal = (4, 0), obstacles = [(2, 0, 0.5)],
        samples = [(4, 0), (1, 2), (4, 2), (4, 0), (4, 0)], step = 1.5, goal_radius = 0.5
Output: [(0.0, 0.0), (0.6708203932499369, 1.3416407864998738), (2.142323497022232, 1.6326366895440598),
         (3.2690308160687436, 0.6424192299893302), (4.0, 0.0)]
Explanation: the first sample would end the branch at (1.5, 0), exactly 0.5 m from the
boulder's centre: touching counts, so it is skipped. The tree climbs over the boulder.

Input:  start = (0, 0), goal = (4, 0), obstacles = [(2, 0, 0.5)], samples = [(4, 0), (1, 2)],
        step = 1.5, goal_radius = 0.5
Output: None

Constraints

  • start lies outside every boulder; answers are compared with a tolerance of 1e-6

Goals

  • Grow a rapidly-exploring random tree from a given list of samples, one rule at a time
  • Reject a branch whose segment comes within an obstacle's radius
  • Recover the path from the tree by following parent links back to the root
Starting Python…