from PathGraph import * from Queue import PriorityQueue from math import sqrt from collections import deque from random import shuffle from time import sleep from copy import deepcopy # This returns an instantiated version of the pathfinding classes specified. # If a pathfinding algorithm is added here, it will automatically show up in the gui. def allPathfinding(graph): return [ Laplace(graph), RRT(graph), RRT_Connect(graph), Dijkstra1(graph), Dijkstra2(graph), Dijkstra3(graph), AStar2(graph), AStar(graph), PrintAllNode(graph) ] # Returns the euclidean distance between two node id's in the given graph def euclidDist(a,b, graph): sum = 0 for i in range(graph.coords): sum += (graph.getNode(a).coords[i] - graph.getNode(b).coords[i]) ** 2 return float(sqrt(sum)) # The base pathfinding class. Inherit and overwrite FindPath to implement a pathfinding algorithm. class Pathfind: label = "" # The label of the Pathfinding class. Used as the label on the gui # Creates a new instance of the pathfinding class, over the specified PathGraph. # Does not have to be linked to a gui. def __init__(self, graph, gui=None): self.graph = graph self.gui = gui self.path = None self.pathLen = -1 self.drawnObjects = [] # Find a path from the starting node id to the goal node id. # Such a path found is a list of node id's, which form a valid path from startNodeID # to goalNodeID, such that each node represented in the array is adjacent to its # immediate neighbors in the list. List is NOT returned, instead set it to self.path # Default behavior is to fail to find a path. Overwrite this in path algorithms implemented. def FindPath(self, startNodeID, goalNodeID): self.path = None return # Returns a list of tuples of imformation about the path. # The list is of 2-tuples. Entry 1 is the label, entry 2 is the information. # Default behavior is to return the length. Overwrite this is desired in path algorithms implemented. def GetStats(self): if self.path == None: return [("No Path Found", "")] pathStats = [] length = 0 for nodeID, adjNodeID in zip(self.path[:-1], self.path[1:]): adjCost = self.graph.getAdjacencyCost(nodeID, adjNodeID) if adjCost: length += adjCost pathStats.append(("Path Length:", round(length,3))) return pathStats ############################################# # Nothing needs to be overwritten past here # ############################################# # Set which gui this is attached to. def SetGui(self, gui): self.gui = gui # Smooth the path. Currently does not work as intended. def Smooth(self): if self.path: #pathobjects = [] thisnodem2=[] # node-2 thisnodem1=[] # node-1 xm1 = -1 ym1 = -1 xp1 = -1 yp1 = -1 xavg = -1 yavg = -1 for index in range(1,len(self.path)-1,2): # starting at 1, ending at next to last, in steps of 2 nodeID = self.path[index] thisnode = self.graph.getNode(nodeID) (x,y) = thisnode.getCoordinates() prevnode = self.graph.getNode(nodeID-1) (xm1,ym1) = prevnode.getCoordinates() nextnode = self.graph.getNode(nodeID+1) (xp1,yp1) = nextnode.getCoordinates() xavg = 0.5* (xm1 + xp1) yavg = 0.5* (ym1 + yp1) x2 = int(0.5+(x - 2.0*(x-xavg))) y2 = int(0.5+(y - 2.0*(y-yavg))) targetnode = self.graph.nodeDict.get((x2,y2),None) if (targetnode != None): print x, ' -> ', x2, ', ', y, ' -> ',y2 self.path[index] = targetnode else: print "No change: ",x,", ",y #thisnode.setCoordinates((x2,y2)) #print "(x,y)= (", x,",",y,") (x2,y2)= (", x2,",",y2,")", #print "(xm1,ym1)= (", xm1,",",ym1,") (xp1,yp1)= (", xp1,",",yp1,")" #print self.path else: return # If there is an attached gui, draw the list of nodeID's in drawList. color and fill are the colors desired, # height and width control the size of the rectangles drawn. If clear is False, does not remove any previous # results from calling this. # This is used by the "animation" feature from the gui, to animate your planning algorithm. def DrawList(self, drawList, height=0, width=0, color='green', fill=None, clear=True): if self.gui and self.gui.planDraw.get(): if clear: self.ClearDraws() for node in drawList: (x,y) = self.graph.getNode(node).getCoordinates() self.drawnObjects.append(self.gui.map.create_rectangle(x+width,y+height,x-width,y-height,outline=color, fill=fill)) self.gui.tkroot.update_idletasks() # Clear all the graphics from calling DrawList def ClearDraws(self): if self.drawnObjects: for obj in self.drawnObjects: self.gui.map.delete(obj) self.drawnObjects = [] # Return the path found (or the lack of such) def ReturnPath(self): return self.path # Iterative Deepening Depth-First Search. A very slow algorithm, this is not advised # to be run on any large graph. # http://en.wikipedia.org/wiki/Iterative_deepening_depth-first_search class DepthFirst(Pathfind): label = "Depth-First" # Get all the vertices adjacent to the specified node ID def getAdj(self, node): tovisit = [] for n in self.graph.getAdjacencies(node): tovisit.append(n[0]) return tovisit # Do a depth-first search from node to goal, moving at most depth steps. def DepthFirst(self, node, goal, depth): if (depth == 0 and node == goal): return True elif (depth > 0): for nbr in self.getAdj(node): self.path.append(nbr) self.DrawList(self.path, width=1, height=1) #Draw the path found so far. if self.DepthFirst(nbr, goal, depth-1): return True self.path.pop() return False else: return False # Find a shortest path from node id sn to node id en by putting an increasing maximum distance # to do a depth-first search on. def FindPath(self, sn, en): self.path = [sn] for maxDepth in range(int(euclidDist(sn, en, self.graph)+ 0.5)/2, len(self.graph.nodeList)+1): if self.DepthFirst(sn, en, maxDepth): return self.path = None # Keep track of "unvisited" nodes # Node state: [distance, previousNodeId] class Dijkstra1(Pathfind): label = "Dijkstra 1" def FindPath(self, start, goal): # some initialization draw = deque([]) unvisited = [] for i in range(len(self.graph.nodeList)): self.graph.nodeList[i].setState(float("Inf"), 0) self.graph.nodeList[i].setState(-1, 1) #previousNodeId unvisited.append(i) # set the start node's distance to zero self.graph.nodeList[start].state[0] = 0 #del unvisited[0] # main loop while (1): #find the nodes with smallest distance minNode, minNodeId, minDist, idx = self.findSmallestDist(unvisited) # The goal has been reached if minNodeId == goal: break # remove this node from unvisited set del unvisited[idx] #compute the distance for all neighbors for adj in minNode.getAdjacencies() : adj_nid = adj[0] adj_dist = adj[1] adj_node = self.graph.getNode(adj_nid) # Improved distance if (minNode.state[0] + adj_dist) < adj_node.state[0]: adj_node.setState(minDist + adj_dist, 0) #update the distance adj_node.setState(minNodeId, 1) #update previousNodeId # Visualizaion if len(draw) >= 50: draw.popleft() draw.append(minNodeId) #for visualization self.DrawList(draw, width=2, height=2) self.DrawList([minNodeId], clear=False, height=2, width=2, color='red') #draw current node # Form the path self.formPath(start, goal) return # Find the node with smallest distance in the unvisited set def findSmallestDist(self, unvisited): minNodeId = 0 minDist = float("Inf") idx = 0 count = 0 for i in unvisited: if self.graph.nodeList[i].state[0] < minDist: idx = count minDist = self.graph.nodeList[i].state[0] minNodeId = i count += 1 minNode = self.graph.nodeList[minNodeId] return minNode, minNodeId, minDist, idx # Reverse the Nodes and get the path def formPath(self, start, goal): self.path = [] self.path.append(goal) current_nid = goal while current_nid != start: self.path.append(self.graph.nodeList[current_nid].state[1]) current_nid = self.graph.nodeList[current_nid].state[1] print self.path print "path length=", self.graph.nodeList[goal].state[0] def printAllState(self): for i in range(len(self.graph.nodeList)): print self.graph.nodeList[i].state class PrintAllNode(Pathfind): label = "PrintAllNode" def FindPath(self, start, goal): for i in range(len(self.graph.nodeList)): print "Node " + str(self.graph.nodeList[i].nid) + " at " + self.graph.nodeList[i].coordinateString() print "Number of AdjNode: " , len(self.graph.nodeList[i].adjs) ''' for j in range(len(self.graph.nodeList[i].adjs)): print "\tAdjacent to " + str(self.graph.nodeList[i].adjs[j][0]) + " with cost " + str(self.graph.nodeList[i].adjs[j][1]) ''' # Keep track of "toVisit" nodes # Node state: [distance, previousNodeId, visited] class Dijkstra2(Pathfind): label = "Dijkstra 2" def FindPath(self, start, goal): # some initialization draw = deque([]) toVisit = [] for i in range(len(self.graph.nodeList)): self.graph.nodeList[i].setState(float("Inf"), 0) self.graph.nodeList[i].setState(-1, 1) #previousNodeId self.graph.nodeList[i].setState(False, 2) # set the start node's distance to zero self.graph.nodeList[start].state[0] = 0 self.graph.nodeList[start].state[2] = False toVisit.append(start) # main loop while (1): #find the nodes with smallest distance minNode, minNodeId, minDist, idx = self.findSmallestDist(toVisit) # The goal has been reached if minNodeId == goal: break # mark as visited #self.graph.nodeList[minNodeId].setState(True, 2) # remove this node from toVisit set del toVisit[idx] #compute the distance for all neighbors for adj in minNode.getAdjacencies() : adj_nid = adj[0] adj_dist = adj[1] adj_node = self.graph.getNode(adj_nid) if adj_node.state[2] == False: toVisit.append(adj_nid) #want to keep track of this nodes adj_node.setState(True, 2) # Improved distance if (minNode.state[0] + adj_dist) < adj_node.state[0]: adj_node.setState(minDist + adj_dist, 0) #update the distance adj_node.setState(minNodeId, 1) #update previousNodeId # Visualizaion if len(draw) >= 50: draw.popleft() draw.append(minNodeId) #for visualization self.DrawList(draw, width=2, height=2) self.DrawList([minNodeId], clear=False, height=2, width=2, color='red') #draw current node # Form the path self.formPath(start, goal) return # Find the node with smallest distance in the unvisited set def findSmallestDist(self, unvisited): minNodeId = 0 minDist = float("Inf") idx = 0 count = 0 for i in unvisited: if self.graph.nodeList[i].state[0] < minDist: idx = count minDist = self.graph.nodeList[i].state[0] minNodeId = i count += 1 minNode = self.graph.nodeList[minNodeId] return minNode, minNodeId, minDist, idx # Reverse the Nodes and get the path def formPath(self, start, goal): self.path = [] self.path.append(goal) current_nid = goal while current_nid != start: self.path.append(self.graph.nodeList[current_nid].state[1]) current_nid = self.graph.nodeList[current_nid].state[1] print self.path print "path length=", self.graph.nodeList[goal].state[0] # use priority queue instead of list class Dijkstra3(Pathfind): label = "Dijkstra 3" def FindPath(self, start, goal): # Need an array keeping track of the cost to get to a node, # and what node it came from # This means that we can overwrite particular entries, # and do this search iteratively # So, we make a new array of tuples, # containing the previous nid and the cost to get there # List(costTo, prev) # If unself.visited, is None self.visited = [] if self.graph != None: for i in range(self.graph.getNodeCount()): self.visited.append(None) # The toVisit array contains a list of lists, containing the node, # where it came from, and cost. # Priority queue is based on this cost. # Maybe split up cost into to and from. # PriorityQueue toVisit = PriorityQueue() # Start of the algorithm self.visited[start] = (0, None) toVisit.put( (0, start, None) ) draw = deque([]) while not toVisit.empty(): # Start of the main loop nextTuple = toVisit.get() #Get the next node next = nextTuple[1] nextDist = self.visited[next][0] #Total distance travelled to get to here. #Use this instead of the toVisit, as that is estimated dist. to goal # drawing if len(draw) >= 50: draw.popleft() draw.append(next) self.DrawList(draw, width=2, height=2) if next == goal: #Success! lastPathNode = goal # Construct the path list self.pathCost = self.visited[goal][0] # 0 denotes cost to get to this node self.path = [] while self.visited[lastPathNode][1] != None: self.path.append(lastPathNode) lastPathNode = self.visited[lastPathNode][1] # 1 denotes the node we just came from self.path.append(start) self.path.reverse() return #check all neibors for adjacency in self.graph.getAdjacencies(next): # For each neightbor, see if have a path to nextNbr, cost = adjacency[0], adjacency[1] totalTravel = nextDist + cost if self.visited[nextNbr] == None: # The neighbor is unvisited self.visited[nextNbr] = ( totalTravel, next ) # Mark the node as visited toVisit.put( (totalTravel, nextNbr, next) ) # Add node to priority queue elif totalTravel < self.visited[nextNbr][0]: # The neighbor has been visited, but that is shorter # if we've found a shorter route to nextNbr self.visited[nextNbr] = (totalTravel, next) # then replace old route to nextNbr with new toVisit.put( (totalTravel, nextNbr, next) ) #else: Node has been visited, but current path is equal or longer self.path = None return # Keep track of "toVisit" nodes # Node state: [distance, previousNodeId, visited, g+h] class AStar2(Pathfind): label = "AStar 2" def FindPath(self, start, goal): draw = deque([]) toVisit = [] # a list to keep track of potential next nodes # some initialization for i in range(len(self.graph.nodeList)): self.graph.nodeList[i].setState(float("Inf"), 0) #distance self.graph.nodeList[i].setState(-1, 1) #previousNodeId self.graph.nodeList[i].setState(False, 2) #have been added to toVisit or not? self.graph.nodeList[i].setState(float("Inf"), 3) # set the start node's distance to zero self.graph.nodeList[start].state[0] = 0 #distance is zero self.graph.nodeList[start].state[3] = euclidDist(start, goal, self.graph) self.graph.nodeList[start].state[2] = False toVisit.append(start) # main loop while (1): #find the nodes with smallest distance minNode, minNodeId, minDist, idx = self.findSmallestDist(toVisit) # The goal has been reached if minNodeId == goal: break # remove this node from toVisit set del toVisit[idx] #compute the distance for all neighbors for adj in minNode.getAdjacencies() : adj_nid = adj[0] adj_dist = adj[1] adj_node = self.graph.getNode(adj_nid) if adj_node.state[2] == False: toVisit.append(adj_nid) #want to keep track of this nodes adj_node.setState(True, 2) # Improved distance h = euclidDist(adj_nid, goal, self.graph) g = minNode.state[0] + adj_dist if (g+h < adj_node.state[3]): adj_node.setState(g, 0) #update the distance adj_node.setState(minNodeId, 1) #update previousNodeId #adj_node.setState(max(g+h, minNode.state[3]), 3) #update the g+h adj_node.setState(g+h, 3) #update the g+h # Visualizaion if len(draw) >= 50: draw.popleft() draw.append(minNodeId) #for visualization self.DrawList(draw, width=2, height=2) self.DrawList([minNodeId], clear=False, height=2, width=2, color='red') #draw current node # Form the path self.formPath(start, goal) return # Find the node with smallest distance in the unvisited set def findSmallestDist(self, toVisit): minNodeId = 0 minDist = float("Inf") idx = 0 count = 0 for i in toVisit: if self.graph.nodeList[i].state[3] < minDist: idx = count minDist = self.graph.nodeList[i].state[3] minNodeId = i count += 1 minNode = self.graph.nodeList[minNodeId] return minNode, minNodeId, minDist, idx # Reverse the Nodes and get the path def formPath(self, start, goal): print "start id=", start print "goal id=", goal self.path = [] self.path.append(goal) current_nid = goal while current_nid != start: self.path.append(self.graph.nodeList[current_nid].state[1]) print "current id=" ,self.graph.nodeList[current_nid].state[1] current_nid = self.graph.nodeList[current_nid].state[1] print self.path print "path length=", self.graph.nodeList[goal].state[0] # A-Star pathfinding algorithm. # A greedy algorithm that uses an admissable heuristic, so it finds a guaranteed shortest path. # http://en.wikipedia.org/wiki/A-star class AStar(Pathfind): label = "A-Star 3" # Find the shortest path between the node id start and the node id goal, using A-Star. def FindPath(self, start, goal): # Need an array keeping track of the cost to get to a node, # and what node it came from # This means that we can overwrite particular entries, # and do this search iteratively # So, we make a new array of tuples, # containing the previous nid and the cost to get there # List(costTo, prev) # If unself.visited, is None self.visited = [] if self.graph != None: for i in range(self.graph.getNodeCount()): self.visited.append(None) # The toVisit array contains a list of lists, containing the node, # where it came from, and cost. # Priority queue is based on this cost. # Maybe split up cost into to and from. # PriorityQueue toVisit = PriorityQueue() # Start of the algorithm self.visited[start] = (0, None) toVisit.put( (0 + euclidDist(start, goal, self.graph), start, None) ) draw = deque([]) while not toVisit.empty(): # Start of the main loop nextTuple = toVisit.get() #Get the next node next = nextTuple[1] nextDist = self.visited[next][0] #Total distance travelled to get to here. #Use this instead of the toVisit, as that is estimated dist. to goal # The nature of A-Star makes it difficult to determine what the current idea of what the best path is. # Instead of trying to do that, we draw the last 50 nodes explored. # original drawing code if len(draw) >= 50: draw.popleft() draw.append(next) self.DrawList(draw, width=2, height=2) if next == goal: #Success! lastPathNode = goal # Construct the path list self.pathCost = self.visited[goal][0] # 0 denotes cost to get to this node self.path = [] while self.visited[lastPathNode][1] != None: self.path.append(lastPathNode) lastPathNode = self.visited[lastPathNode][1] # 1 denotes the node we just came from self.path.append(start) self.path.reverse() return for adjacency in self.graph.getAdjacencies(next): # For each neightbor, see if have a path to nextNbr, cost = adjacency[0], adjacency[1] totalTravel = nextDist + cost if self.visited[nextNbr] == None: # The neighbor is unvisited self.visited[nextNbr] = ( totalTravel, next ) # Mark the node as visited goalDist = euclidDist(nextNbr, goal, self.graph) # Estimated distance (euclidean) toVisit.put( (totalTravel + goalDist, nextNbr, next) ) # Add node to priority queue elif totalTravel < self.visited[nextNbr][0]: # The neighbor has been visited, but that is shorter # if we've found a shorter route to nextNbr self.visited[nextNbr] = (totalTravel, next) # then replace old route to nextNbr with new goalDist = euclidDist(nextNbr, goal, self.graph) toVisit.put( (totalTravel + goalDist, nextNbr, next) ) #else: Node has been visited, but current path is equal or longer self.path = None return # A pathfinding algorithm that utilizes Rapidly-exploring Random Trees for pathfinding. # Trees are rapidly grown from the start and end nodes, and once they meet, a path is created # Using the branches of the trees created. # NOT guaranteed to be optimal (In fact, most often isn't) # http://en.wikipedia.org/wiki/Rapidly-exploring_random_tree # http://www.kuffner.org/james/plan/algorithm.php class RRT(Pathfind): label = "RRT" # Finds a path between the node id startID and the node id goalID by growing two trees, one rooted # at startID, the other at goalID def FindPath(self, startID, goalID): # Randomize the list of nodes, so we can iterate over it to get random nodes. nodeList = self.graph.getNodes()[:] nodeCount = len(nodeList) shuffle(nodeList) # Store the RRTs as dictionaries, mapping node ids to the node they came from. Separate dictionaries, # as there may be overlap, and each node points in a different direction. # JRS: Hmm, possible optimization? If we just had one dict, then when we looked up the # nearest neb and found it was in the other tree, maybe we could make a connection? # Would probably need to add a "color" variable to the tree...? startPathDictionary = {startID : None} goalPathDictionary = {goalID : None} # We swap the "primary" tree each pass currPathDict = startPathDictionary otherPathDict = goalPathDictionary # Determines which dictionary is which tree, for coloring purposes. Negated every pass. dictOneCurr = True # Get the nearest neighbor in the given dictionary to the nodeID specified. def NearestNbr(dictionary, nodeID): nearest = None dist = -1 for n in dictionary.keys(): d = euclidDist(n,nodeID, self.graph) if d < dist or dist < 0: dist = d nearest = n return nearest # Keep getting random nodes; for each one, grow both trees toward that randomly chosen node for i in range(nodeCount): qRand = nodeList[i] # Create a trajectory between the random node and the nearest neighbor, and traverse it as far as possible. qNbr = NearestNbr(currPathDict, qRand.nid) traj = PathGraph.Trajectory(self.graph, trajectory=[qNbr, qRand.nid]) traj.ConvertToPath() #rndtargnode = self.graph.getNode(qRand.nid)#jrs #print "Random node (end target): ",rndtargnode.getCoordinates(), " ", #jrs #qNbrnode = self.graph.getNode(qNbr)#jrs #print "Nearest nbr on tree (start node of extension): ", qNbrnode.getCoordinates() #jrs #print "Traj: ", #jrs #for anodeID in traj.path: #jrs # anode = self.graph.getNode(anodeID) # jrs # print anode.getCoordinates(), " " #jrs # Draw the new part of the RRT if dictOneCurr: self.DrawList(traj.path, clear=False, color='green') else: self.DrawList(traj.path, clear=False, color='red') # Add the part of the trajectory traversed to the main dictionary for index in range(1, len(traj.path)): nodeID = traj.path[index] currPathDict[nodeID] = traj.path[index-1] # Create a trajectory between the random node and the nearest neightbor in the OTHER dictionary, and traverse it as far as possible. qNbrOther = NearestNbr(otherPathDict, qRand.nid) trajOther = PathGraph.Trajectory(self.graph, trajectory=[qNbrOther, qRand.nid]) trajOther.ConvertToPath() # Draw the new part of the RRT if dictOneCurr: self.DrawList(trajOther.path, clear=False, color='red') else: self.DrawList(trajOther.path, clear=False, color='green') # Add the part of the trajectory traversed to the other dictionary for index in range(1, len(trajOther.path)): nodeID = trajOther.path[index] otherPathDict[nodeID] = trajOther.path[index-1] # If both trajectories reach the goal, then the trees intersect. if traj.completePath and trajOther.completePath: currNodeID = traj.path[-1] self.path = deque([currNodeID]) # Add the path from the start node dictionary while (currNodeID != startID): currNodeID = startPathDictionary[currNodeID] self.path.appendleft(currNodeID) currNodeID = traj.path[-1] # Add the path from the goal node dictionary while (currNodeID != goalID): currNodeID = goalPathDictionary[currNodeID] self.path.append(currNodeID) self.path = list(self.path) return else: # trees have not yet reached same randomly chosen node # Do the swaps! dictSwap = currPathDict currPathDict = otherPathDict otherPathDict = dictSwap dictOneCurr = not dictOneCurr #raw_input('Press the any key to continue! ') #jrs self.path = None class RRT_Connect(Pathfind): label = "RRT-Connect" # Finds a path between the node id startID and the node id goalID by growing two trees, one rooted # at startID, the other at goalID def FindPath(self, startID, goalID): # Randomize the list of nodes, so we can iterate over it to get random nodes. nodeList = self.graph.getNodes()[:] nodeCount = len(nodeList) shuffle(nodeList) # Store the RRTs as dictionaries, mapping node ids to the node they came from. Separate dictionaries, # as there may be overlap, and each node points in a different direction. # JRS: Hmm, possible optimization? If we just had one dict, then when we looked up the # nearest neb and found it was in the other tree, maybe we could make a connection? # Would probably need to add a "color" variable to the tree...? startPathDictionary = {startID : None} goalPathDictionary = {goalID : None} # We swap the "primary" tree each pass currPathDict = startPathDictionary otherPathDict = goalPathDictionary # Determines which dictionary is which tree, for coloring purposes. Negated every pass. dictOneCurr = True # Get the nearest neighbor in the given dictionary to the nodeID specified. def NearestNbr(dictionary, nodeID): nearest = None dist = -1 for n in dictionary.keys(): d = euclidDist(n,nodeID, self.graph) if d < dist or dist < 0: dist = d nearest = n return nearest def Extend(dic, q): qNear = NearestNbr(dic, q) #try the advance a distance d ( move how many times) d = 5 pNode = self.graph.getNode(qNear) #from the nearest node print 'Nearest Node=', qNear visited = [qNear] for i in range(0,d): #advance d times tmp_dist = [] tmp_nid = [] for adj in pNode.getAdjacencies() : #check neighbors and pick the nearest one if not adj[0] in visited: #adj_node = self.graph.getNode(adj[0]) dist = euclidDist(adj[0], q, self.graph) tmp_dist.append(dist) tmp_nid.append(adj[0]) #if no neighbor can be visit in this step, got Trapped if len(tmp_dist) == 0: return "Trapped" print 'i=', i print "min(tmp_dist)=", min(tmp_dist) print "tmp_dist=",tmp_dist print "tmp_nid=", tmp_nid print "visited=", visited minNodeId = tmp_nid[tmp_dist.index(min(tmp_dist))] self.DrawList([qNear, minNodeId], clear=False, color='green') print "minNodeId=", minNodeId, ", Distance=", min(tmp_dist) print "RnadId=", q print " " #if the minNode is the random target, Reached if minNodeId == q: dic[q] = adj[0] return "Reached" pNode = self.graph.getNode(minNodeId) #update the current node visited.append(minNodeId) #keep track of all the visited nodes locally dic[minNodeId] = adj[0] #add to the tree return "Advanced" def Connect(dic, q): while 1: S = Extend(dic, q) if S != "Advanced": return S return "BAD" # Keep getting random nodes; for each one, grow both trees toward that randomly chosen node for i in range(nodeCount): qRand = nodeList[i] #if not Extend(currPathDict, qRand.nid) == "Trapped" : # if Connect(otherPathDict, qRand.nid) == "Reached" : S = Extend(currPathDict, qRand.nid) print "Extend ", S if not S == "Trapped" : P = Connect(otherPathDict, qRand.nid) print "Connect ", P if P == "Reached" : #goal! currNodeID = qRand.nid self.path = deque([currNodeID]) # Add the path from the start node dictionary while (currNodeID != startID): currNodeID = startPathDictionary[currNodeID] self.path.appendleft(currNodeID) currNodeID = qRand.nid # Add the path from the goal node dictionary while (currNodeID != goalID): currNodeID = goalPathDictionary[currNodeID] self.path.append(currNodeID) self.path = list(self.path) return #swap dictSwap = currPathDict currPathDict = otherPathDict otherPathDict = dictSwap dictOneCurr = not dictOneCurr ''' # Draw the new part of the RRT l = [qNear.nid, pNode.nid] if dictOneCurr: self.DrawList(l, clear=False, color='green') else: self.DrawList(l, clear=False, color='red') ''' self.path = None # Laplace Path Planning Algorithm class Laplace(Pathfind): label = "Laplace" #identify boundary and free nodes def FindBoundary(self): self.u = [] self.boundary = [] self.free = [] for i in range(len(self.graph.nodeList)): if i == self.goal: #the goal self.u.append(0.0) elif (len(self.graph.nodeList[i].getAdjacencies()) == 8): #free self.u.append(0.5) self.free.append(i) else: #boundary self.u.append(1.0) self.boundary.append(i) self.DrawList(self.boundary, clear=False, height=0, width=0, color='red') #self.DrawList(self.free, clear=False, height=0, width=0, color='blue') print "size of boundary (red): ", len(self.boundary) print "size of free (blue): ", len(self.free) print "size of all nodes: ", len(self.graph.nodeList) self.u2 = deepcopy(self.u) # after this call, the Laplacian self.u1 will be actually computed twice def update(self): # u1 -> u2 for nid in self.free: sumup = 0 count = 0 for adj in self.graph.nodeList[nid].getAdjacencies(): sumup += self.u[adj[0]] count += 1 self.u2[nid] = sumup/count # u2 -> u1 for nid in self.free: sumup = 0 count = 0 for adj in self.graph.nodeList[nid].getAdjacencies(): sumup += self.u2[adj[0]] count += 1 self.u[nid] = sumup/count self.itr += 2 # main function def FindPath(self, start, goal): self.goal = goal self.itr = 0 self.err = 0 self.MAX_ITER = 2000 self.FindBoundary() for i in range(0, self.MAX_ITER/2): self.update() print 'total iteration = ', self.itr self.GreyMap(self.u) #input('Finish') self.FollowGradient(start, goal) #visualize the Laplacian value as a grey map def GreyMap(self, drawList): a_min = min(drawList) a = max(drawList) - min(drawList) self.DrawList([], clear=True, height=0, width=0, color='red') for idx in range(0, len(drawList)): k = int ((self.u[idx]-a_min) / a * 100) colorName = 'grey' + str(k) self.DrawList([idx], clear=False, height=0, width=0, color=colorName) def FollowGradient(self, start, goal): self.path = [] currentNodeId = start currentNode = self.graph.nodeList[start] currentVal = self.u[start] self.path.append(start) while currentNodeId != goal: for adj in currentNode.getAdjacencies(): if self.u[adj[0]] <= currentVal: minNodeId = adj[0] minVal = self.u[adj[0]] currentNodeId = minNodeId print 'currentNodeId=', minNodeId currentNode = self.graph.nodeList[currentNodeId] currentVal = self.u[currentNodeId] print 'currentVal=', currentVal self.path.append(currentNodeId) self.DrawList([currentNodeId], clear=False, height=1, width=1, color='yellow')