from PathGraph import * from Queue import PriorityQueue from math import sqrt from collections import deque from random import shuffle from time import sleep import random # 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[ RRT(graph), AStar(graph), Dijkstra(graph), DepthFirst(graph), Astar2(graph), RRTconnect(graph), Laplace(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)) # clears the state variable for all nodes in graph def clearStates(graph): for i in range(len(graph.nodeList)): graph.nodeList[i].state = [] # 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 # 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" # 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 Dijkstra(Pathfind): label = "Dijkstra" # Finds a path between the node id startID and the node id goalID def FindPath(self, startID, goalID): self.dijsktrasMethod(startID, goalID) if self.graph.getNode(goalID).getState(1) == 'visited': self.planPath(startID, goalID) return #finds a path from start to goal using the path recorded as previous nodes in state[2] def planPath(self, startID, goalID): self.path = [] found = 0 thisNodeID = goalID nextNodeID = float("inf") while found == 0: self.path.append(thisNodeID) nextNodeID = self.graph.getNode(thisNodeID).getState(2) if thisNodeID == startID: found = 1 thisNodeID = nextNodeID self.path.reverse() #print self.path return #empty heuristic function included for A* inheritance def heuristic(self, nodeID, goalID): return (0 * nodeID) def dijsktrasMethod(self, startID, goalID): unvisited = [] nodeID = float("inf") currentNodeID = startID currentNode = float("inf") nextNodeID = float("inf") done = 0 dist = float("inf") thisAdj = [] minDist = float("inf") heuristic = 0; tentDist = float("inf") draw = deque([]) #fill all nodes as unvisited, mark distance as infinity and add a heuristic value (0 for dijkstra's method) #each node has state = (dist, visited?, prev node in shortest path, in unvisited[]?, heuristic value, tentative distance) for i in range(len(self.graph.nodeList)): nodeID = self.graph.nodeList[i].nid if nodeID != startID: #unvisited.append(nodeID) self.graph.getNode(nodeID).setState(float("inf"), 0) #distance self.graph.getNode(nodeID).setState(float("inf"), 2) #previous else: #start node self.graph.getNode(nodeID).setState(0, 0) self.graph.getNode(nodeID).setState('unvisited', 1)#visited? self.graph.getNode(nodeID).setState('no', 3)#in unvisited[]? self.graph.getNode(nodeID).setState(self.heuristic(nodeID, goalID), 4)#heuristic value self.graph.getNode(nodeID).setState(float("inf"), 5)#tent dist while done != 1: #start search algorithm - loop until done is flagged currentNode = self.graph.getNode(currentNodeID) #grab current node for i in range(len(currentNode.adjs)): #cycle through adjacents thisAdj = self.graph.getNode(currentNodeID).adjs[i] if self.graph.getNode(thisAdj[0]).getState(1) == 'unvisited': #look for smallest unvisited distance dist = thisAdj[1] + currentNode.getState(0) heuristic = currentNode.getState(4) tentDist = dist + heuristic #if the distance < current state, reassign distances and path nodes if dist < self.graph.getNode(thisAdj[0]).getState(0): self.graph.getNode(thisAdj[0]).setState(tentDist, 5)#tentative distance self.graph.getNode(thisAdj[0]).setState(currentNodeID, 2) #previous node self.graph.getNode(thisAdj[0]).setState(dist, 0)#true distance if self.graph.getNode(thisAdj[0]).getState(3) == 'no':#check if node is in unvisited[] unvisited.append(thisAdj[0])#place adj node into unvisited[] self.graph.getNode(thisAdj[0]).setState('yes', 3) self.graph.getNode(currentNodeID).setState('visited', 1) if currentNodeID != startID: unvisited.remove(currentNodeID) #remove the current node from unvisited array if self.graph.getNode(goalID).getState(1) == 'visited': done = 1 print "Found the goal!!!" for i in range(len(unvisited)): #look for smallest (closest) tentative distance if self.graph.getNode(unvisited[i]).getState(5) < minDist: minDist = self.graph.getNode(unvisited[i]).getState(5)#dist + heuristic nextNodeID = unvisited[i] if len(unvisited) == 0 and done != 1: done = 1 print "couldn't find it" #draw stuff # The nature of Dijkstra/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(currentNodeID) self.DrawList(draw, width=2, height=2) #reset stuff currentNodeID = nextNodeID nextNodeID = float("inf") minDist = float("inf") #loop #this class inherits a specially designed dijkstra class #to rewrite as little code as possible - the heuristic #function is overwritten to reflect the straight-line #distance between the node and the goal node class Astar2(Dijkstra): label = "A*" def heuristic(self, nodeID, goalID): myFrisbee = euclidDist(nodeID, goalID, self.graph) return myFrisbee class RRTconnect(Pathfind): label = "RRT Connect" # node state locations # WHICH = 0 # PREV = 1 # PREV2 = 2 #given the goal nodeID, returns node in RRT list of nodeIDs that is the closest euclidean neighbor def nearestNbr(self, nodeID, RRT): dist = float("inf") for i in range(len(RRT)): thisDist = euclidDist(nodeID, RRT[i], self.graph) if thisDist < dist: nearest = RRT[i] dist = thisDist return nearest def appendToRRT(self, nodeID, prevN, RRT, RRTName): RRT.append(nodeID) # is the node in an RRT yet? if self.graph.getNode(nodeID).getState(0) == None: self.graph.getNode(nodeID).setState(RRTName, 0) self.graph.getNode(nodeID).setState(prevN, 1) return elif self.graph.getNode(nodeID).getState(0) == RRTName: return ['crossing path'] #this will only happen when nearest neighbor doesn't work correctly else: # the node is in the other tree self.graph.getNode(nodeID).setState(prevN, 2) return['found'] return #this function builds the RRT and returns the mid point of the path def buildRRT(self, startID, goalID): self.path= [] startRRT = [startID] goalRRT = [goalID] currentRRT = startRRT currentRRTName = 'start' otherRRT = goalRRT otherRRTName = 'goal' qNew = goalID buff = [] buffName = '' found = 0 nodeList = [] for i in range(len(self.graph.nodeList)): nodeList.append(self.graph.nodeList[i].nid) random.shuffle(nodeList) for i in range(len(nodeList)): #grow in random direction qTarget = nodeList[i] qNbr = self.nearestNbr(qTarget, currentRRT) prevNode = self.graph.getNode(qNbr).getState(1) newTraj = self.graph.Trajectory(self.graph, [qNbr, qTarget]) newTraj.ConvertToPath() print(newTraj.path) #update RRT with path for j in range(len(newTraj.path)): self.appendToRRT(newTraj.path[j], prevNode, currentRRT, currentRRTName) if self.graph.getNode(newTraj.path[j]).getState(2) != None: return newTraj.path[j] prevNode = newTraj.path[j] #end for #connect with otherRRT qTarget = newTraj.path.pop() # takes last node from latest traj path newTraj = None qNbr = self.nearestNbr(qTarget, otherRRT) prevNode = self.graph.getNode(qNbr).getState(1) newTraj = self.graph.Trajectory(self.graph, [qNbr, qTarget]) newTraj.ConvertToPath() # update draw #update other RRT with path for j in range(len(newTraj.path)): self.appendToRRT(newTraj.path[j], prevNode, otherRRT, otherRRTName) if self.graph.getNode(newTraj.path[j]).getState(2) != None: return newTraj.path[j] prevNode = newTraj.path[j] self.DrawList(startRRT, clear=False, color='red') self.DrawList(goalRRT, clear=False, color='green') #swap RRTs and reset vars newTraj = None buff = otherRRT buffName = otherRRTName otherRRT = currentRRT otherRRTName = currentRRTName currentRRT = buff currentRRTName = buffName #random.shuffle(nodeList) #loop return float("inf") # given the midpoint, def buildPath(self, midpoint, startID, goalID): firstHalf = [] secondHalf = [] i = 0 done = 0 done2 = 0 if self.graph.getNode(midpoint).getState(0) == 'start': prevNode = self.graph.getNode(midpoint).getState(1) prevNode2 = self.graph.getNode(midpoint).getState(2) else: prevNode = self.graph.getNode(midpoint).getState(2) prevNode2 = self.graph.getNode(midpoint).getState(1) firstHalf.append(midpoint) while done == 0: firstHalf.append(prevNode) prevNode = self.graph.getNode(prevNode).getState(1) if prevNode == startID: firstHalf.append(startID) done = 1 while done2 == 0: secondHalf.append(prevNode2) prevNode2 = self.graph.getNode(prevNode2).getState(1) if prevNode2 == goalID: secondHalf.append(goalID) done2 = 1 firstHalf.reverse() firstHalf.append(secondHalf) self.path = firstHalf return def FindPath(self, startID, goalID): midpoint = self.buildRRT(startID, goalID) print(midpoint) self.buildPath(midpoint, startID, goalID) # self.path = [] # traj = self.graph.Trajectory(self.graph, [startID, goalID]) # traj.ConvertToPath() # print(str(traj.path)) # print(str(traj.completePath)) # self.path = traj.path # return class Laplace(Pathfind): label = "Laplace" def laplace(self, goalID): for z in range(1000): for i in range(len(self.graph.nodeList)): total = 0 node = self.graph.nodeList[i] adjs = node.adjs if node.nid == goalID: if z == 0: node.setState(0.00, 0) elif len(adjs) != 8: if z ==0: node.setState(10000.00, 0) else: if z == 0: node.setState(5000.00,0) else: for j in range(8): adj = adjs[j] total = total + self.graph.getNode(adj[0]).getState(0) node.setState(total/8, 0) print("finished laplacing") vals =[] #for i in range(len(self.graph.nodeList)): # vals.append(self.graph.nodeList[i].getState(0)) #print(vals) def tracePath(self, startID, goalID): found = 0 currentNode = startID self.path = [] last = float("inf") while found != 1: self.path.append(currentNode) if currentNode == goalID: found = 1 print("found!") else: adjs = self.graph.getNode(currentNode).adjs min = float("inf") for i in range(len(adjs)): adj = adjs[i] if self.graph.getNode(adj[0]).getState(0) < min: min = self.graph.getNode(adj[0]).getState(0) currentNode = adj[0] if last == min: print("stuck") return last = min print(min) def FindPath(self, startID, goalID): clearStates(self.graph) #solve eqn self.laplace(goalID) #follow path self.tracePath(startID, goalID)