├── requirements.txt ├── environment.txt ├── 1000_samples.png ├── 5000_samples.png ├── classes ├── __init__.py ├── __pycache__ │ ├── Utils.cpython-36.pyc │ ├── Dijkstra.cpython-36.pyc │ ├── Obstacle.cpython-36.pyc │ ├── __init__.cpython-36.pyc │ └── PRMController.cpython-36.pyc ├── Utils.py ├── Obstacle.py ├── Dijkstra.py └── PRMController.py ├── .gitignore ├── main.py └── README.md /requirements.txt: -------------------------------------------------------------------------------- 1 | shapely -------------------------------------------------------------------------------- /environment.txt: -------------------------------------------------------------------------------- 1 | 10,10;80,30 2 | 20,10;20,50 3 | 20,50;90,50 4 | 30,30;40,40 5 | -1 6 | -------------------------------------------------------------------------------- /1000_samples.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/1000_samples.png -------------------------------------------------------------------------------- /5000_samples.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/5000_samples.png -------------------------------------------------------------------------------- /classes/__init__.py: -------------------------------------------------------------------------------- 1 | from .PRMController import * 2 | from .Obstacle import * 3 | from .Utils import * 4 | -------------------------------------------------------------------------------- /classes/__pycache__/Utils.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/classes/__pycache__/Utils.cpython-36.pyc -------------------------------------------------------------------------------- /classes/__pycache__/Dijkstra.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/classes/__pycache__/Dijkstra.cpython-36.pyc -------------------------------------------------------------------------------- /classes/__pycache__/Obstacle.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/classes/__pycache__/Obstacle.cpython-36.pyc -------------------------------------------------------------------------------- /classes/__pycache__/__init__.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/classes/__pycache__/__init__.cpython-36.pyc -------------------------------------------------------------------------------- /classes/__pycache__/PRMController.cpython-36.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/KaleabTessera/PRM-Path-Planning/HEAD/classes/__pycache__/PRMController.cpython-36.pyc -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.py[cod] 3 | *$py.class 4 | .vscode/* 5 | !.vscode/settings.json 6 | !.vscode/tasks.json 7 | !.vscode/launch.json 8 | !.vscode/extensions.json 9 | -------------------------------------------------------------------------------- /classes/Utils.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | from matplotlib.patches import Rectangle 4 | 5 | 6 | class Utils: 7 | def isWall(self, obs): 8 | x = [item[0] for item in obs.allCords] 9 | y = [item[1] for item in obs.allCords] 10 | if(len(np.unique(x)) < 2 or len(np.unique(y)) < 2): 11 | return True # Wall 12 | else: 13 | return False # Rectangle 14 | 15 | def drawMap(self, obs, curr, dest): 16 | fig = plt.figure() 17 | currentAxis = plt.gca() 18 | for ob in obs: 19 | if(self.isWall(ob)): 20 | x = [item[0] for item in ob.allCords] 21 | y = [item[1] for item in ob.allCords] 22 | plt.scatter(x, y, c="red") 23 | plt.plot(x, y) 24 | else: 25 | currentAxis.add_patch(Rectangle( 26 | (ob.bottomLeft[0], ob.bottomLeft[1]), ob.width, ob.height, alpha=0.4)) 27 | 28 | plt.scatter(curr[0], curr[1], s=200, c='green') 29 | plt.scatter(dest[0], dest[1], s=200, c='green') 30 | fig.canvas.draw() 31 | -------------------------------------------------------------------------------- /classes/Obstacle.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class Obstacle: 5 | def __init__(self, topLeft, bottomRight): 6 | self.topLeft = topLeft 7 | self.bottomRight = bottomRight 8 | self.calcFullCord() 9 | 10 | def printFullCords(self): 11 | print(self.topLeft, self.topRight, self.bottomLeft, self.bottomRight) 12 | 13 | def calcFullCord(self): 14 | otherP1 = [self.topLeft[0], self.bottomRight[1]] 15 | otherP2 = [self.bottomRight[0], self.topLeft[1]] 16 | 17 | points = [self.topLeft, otherP1, 18 | otherP2, self.bottomRight] 19 | 20 | # Finding correct coords and what part of rectangle they represent - we can't always assume we receive the top left and bottomRight 21 | x = [item[0] for item in points] 22 | y = [item[1] for item in points] 23 | 24 | minX = np.min(x) 25 | minY = np.min(y) 26 | 27 | maxX = np.max(x) 28 | maxY = np.max(y) 29 | 30 | self.topRight = np.array([maxX, maxY]) 31 | self.bottomLeft = np.array([minX, minY]) 32 | 33 | self.topLeft = np.array([minX, maxY]) 34 | self.bottomRight = np.array([maxX, minY]) 35 | 36 | self.allCords = [self.topLeft, self.topRight, 37 | self.bottomLeft, self.bottomRight] 38 | 39 | self.width = self.topRight[0] - self.topLeft[0] 40 | self.height = self.topRight[1] - self.bottomRight[1] 41 | -------------------------------------------------------------------------------- /main.py: -------------------------------------------------------------------------------- 1 | 2 | import sys 3 | import numpy as np 4 | import argparse 5 | from classes import PRMController, Obstacle, Utils 6 | 7 | 8 | def main(args): 9 | 10 | parser = argparse.ArgumentParser(description='PRM Path Planning Algorithm') 11 | parser.add_argument('--numSamples', type=int, default=1000, metavar='N', 12 | help='Number of sampled points') 13 | args = parser.parse_args() 14 | 15 | numSamples = args.numSamples 16 | 17 | env = open("environment.txt", "r") 18 | l1 = env.readline().split(";") 19 | 20 | current = list(map(int, l1[0].split(","))) 21 | destination = list(map(int, l1[1].split(","))) 22 | 23 | print("Current: {} Destination: {}".format(current, destination)) 24 | 25 | print("****Obstacles****") 26 | allObs = [] 27 | for l in env: 28 | if(";" in l): 29 | line = l.strip().split(";") 30 | topLeft = list(map(int, line[0].split(","))) 31 | bottomRight = list(map(int, line[1].split(","))) 32 | obs = Obstacle(topLeft, bottomRight) 33 | obs.printFullCords() 34 | allObs.append(obs) 35 | 36 | utils = Utils() 37 | utils.drawMap(allObs, current, destination) 38 | 39 | prm = PRMController(numSamples, allObs, current, destination) 40 | # Initial random seed to try 41 | initialRandomSeed = 0 42 | prm.runPRM(initialRandomSeed) 43 | 44 | 45 | if __name__ == '__main__': 46 | main(sys.argv) 47 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PRM-Path-Planning 2 | Probabilistic Roadmaps is a path planning algorithm used in Robotics. 3 | 4 | ## How to run 5 | ``` 6 | pip install -r requirements.txt 7 | python main.py 8 | ``` 9 | 10 | You can also specify number of samples: 11 | ``` 12 | python main.py --numSamples [number] 13 | ``` 14 | 15 | ## Example Path 16 | ### PRM with 1000 sample points. 17 | ![PRM_1000](1000_samples.png) 18 | 19 | ### PRM with 5000 sample points. 20 | ![PRM_5000](5000_samples.png) 21 | 22 | ## Steps of algorithm 23 | 1. Generate n random samples called milestones. 24 | 2. Check if milestones are collision free. 25 | 3. Find k valid neighbours/paths: 26 | - Link each milestone to k nearest neighbours. 27 | - Retain collision free links as local paths. 28 | 4. Search for shortest path from start to end node using an algoruthm. In this case we are using Dijksta's shortest path algorithm. 29 | 30 | ## PRM vs RRT (Randomly Expanding Trees) 31 | PRM was choosen since it is probabilistically complete and in a small map like the one given in this problem, sampling is time efficient. 32 | 33 | ## Problem 34 | "You are given as input the current and target coordinates of a robot, as well as the top left and bottom right points of rectangular obstacles." 35 | 36 | ## How to Specify Obstacles - Use environment.txt 37 | ``` 38 | starting point;target point 39 | list of obstacle coordinates 40 | -1 41 | ``` 42 | Example: 43 | ``` 44 | 10,10;80,30 45 | 20,10;20,50 46 | 20,50;90,50 47 | 30,30;40,40 48 | -1 49 | ``` 50 | -------------------------------------------------------------------------------- /classes/Dijkstra.py: -------------------------------------------------------------------------------- 1 | # Adapted from https://gist.github.com/betandr/541a1f6466b6855471de5ca30b74cb31 2 | from decimal import Decimal 3 | 4 | 5 | class Edge: 6 | def __init__(self, to_node, length): 7 | self.to_node = to_node 8 | self.length = length 9 | 10 | 11 | class Graph: 12 | def __init__(self): 13 | self.nodes = set() 14 | self.edges = dict() 15 | 16 | def add_node(self, node): 17 | self.nodes.add(node) 18 | 19 | def add_edge(self, from_node, to_node, length): 20 | edge = Edge(to_node, length) 21 | if from_node in self.edges: 22 | from_node_edges = self.edges[from_node] 23 | else: 24 | self.edges[from_node] = dict() 25 | from_node_edges = self.edges[from_node] 26 | from_node_edges[to_node] = edge 27 | 28 | 29 | def min_dist(q, dist): 30 | """ 31 | Returns the node with the smallest distance in q. 32 | Implemented to keep the main algorithm clean. 33 | """ 34 | min_node = None 35 | for node in q: 36 | if min_node == None: 37 | min_node = node 38 | elif dist[node] < dist[min_node]: 39 | min_node = node 40 | 41 | return min_node 42 | 43 | 44 | INFINITY = float('Infinity') 45 | 46 | 47 | def dijkstra(graph, source): 48 | q = set() 49 | dist = {} 50 | prev = {} 51 | 52 | for v in graph.nodes: # initialization 53 | dist[v] = INFINITY # unknown distance from source to v 54 | prev[v] = INFINITY # previous node in optimal path from source 55 | q.add(v) # all nodes initially in q (unvisited nodes) 56 | 57 | # distance from source to source 58 | dist[source] = 0 59 | 60 | while q: 61 | # node with the least distance selected first 62 | u = min_dist(q, dist) 63 | 64 | q.remove(u) 65 | 66 | try: 67 | if u in graph.edges: 68 | for _, v in graph.edges[u].items(): 69 | alt = dist[u] + v.length 70 | if alt < dist[v.to_node]: 71 | # a shorter path to v has been found 72 | dist[v.to_node] = alt 73 | prev[v.to_node] = u 74 | except: 75 | pass 76 | 77 | return dist, prev 78 | 79 | 80 | def to_array(prev, from_node): 81 | """Creates an ordered list of labels as a route.""" 82 | previous_node = prev[from_node] 83 | route = [from_node] 84 | 85 | while previous_node != INFINITY: 86 | route.append(previous_node) 87 | temp = previous_node 88 | previous_node = prev[temp] 89 | 90 | route.reverse() 91 | return route 92 | -------------------------------------------------------------------------------- /classes/PRMController.py: -------------------------------------------------------------------------------- 1 | from collections import defaultdict 2 | import sys 3 | import math 4 | import matplotlib.pyplot as plt 5 | import matplotlib.patches as patches 6 | from matplotlib.patches import Rectangle 7 | import numpy as np 8 | from sklearn.neighbors import NearestNeighbors 9 | import shapely.geometry 10 | import argparse 11 | 12 | from .Dijkstra import Graph, dijkstra, to_array 13 | from .Utils import Utils 14 | 15 | 16 | class PRMController: 17 | def __init__(self, numOfRandomCoordinates, allObs, current, destination): 18 | self.numOfCoords = numOfRandomCoordinates 19 | self.coordsList = np.array([]) 20 | self.allObs = allObs 21 | self.current = np.array(current) 22 | self.destination = np.array(destination) 23 | self.graph = Graph() 24 | self.utils = Utils() 25 | self.solutionFound = False 26 | 27 | def runPRM(self, initialRandomSeed, saveImage=True): 28 | seed = initialRandomSeed 29 | # Keep resampling if no solution found 30 | while(not self.solutionFound): 31 | print("Trying with random seed {}".format(seed)) 32 | np.random.seed(seed) 33 | 34 | # Generate n random samples called milestones 35 | self.genCoords() 36 | 37 | # Check if milestones are collision free 38 | self.checkIfCollisonFree() 39 | 40 | # Link each milestone to k nearest neighbours. 41 | # Retain collision free links as local paths. 42 | self.findNearestNeighbour() 43 | 44 | # Search for shortest path from start to end node - Using Dijksta's shortest path alg 45 | self.shortestPath() 46 | 47 | seed = np.random.randint(1, 100000) 48 | self.coordsList = np.array([]) 49 | self.graph = Graph() 50 | 51 | if(saveImage): 52 | plt.savefig("{}_samples.png".format(self.numOfCoords)) 53 | plt.show() 54 | 55 | def genCoords(self, maxSizeOfMap=100): 56 | self.coordsList = np.random.randint( 57 | maxSizeOfMap, size=(self.numOfCoords, 2)) 58 | # Adding begin and end points 59 | self.current = self.current.reshape(1, 2) 60 | self.destination = self.destination.reshape(1, 2) 61 | self.coordsList = np.concatenate( 62 | (self.coordsList, self.current, self.destination), axis=0) 63 | 64 | def checkIfCollisonFree(self): 65 | collision = False 66 | self.collisionFreePoints = np.array([]) 67 | for point in self.coordsList: 68 | collision = self.checkPointCollision(point) 69 | if(not collision): 70 | if(self.collisionFreePoints.size == 0): 71 | self.collisionFreePoints = point 72 | else: 73 | self.collisionFreePoints = np.vstack( 74 | [self.collisionFreePoints, point]) 75 | self.plotPoints(self.collisionFreePoints) 76 | 77 | def findNearestNeighbour(self, k=5): 78 | X = self.collisionFreePoints 79 | knn = NearestNeighbors(n_neighbors=k) 80 | knn.fit(X) 81 | distances, indices = knn.kneighbors(X) 82 | self.collisionFreePaths = np.empty((1, 2), int) 83 | 84 | for i, p in enumerate(X): 85 | # Ignoring nearest neighbour - nearest neighbour is the point itself 86 | for j, neighbour in enumerate(X[indices[i][1:]]): 87 | start_line = p 88 | end_line = neighbour 89 | if(not self.checkPointCollision(start_line) and not self.checkPointCollision(end_line)): 90 | if(not self.checkLineCollision(start_line, end_line)): 91 | self.collisionFreePaths = np.concatenate( 92 | (self.collisionFreePaths, p.reshape(1, 2), neighbour.reshape(1, 2)), axis=0) 93 | 94 | a = str(self.findNodeIndex(p)) 95 | b = str(self.findNodeIndex(neighbour)) 96 | self.graph.add_node(a) 97 | self.graph.add_edge(a, b, distances[i, j+1]) 98 | x = [p[0], neighbour[0]] 99 | y = [p[1], neighbour[1]] 100 | plt.plot(x, y) 101 | 102 | def shortestPath(self): 103 | self.startNode = str(self.findNodeIndex(self.current)) 104 | self.endNode = str(self.findNodeIndex(self.destination)) 105 | 106 | dist, prev = dijkstra(self.graph, self.startNode) 107 | 108 | pathToEnd = to_array(prev, self.endNode) 109 | 110 | if(len(pathToEnd) > 1): 111 | self.solutionFound = True 112 | else: 113 | return 114 | 115 | # Plotting shorest path 116 | pointsToDisplay = [(self.findPointsFromNode(path)) 117 | for path in pathToEnd] 118 | 119 | x = [int(item[0]) for item in pointsToDisplay] 120 | y = [int(item[1]) for item in pointsToDisplay] 121 | plt.plot(x, y, c="blue", linewidth=3.5) 122 | 123 | pointsToEnd = [str(self.findPointsFromNode(path)) 124 | for path in pathToEnd] 125 | print("****Output****") 126 | 127 | print("The quickest path from {} to {} is: \n {} \n with a distance of {}".format( 128 | self.collisionFreePoints[int(self.startNode)], 129 | self.collisionFreePoints[int(self.endNode)], 130 | " \n ".join(pointsToEnd), 131 | str(dist[self.endNode]) 132 | ) 133 | ) 134 | 135 | def checkLineCollision(self, start_line, end_line): 136 | collision = False 137 | line = shapely.geometry.LineString([start_line, end_line]) 138 | for obs in self.allObs: 139 | if(self.utils.isWall(obs)): 140 | uniqueCords = np.unique(obs.allCords, axis=0) 141 | wall = shapely.geometry.LineString( 142 | uniqueCords) 143 | if(line.intersection(wall)): 144 | collision = True 145 | else: 146 | obstacleShape = shapely.geometry.Polygon( 147 | obs.allCords) 148 | collision = line.intersects(obstacleShape) 149 | if(collision): 150 | return True 151 | return False 152 | 153 | def findNodeIndex(self, p): 154 | return np.where((self.collisionFreePoints == p).all(axis=1))[0][0] 155 | 156 | def findPointsFromNode(self, n): 157 | return self.collisionFreePoints[int(n)] 158 | 159 | def plotPoints(self, points): 160 | x = [item[0] for item in points] 161 | y = [item[1] for item in points] 162 | plt.scatter(x, y, c="black", s=1) 163 | 164 | def checkCollision(self, obs, point): 165 | p_x = point[0] 166 | p_y = point[1] 167 | if(obs.bottomLeft[0] <= p_x <= obs.bottomRight[0] and obs.bottomLeft[1] <= p_y <= obs.topLeft[1]): 168 | return True 169 | else: 170 | return False 171 | 172 | def checkPointCollision(self, point): 173 | for obs in self.allObs: 174 | collision = self.checkCollision(obs, point) 175 | if(collision): 176 | return True 177 | return False 178 | --------------------------------------------------------------------------------