├── .gitattributes ├── .gitignore ├── README.md ├── app.py ├── bi_rrt.py ├── images ├── apf.png └── bi_rrt.png ├── pf.py └── requirements.txt /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .env 2 | .vim 3 | __pycache__/ 4 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Path planning algorithms 2 | 3 | ### Visualization using Streamlit 4 | 5 | ```bash 6 | streamlit run app.py 7 | ``` 8 | 9 | 10 | ### Bi-RRT 11 | Creates random trees simulatneously from both, the start and goal. 12 | ``` 13 | usage: bi_rrt.py [-h] [-i ITER] 14 | 15 | Bi-Directional RRT 16 | 17 | optional arguments: 18 | -h, --help show this help message and exit 19 | -i ITER, --iter ITER Number of iterations (integer); Default=100 20 | ``` 21 | 22 | ![birrt](./images/bi_rrt.png) 23 | 24 | ### APF (Artificial Potential Function) 25 | Assigns a potential value to each cell in the grid which is the sum of the attractive 26 | and repulsive potential strengths. 27 | 28 | ``` 29 | usage: pf.py [-h] [-g GRID] [-f FUNCTION] [-a ATTRACTIVE] [-r REPULSIVE] 30 | 31 | Artificial Potential Function 32 | 33 | optional arguments: 34 | -h, --help show this help message and exit 35 | -g GRID, --grid GRID Grid size; Default=0.5 36 | -f FUNCTION, --function FUNCTION 37 | Attractive Potential function(p=paraboloid, c=conical); Default=c 38 | -a ATTRACTIVE, --attractive ATTRACTIVE 39 | Attractive Potential Gain; Default=1 40 | -r REPULSIVE, --repulsive REPULSIVE 41 | Repuslive Potential Gain; Default=5000 42 | ``` 43 | 44 | ![apf](./images/apf.png) 45 | -------------------------------------------------------------------------------- /app.py: -------------------------------------------------------------------------------- 1 | import streamlit as st 2 | import bi_rrt 3 | import pf 4 | 5 | 6 | st.title("Path Planning algorithms") 7 | st.header("Bi-RRT") 8 | 9 | max_iter = st.slider('Iterations', 0, 1000, 100) # min: 0h, max: 23h, default: 17h 10 | fig_birrt = bi_rrt.main(max_iter) 11 | st.pyplot(fig_birrt) 12 | 13 | st.header("APF") 14 | grid_size = st.slider("Grid size", 0.0, 5.0, 0.5) 15 | func = st.radio("Attractive Potential Function", ['Paraboloid', 'Conical']) 16 | K = st.slider("Attractive Potential Gain", 0.1, 10.0, 1.0) 17 | ETA = st.slider("Repulsive Potential Gain", 500, 10000, 5000) 18 | fig_apf = pf.main(grid_size, func, ETA, K) 19 | st.pyplot(fig_apf) 20 | -------------------------------------------------------------------------------- /bi_rrt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | import matplotlib.pyplot as plt 5 | import math 6 | import numpy as np 7 | import random 8 | import argparse 9 | 10 | 11 | class Node: 12 | def __init__(self, x, y): 13 | self.x = x 14 | self.y = y 15 | self.path_so_far = [] 16 | 17 | 18 | def get_random_point(lc=0, rc=30): 19 | rand_point = Node(random.uniform(lc,rc),random.uniform(lc, rc)) 20 | return rand_point 21 | 22 | def merge_intersetion(node1, node2): 23 | global obslist 24 | for obs in obslist: 25 | dr = np.hypot(node2.x-node1.x, node2.y-node1.y) 26 | D = (node1.x-obs[0])*(node2.y-obs[1]) - (node2.x-obs[0])*(node1.y-obs[1]) 27 | d = (obs[2]*dr)**2 - D**2 28 | if d >= 0: 29 | return False 30 | return True 31 | 32 | def get_nearest_point(point, tree): 33 | distance_list = [np.hypot(node.x - point.x, node.y - point.y)**2 for node in tree] 34 | return distance_list.index(min(distance_list)) 35 | 36 | def get_new_point(rnd_point, old_point, delta=1): 37 | theta = math.atan((old_point.y-rnd_point.y)/(old_point.x-rnd_point.x)) 38 | temp_p1 = Node(delta*math.cos(theta) + old_point.x, delta*math.sin(theta) + old_point.y) 39 | temp_p2 = Node(-delta*math.cos(theta) + old_point.x, -delta*math.sin(theta) + old_point.y) 40 | temp_tree = [temp_p1, temp_p2] 41 | new_node = temp_tree[get_nearest_point(rnd_point, temp_tree)] 42 | val = merge_intersetion(new_node, old_point) 43 | if val and new_node.x >= 0 and new_node.y >=0 and new_node.x <= 30 and new_node.y <=30: 44 | return new_node 45 | return False 46 | 47 | def add_point(tree): 48 | rand_point = get_random_point() 49 | min_dist_index = get_nearest_point(rand_point, tree) 50 | old_point = tree[min_dist_index] 51 | new_node = get_new_point(rand_point, old_point) 52 | if new_node: 53 | tree.append(new_node) 54 | new_node.path_so_far = old_point.path_so_far + [new_node] 55 | 56 | def merge(tree1, tree2): 57 | flag = 0 58 | nl = [] 59 | for node in tree1[::-1]: 60 | for n in tree2[::-1]: 61 | if merge_intersetion(node, n): 62 | nl.append((node, n, len(node.path_so_far) + len(n.path_so_far))) 63 | if nl != []: 64 | nl.sort(key=lambda x: x[2]) 65 | path = nl[0][0].path_so_far + nl[0][1].path_so_far[::-1] 66 | print("Done!!") 67 | else: 68 | print("No path found") 69 | path = None 70 | return path 71 | 72 | def main(max_iter): 73 | global obslist 74 | obslist = [(4.5, 3, 2), (3, 12, 2), (15, 15, 3)] #[(x, y, radius)] 75 | start_node = Node(1,1) 76 | start_node.path_so_far.append(start_node) 77 | goal_node = Node(20,20) 78 | goal_node.path_so_far.append(goal_node) 79 | start_tree = [start_node] 80 | goal_tree = [goal_node] 81 | for i in range(max_iter): 82 | add_point(start_tree) 83 | add_point(goal_tree) 84 | path = merge(start_tree, goal_tree) 85 | 86 | """ 87 | Plotting 88 | """ 89 | if path: 90 | figure, axes = plt.subplots() 91 | plt.rcParams["figure.figsize"] = (15,15) 92 | 93 | # plotting the obstacles 94 | for obs in obslist: 95 | obstacle = plt.Circle((obs[0], obs[1]), obs[2], color="black") 96 | axes.add_artist(obstacle) 97 | 98 | # plotting both the trees 99 | trees = [start_tree, goal_tree] 100 | color = ["blue", "green"] 101 | lab = ["Tree from start point", "Tree from end point"] 102 | for i,tree in enumerate(trees): 103 | x_cord = [] 104 | y_cord = [] 105 | for v in tree: 106 | x_cord.append(v.x) 107 | y_cord.append(v.y) 108 | plt.scatter(x_cord, y_cord, c=[color[i]], label=lab[i]) 109 | 110 | # plotting path 111 | plt.plot(1,1,'kp') #start 112 | plt.plot(20,20,'kp') #goal 113 | x_cord = [] 114 | y_cord = [] 115 | for v in path: 116 | x_cord.append(v.x) 117 | y_cord.append(v.y) 118 | plt.plot(x_cord, y_cord, "r-", linewidth=1, label='Final Path') 119 | 120 | axes.set_aspect(1) 121 | plt.xlim(0,30) 122 | plt.ylim(0,30) 123 | plt.legend() 124 | plt.title('Configuration Space') 125 | plt.savefig("./images/bi_rrt.png") 126 | # plt.show() 127 | return figure 128 | 129 | 130 | if __name__ == '__main__': 131 | parser = argparse.ArgumentParser(description='Bi-Directional RRT') 132 | parser.add_argument('-i','--iter', type=int, 133 | help='Number of iterations (integer); Default=100', default=100) 134 | args = parser.parse_args() 135 | max_iter = args.iter 136 | 137 | main(max_iter) 138 | -------------------------------------------------------------------------------- /images/apf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/siddharth17196/path-planning-algorithms/e0302762b62f1cf337d5b87ac9fb0e4328025e4b/images/apf.png -------------------------------------------------------------------------------- /images/bi_rrt.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/siddharth17196/path-planning-algorithms/e0302762b62f1cf337d5b87ac9fb0e4328025e4b/images/bi_rrt.png -------------------------------------------------------------------------------- /pf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # coding: utf-8 3 | 4 | import matplotlib.pyplot as plt 5 | import numpy as np 6 | import argparse 7 | 8 | 9 | class Node: 10 | def __init__(self, x, y): 11 | self.x = x 12 | self.y = y 13 | 14 | 15 | def attractive_potential(x, y, goal, K_att, func): 16 | if func == 'p': 17 | return K_att * (np.hypot(x - goal.x, y - goal.y))**2 18 | else: 19 | return K_att * (np.hypot(x - goal.x, y - goal.y)) 20 | 21 | 22 | def repulsive_potential(x, y, K_rep): 23 | global obslist 24 | influence_region = 1 25 | min_dist = float("inf") 26 | dist_list = [np.hypot(x-o[0], y-o[1]) for o in obslist] 27 | closest_index = dist_list.index(min(dist_list)) 28 | dq = np.hypot(x - obslist[closest_index][0], y - obslist[closest_index][1]) 29 | region = obslist[closest_index][2] + influence_region 30 | if dq <= region: 31 | if dq <= 0.3: 32 | dq = 0.3 33 | return 0.5 * K_rep * (1/dq - 1/region) ** 2 # gamma = 2 34 | else: 35 | return 0 36 | 37 | def get_potential_matrix(goal, grid_size, params, func, minx=0, maxx=30): 38 | global obslist 39 | min_x = min_y = minx 40 | max_x = max_y = maxx 41 | x_grid = int(round((max_x - min_x) / grid_size)) 42 | y_grid = int(round((max_y - min_y) / grid_size)) 43 | 44 | potential_grid = [[0.0 for i in range(y_grid)] for i in range(x_grid)] 45 | 46 | for i in range(x_grid): 47 | x = i * grid_size + min_x 48 | for j in range(y_grid): 49 | y = j * grid_size + min_y 50 | u_att = attractive_potential(x, y, goal, params[0], func) 51 | u_rep = repulsive_potential(x, y, params[1]) 52 | uf = u_rep + u_att 53 | potential_grid[i][j] = uf 54 | 55 | return potential_grid, min_x, min_y 56 | 57 | 58 | def potential_field_planning(start, goal, grid_size, params, func): 59 | pmap, minx, miny = get_potential_matrix(goal, grid_size, params, func) 60 | robot_path = Node(start.x, start.y) 61 | move = [] 62 | directions = [0,1,1] # move at max 1 step 63 | for i in directions: 64 | for j in directions: 65 | move.append([i,j]) 66 | move.remove([0,0]) # [0,0] => robot doesnt move 67 | path = [start] 68 | d = np.hypot(start.x - goal.x, start.y - goal.y) 69 | lim = 0 70 | while d >= grid_size: 71 | min_potential = float("inf") 72 | min_pot_x, min_pot_y = -100, -100 73 | for i, _ in enumerate(move): 74 | moved_x = int(robot_path.x + move[i][0]) 75 | moved_y = int(robot_path.y + move[i][1]) 76 | if moved_x >= len(pmap) or moved_y >= len(pmap[0]) or moved_x < 0 or moved_y < 0: 77 | p = float("inf") # outside area 78 | print("outside region!") 79 | else: 80 | p = pmap[moved_x][moved_y] 81 | if min_potential > p: 82 | min_potential = p 83 | min_pot_x = moved_x 84 | min_pot_y = moved_y 85 | robot_path.x = min_pot_x 86 | robot_path.y = min_pot_y 87 | x_final = robot_path.x * grid_size + minx 88 | y_final = robot_path.y * grid_size + miny 89 | d = np.hypot(goal.x - x_final, goal.y - y_final) 90 | path.append(Node(x_final, y_final)) 91 | lim +=1 92 | 93 | print("Done!!") 94 | return path, pmap 95 | 96 | 97 | def main(grid_size, func, ETA, K): 98 | global obslist 99 | start = Node(1,1) 100 | goal = Node(20,20) 101 | obslist = [(4.5, 3, 2), (3, 12, 2), (15, 15, 3)] #[(x, y, radius)] 102 | grid_size = 0.5 103 | path, pmap = potential_field_planning(start, goal, grid_size, [K, ETA], func) 104 | 105 | """ 106 | Plotting 107 | """ 108 | figure, axes = plt.subplots() 109 | plt.rcParams["figure.figsize"] = (15,15) 110 | 111 | # plotting the obstacles 112 | for obs in obslist: 113 | obstacle = plt.Circle((obs[0], obs[1]), obs[2], color="black", fill=False) 114 | axes.add_artist(obstacle) 115 | 116 | # plotting path 117 | plt.plot(1,1,'kp') #start 118 | plt.plot(20,20,'kp') #goal 119 | x_cord = [] 120 | y_cord = [] 121 | for v in path: 122 | x_cord.append(v.x) 123 | y_cord.append(v.y) 124 | plt.plot(x_cord, y_cord, "r-", linewidth=1, label='Final Path') 125 | 126 | axes.set_aspect(1) 127 | plt.xlim(0,30) 128 | plt.ylim(0,30) 129 | plt.legend() 130 | plt.title('Configuration Space') 131 | plt.savefig("./images/apf.png") 132 | # plt.show() 133 | return figure 134 | 135 | 136 | if __name__ == '__main__': 137 | parser = argparse.ArgumentParser(description='Artificial Potential Function') 138 | parser.add_argument('-g','--grid', type=float, 139 | help='Grid size; Default=0.5', default=0.5) 140 | parser.add_argument('-f','--function', type=str, 141 | help='Attractive Potential function(p=paraboloid, c=conical); Default=c', default='c') 142 | parser.add_argument('-a','--attractive', type=float, 143 | help='Attractive Potential Gain; Default=1', default=1) 144 | parser.add_argument('-r','--repulsive', type=float, 145 | help='Repuslive Potential Gain; Default=5000', default=5000) 146 | args = parser.parse_args() 147 | 148 | 149 | grid_size = args.grid 150 | func = args.function 151 | K = args.attractive 152 | ETA = args.repulsive 153 | 154 | main(grid_size, func, ETA, K) 155 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | altair==4.1.0 2 | argon2-cffi==20.1.0 3 | astor==0.8.1 4 | async-generator==1.10 5 | attrs==21.2.0 6 | backcall==0.2.0 7 | base58==2.1.0 8 | bleach==3.3.0 9 | blinker==1.4 10 | cachetools==4.2.2 11 | certifi==2020.12.5 12 | cffi==1.14.5 13 | chardet==4.0.0 14 | click==7.1.2 15 | cycler==0.10.0 16 | decorator==5.0.7 17 | defusedxml==0.7.1 18 | entrypoints==0.3 19 | gitdb==4.0.7 20 | GitPython==3.1.14 21 | idna==2.10 22 | ipykernel==5.5.4 23 | ipython==7.23.1 24 | ipython-genutils==0.2.0 25 | ipywidgets==7.6.3 26 | jedi==0.18.0 27 | Jinja2==2.11.3 28 | jsonschema==3.2.0 29 | jupyter-client==6.1.12 30 | jupyter-core==4.7.1 31 | jupyterlab-pygments==0.1.2 32 | jupyterlab-widgets==1.0.0 33 | kiwisolver==1.3.1 34 | MarkupSafe==1.1.1 35 | matplotlib==3.4.2 36 | matplotlib-inline==0.1.2 37 | mistune==0.8.4 38 | nbclient==0.5.3 39 | nbconvert==6.0.7 40 | nbformat==5.1.3 41 | nest-asyncio==1.5.1 42 | notebook==6.3.0 43 | numpy==1.20.2 44 | packaging==20.9 45 | pandas==1.2.4 46 | pandocfilters==1.4.3 47 | parso==0.8.2 48 | pexpect==4.8.0 49 | pickleshare==0.7.5 50 | Pillow==8.2.0 51 | prometheus-client==0.10.1 52 | prompt-toolkit==3.0.18 53 | protobuf==3.16.0 54 | ptyprocess==0.7.0 55 | pycparser==2.20 56 | pydeck==0.6.2 57 | Pygments==2.9.0 58 | pyparsing==2.4.7 59 | PyQt5==5.15.4 60 | PyQt5-Qt5==5.15.2 61 | PyQt5-sip==12.8.1 62 | pyrsistent==0.17.3 63 | python-dateutil==2.8.1 64 | pytz==2021.1 65 | pyzmq==22.0.3 66 | requests==2.25.1 67 | Send2Trash==1.5.0 68 | six==1.16.0 69 | smmap==4.0.0 70 | streamlit==0.81.1 71 | terminado==0.9.4 72 | testpath==0.4.4 73 | toml==0.10.2 74 | toolz==0.11.1 75 | tornado==6.1 76 | traitlets==5.0.5 77 | tzlocal==2.1 78 | urllib3==1.26.4 79 | validators==0.18.2 80 | watchdog==2.1.0 81 | wcwidth==0.2.5 82 | webencodings==0.5.1 83 | widgetsnbextension==3.5.1 84 | --------------------------------------------------------------------------------