Notebook 7.2 - Rapidly Exploring Random Trees

This notebook contains a simple implementation of the RRT algorithm that we covered in Part 4 of our session.

RRTs are algorithms that allow the exploration of a search space. They are space-filling algorithm that randomly grow towards unexplored areas of the problem and are widely used in autonomous motion planning.

RRTs have 3 main processes:

1. Random node generation - where an area is selected at random to be explored.
2. Closest node finding - assign the newly generated random node to the closest node in the existing tree.
3. Step node generation - the newly generated node is then moved so that the distance to the closest node in the tree equals a specified step distance.

Our implementation is purely numerical, and we do not rely on external packages. We only need matplotlib, for visualisation - so we check that it has been installed correctly.

!pip install matplotlib

Requirement already satisfied: matplotlib in /opt/anaconda3/lib/python3.8/site-packages (3.2.2)
Requirement already satisfied: cycler>=0.10 in /opt/anaconda3/lib/python3.8/site-packages (from matplotlib) (0.10.0)
Requirement already satisfied: kiwisolver>=1.0.1 in /opt/anaconda3/lib/python3.8/site-packages (from matplotlib) (1.2.0)
Requirement already satisfied: numpy>=1.11 in /opt/anaconda3/lib/python3.8/site-packages (from matplotlib) (1.19.2)
Requirement already satisfied: python-dateutil>=2.1 in /opt/anaconda3/lib/python3.8/site-packages (from matplotlib) (2.8.1)
Requirement already satisfied: pyparsing!=2.0.4,!=2.1.2,!=2.1.6,>=2.0.1 in /opt/anaconda3/lib/python3.8/site-packages (from matplotlib) (2.4.7)
Requirement already satisfied: six in /opt/anaconda3/lib/python3.8/site-packages (from cycler>=0.10->matplotlib) (1.15.0)

import math
import random
import matplotlib.pyplot as plt

def generate_random(num_id, max_iters):
    num_id += 1
    from_id = None
    # final iterations generate the nodes closer to the destination
    rand_x = end_x + (random.random() * 100 - end_x) * (1 - num_id / max_iters)
    rand_y = end_y + (random.random() * 100 - end_y) * (1 - num_id / max_iters)
    rand_node = (rand_x, rand_y, num_id, from_id)

    return rand_node

def find_closest_node(rand_node):
    distance_min = float('inf')
    closest_node = None
    closest_node_idx = None

    for i,each_node in enumerate(point_list):
        distance = math.sqrt((rand_node[0] - each_node[0]) ** 2 + (rand_node[1] - each_node[1]) ** 2)

        if distance < distance_min:
            distance_min = distance
            closest_node = each_node
            closest_node_idx = i

    return distance_min, closest_node, rand_node, closest_node_idx

def choose_new_node(distance_min, closest_node, rand_node, closest_idx):  # take the point according to the step length limit, call that point as new_node

    new_x = closest_node[0] + (rand_node[0] - closest_node[0]) / distance_min * step_distance
    new_y = closest_node[1] + (rand_node[1] - closest_node[1]) / distance_min * step_distance

    if new_x >= obs_x[0] and new_x <= obs_x[1] and new_y >= obs_y[0] and new_y <= obs_y[1]:  # set obstacles
        return
    else:
        new_node = (new_x, new_y, len(point_list), closest_idx)

        point_list.append(new_node)
        return new_node

def step_plot(iteration_num):

    plt.axis([0, 100, 0, 100])
    plt.title(f'RRT - iteration {iteration_num}')
    plt.xlabel('x-axis')
    plt.ylabel('y-axis')

    # plot start and end nodes
    plt.plot([start_x], [start_y], marker='o', markersize=5)
    plt.plot([end_x], [end_y], marker='o', markersize=5)

    # plot obstacle
    plt.plot([obs_x[0], obs_x[1], obs_x[1], obs_x[0], obs_x[0]],
             [obs_y[0], obs_y[0], obs_y[1], obs_y[1], obs_y[0]])

    for n in point_list:
        p_idx = n[3]
        if p_idx is None:
            continue
        p = point_list[p_idx]
        plt.plot([p[0], n[0]], [p[1], n[1]], 'k', marker='o', markersize=2)

    plt.pause(0.1)

def find_path(last_new_node):
    path_list = []
    from_id = last_new_node[2]
    path_list.append(from_id)

    while from_id != None:
        from_id_new = point_list[from_id]
        from_id = from_id_new[3]
        if from_id != None:
            path_list.append(from_id)
    return path_list


def plot_final(path_list):
    plt.axis([0, 100, 0, 100])
    plt.title('RRT')
    plt.xlabel('x-axis')
    plt.ylabel('y-axis')
    plt.plot([start_x], [start_y], marker='o', markersize=5)
    plt.plot([end_x], [end_y], marker='o', markersize=5)

    # plot obstacle
    plt.plot([obs_x[0], obs_x[1], obs_x[1], obs_x[0], obs_x[0]],
             [obs_y[0], obs_y[0], obs_y[1], obs_y[1], obs_y[0]])

    for n in point_list:
        p_idx = n[3]
        if p_idx is None:
            continue
        p = point_list[p_idx]
        plt.plot([p[0], n[0]], [p[1], n[1]], 'k', marker='o', markersize=2)

    for i in range(len(path_list) - 1):
        orig = path_list[i]
        dest = path_list[i+1]
        ni = point_list[orig]
        nj = point_list[dest]
        plt.plot([ni[0], nj[0]], [ni[1], nj[1]], 'r', marker='o', markersize=2)

Algorithm Implementation

point_list = []  # contains tuples (coordinate of nodes)

start_x = 0
start_y = 0
end_x = 60
end_y = 70
obs_x = [30, 40]
obs_y = [30, 40]
step_distance = 5

# maximum number of iterations allowed
max_iters = 200

# add origin node to the tree
point_list.append((start_x, start_y, 0, None))
end_node = (end_x, end_y, None, None)
final_tree = None


for num_id in range(max_iters):
    # create random node
    rand_node = generate_random(num_id, max_iters)
    
    # find closest node in tree
    distance_min, closest_node, rand_node, closest_idx = find_closest_node(rand_node)
    
    # add node to tree
    new_node = choose_new_node(distance_min, closest_node, rand_node, closest_idx)
    
    # if node exists (i.e. node is not inside obstacle)
    if new_node != None:
        # plot tree
        step_plot(num_id)
        
        # find distance to the destination
        distance = math.sqrt((new_node[0] - end_node[0]) ** 2 + (new_node[1] - end_node[1]) ** 2)
        
        # if the destination is within step distance 
        if distance <= step_distance:
            final_tree = (end_node[0], end_node[1], len(point_list), new_node[2])
            point_list.append(final_tree)
            print(f'\nThe destination has been found at iteration {num_id}.')

            break
        else:
            continue
    else:
        continue


if final_tree is None:
    print('\nThe destination was not found, try increasing the number of iterations!')

# obtain the final path from the tree
path = find_path(final_tree)

# plot the final path
plot_final(path)
plt.show()