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:
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)
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()