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 #import pylab as lab # 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 [ AStar(graph), Dijkstra(graph), MyAStar(graph), RRT(graph), MyRRT(graph), MyLaPlace(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 class Dijkstra2(Pathfind): label = "Dijkstra" # Pathfind using Dijkstra def FindPath(self, start, goal): # CAREFUL OF TABS vs. SPACES!!! # Example code for printing the graph... # illustrates how to access nodes, coordinates, etc #for i in range(self.graph.getNodeCount()): #print i #print i,"Node " + str(self.graph.nodeList[i].nid) + " at " + self.graph.nodeList[i].coordinateString() #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]) # Initialize our list of paths paths = [[start]] * len(self.graph.nodeList) # Everything starts off not visited notvisited = self.graph.nodeList[:] # Initialize distance list verybiglength = 999999. D = [verybiglength] * len(self.graph.nodeList) D[start] = 0.0 # Initialize drawing list as a normal list draw = [] #deque([]) # Initialize lists with neighboring nodes for j in range(len(self.graph.nodeList[start].adjs)): D[self.graph.nodeList[start].adjs[j][0]] = self.graph.nodeList[start].adjs[j][1] paths[self.graph.nodeList[start].adjs[j][0]] = [start, self.graph.nodeList[start].adjs[j][0]] draw.append(self.graph.nodeList[start].adjs[j][0]) # Initialize list of remaining distances Dnot = D[:] # Knock off elements for the start del Dnot[start] del notvisited[start] # I want to be able to have a list of remaining nids # ---> Couldn't figure out how to cull from self.graph.nodeList[].nid map_remaining = [] for j in range(len(notvisited)): map_remaining.append(notvisited[j].nid) # Loop until I run out of nodes (I'll break out when I get to the goal) while len(notvisited)>0: # Index tracking; located smallest remaining distance klocal = Dnot.index(min(Dnot)) kglobal = notvisited[klocal].nid # Remove the kth node from all the lists I'm using del notvisited[klocal] del Dnot[klocal] del map_remaining[klocal] del draw[draw.index(kglobal)] # Loop through the neighbors of k, NOT all of the remaining nodes for nbr in range(len(self.graph.nodeList[kglobal].adjs)): # Make sure the neighbor is in the list of the remaining nodes if self.graph.nodeList[kglobal].adjs[nbr][0] in map_remaining: # Indexing/bookkeeping... jglobal = self.graph.nodeList[kglobal].adjs[nbr][0] knode = self.graph.nodeList[kglobal] # Check to see if the neighbor has a shorter distance to the start through the kth node if D[jglobal] > D[kglobal] + knode.adjs[nbr][1]: # If so, update its best distance and path D[jglobal] = D[kglobal] + knode.adjs[nbr][1] Dnot[map_remaining.index(jglobal)] = D[jglobal] paths[jglobal] = paths[kglobal]+[jglobal] # We will draw every node that has had a "reasonable" distance associated with it if not (jglobal in draw): draw.append(jglobal) # Update image/drawing self.DrawList(draw, width=2, height=2) # If we've reached the goal (with kth node), go ahead and stop if kglobal == goal: break # Print out our "winning" path print paths[goal] # Tell the main program about the winning path self.pathCost = D[goal] # 0 denotes cost to get to this node self.path = paths[goal] return def hstraightline(nodes,goal): h = [0] * len(nodes) for i in range(len(nodes)): h[i] = ((nodes[i].coords[0]-nodes[goal].coords[0])**2 + (nodes[i].coords[1]-nodes[goal].coords[1])**2)**0.5 #print "we're in hfunc" return h def hzeros(nodes,goal): h = [0] * len(nodes) return h def Afunc(g,h): #print "h: ",h #print len(g),len(h) f = [i+j for i,j in zip(g,h)] #print "F: ",len(f),min(f) i = f.index(min(f)) #print "i: ",i return i def CombinedMethods(self, start, goal, hfunc): # Initialize our list of paths paths = [[start]] * len(self.graph.nodeList) # Everything starts off not visited notvisited = self.graph.nodeList[:] # Initialize distance list verybiglength = 999999. D = [verybiglength] * len(self.graph.nodeList) D[start] = 0.0 # Initialize drawing list as a normal list draw = [] #deque([]) # Initialize lists with neighboring nodes for j in range(len(self.graph.nodeList[start].adjs)): D[self.graph.nodeList[start].adjs[j][0]] = self.graph.nodeList[start].adjs[j][1] paths[self.graph.nodeList[start].adjs[j][0]] = [start, self.graph.nodeList[start].adjs[j][0]] draw.append(self.graph.nodeList[start].adjs[j][0]) # Initialize list of remaining distances Dnot = D[:] # Knock off elements for the start del Dnot[start] del notvisited[start] helems = hfunc(notvisited,goal) #helems = [0] * len(notvisited) # I want to be able to have a list of remaining nids # ---> Couldn't figure out how to cull from self.graph.nodeList[].nid map_remaining = [] for j in range(len(notvisited)): map_remaining.append(notvisited[j].nid) # Loop until I run out of nodes (I'll break out when I get to the goal) while len(notvisited)>0: klocal = Afunc(Dnot,helems) #print "klocal: ",klocal,len(notvisited) kglobal = notvisited[klocal].nid # Remove the kth node from all the lists I'm using del notvisited[klocal] del Dnot[klocal] del helems[klocal] del map_remaining[klocal] del draw[draw.index(kglobal)] # Loop through the neighbors of k, NOT all of the remaining nodes for nbr in range(len(self.graph.nodeList[kglobal].adjs)): # Make sure the neighbor is in the list of the remaining nodes if self.graph.nodeList[kglobal].adjs[nbr][0] in map_remaining: # Indexing/bookkeeping... jglobal = self.graph.nodeList[kglobal].adjs[nbr][0] knode = self.graph.nodeList[kglobal] # Check to see if the neighbor has a shorter distance to the start through the kth node if D[jglobal] > D[kglobal] + knode.adjs[nbr][1]: # If so, update its best distance and path D[jglobal] = D[kglobal] + knode.adjs[nbr][1] Dnot[map_remaining.index(jglobal)] = D[jglobal] paths[jglobal] = paths[kglobal]+[jglobal] # We will draw every node that has had a "reasonable" distance associated with it if not (jglobal in draw): draw.append(jglobal) # Update image/drawing self.DrawList(draw, width=2, height=2) # If we've reached the goal (with kth node), go ahead and stop if kglobal == goal: break # Print out our "winning" path print paths[goal] # Tell the main program about the winning path self.pathCost = D[goal] # 0 denotes cost to get to this node self.path = paths[goal] return [self,start,goal] class Dijkstra(Pathfind): label = "Dijkstra" def FindPath(self, start, goal): [self, start, goal] = CombinedMethods(self,start,goal,hzeros) return # Different A-Star algorithm, produced by Ryan Keedy class MyAStar(Pathfind): label = "My A-Star" def FindPath(self, start, goal): [self, start, goal] = CombinedMethods(self,start,goal,hstraightline) return # 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() print self.path 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 def findNearest(tree,q,graph): qclose = tree[0] #print 'debug',tree[0], q mindist = euclidDist(qclose,q,graph) for node in tree: if euclidDist(node,q,graph) < mindist: mindist = euclidDist(node,q,graph) qclose = node print 'near:', qclose return qclose def greedyQ(tree,destQ,graph): #,othertree): treeq = findNearest(tree,destQ,graph) currentQ = treeq branch = [treeq] while 1 == 1: blocked = True dist = euclidDist(currentQ,destQ,graph) #for n in range(len(graph.nodeList[currentQ].adjs)): #print "naybs: ",graph.nodeList[currentQ].getAdjacencies() for n in graph.nodeList[currentQ].getAdjacencies(): #if euclidDist(graph.nodeList[currentQ].adjs[n][0],destQ,graph) < dist: if euclidDist(n[0],destQ,graph) < dist: blocked = False dist = euclidDist(n[0],destQ,graph) next = n[0] #graph.nodeList[currentQ].adjs[n][0] d = n[1] #getAdjacencyCost(currentQ,n) #graph.nodeList[currentQ].adjs[n][1] if not blocked: graph.nodeList[next].state = [currentQ,d] branch.append(next) currentQ = next if currentQ == destQ: return (branch,True) #if currentQ in othertree: # return (branch,True) else: return (branch,False) def greedyQStep(currentQ,destQ,graph): #,othertree): blocked = True dist = euclidDist(currentQ,destQ,graph) for n in graph.nodeList[currentQ].getAdjacencies(): if euclidDist(n[0],destQ,graph) < dist: blocked = False dist = euclidDist(n[0],destQ,graph) next = n[0] #graph.nodeList[currentQ].adjs[n][0] d = n[1] #getAdjacencyCost(currentQ,n) #graph.nodeList[currentQ].adjs[n][1] if not blocked: if len(graph.nodeList[next].state) == 0: graph.nodeList[next].state = [currentQ,d] else: graph.nodeList[next].state[2:4] = [currentQ,d] #branch.append(next) #currentQ = next return (next,False) else: return (0,True) def makeRRTPath(treei,qi,treef,qf,graph): pathi = [qi] pathf = [qf] dist = 0.0 node = qi #graph.nodeList[q].nid while node != treei[0]: print node pathi.append(graph.nodeList[node].state[0]) dist += graph.nodeList[node].state[1] node = graph.nodeList[node].state[0] print 'i done' node = qf #graph.nodeList[q].nid while node != treef[0]: print node pathf.append(graph.nodeList[node].state[0]) dist += graph.nodeList[node].state[1] node = graph.nodeList[node].state[0] print 'f done' dist += euclidDist(qf,qi,graph) pathi.reverse() path = pathi + pathf return path, dist class MyRRT(Pathfind): label = "My RRT" def FindPath(self, start, goal): # CAREFUL OF TABS vs. SPACES!!! # Example code fosr printing the graph... # illustrates how to access nodes, coordinates, etc #for i in range(self.graph.getNodeCount()): #print i #print i,"Node " + str(self.graph.nodeList[i].nid) + " at " + self.graph.nodeList[i].coordinateString() #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]) for n in self.graph.nodeList: n.state = [] treei = [start] treef = [goal] notvisited = [] # Everything but start and finish starts off not visited for n in range(len(self.graph.nodeList)): notvisited.append(self.graph.nodeList[n].nid) #notvisited = self.graph.getNodes()[:] #nodeList[:] del notvisited[start] del notvisited[goal] while len(notvisited)>0: qrand = random.choice(notvisited) #(branchi,successi) = greedyQ(treei,qrand,self.graph) #,treef) #(branchf,successf) = greedyQ(treef,qrand,self.graph) #,treei) bi = findNearest(treei,qrand,self.graph) bf = findNearest(treef,qrand,self.graph) branchi = [bi] branchf = [bf] #currentQ = treeq #branch = [treeq] stucki = False stuckf = False connected = False bothstuck = False while not bothstuck: if not stucki: (branch,stucki) = greedyQStep(branchi[-1],qrand,self.graph) if not stucki: branchi.append(branch) if branch in treef: connected = True lasti = self.graph.nodeList[branch].state[2] lastf = branch break treei.append(branch) notvisited.remove(branch) if not stuckf: (branch,stuckf) = greedyQStep(branchf[-1],qrand,self.graph) if not stuckf: branchf.append(branch) if branch in treei: connected = True lastf = self.graph.nodeList[branch].state[2] lasti = branch break treef.append(branch) notvisited.remove(branch) bothstuck = stucki and stuckf print stucki, stuckf, bothstuck print 'connected',connected # Update image/drawing self.DrawList(branchi, clear=False, color='green') self.DrawList(branchf, clear=False, color='blue') self.DrawList([qrand], clear=False, height=2, width=2, color='red') if connected: break #treei = treei + branchi[1:-1] #treef = treef + branchf[1:-1] #if successi and successf: # print 'DONE' # #raw_input() # lasti = branchi[-2] # lastf = branchf[-1] # qmeet = qrand # break #print notvisited.nid #branches = branchi[1:-1] + branchf[1:-1] #print 'branches: ',branches #print qrand #print branchi, successi #print branchf, successf #raw_input() #toremove = [] #print '1: ',notvisited[0:4] #print '2: ',notvisited[-5:-1] #print 'sg',start, goal #for n in branches: # if n in notvisited: # notvisited.remove(n) #wewillbreak = False #for n in treei: # if n in treef: # treef.remove(n) # print 'Houston, we have issues', n # lasti = branchi[-2] # lastf = branchf[-1] # qmeet = n # wewillbreak = True #if wewillbreak: # break #print 'toremove',toremove #toremove.reverse #print toremove #for n in toremove: # del notvisited[n] # del (notvisited.nid.remove(branchi[n]) # print len(notvisited) # Tell the main program about the winning path [self.path, self.pathCost] = makeRRTPath(treei,lasti,treef,lastf,self.graph) # Print out our "winning" path print self.path print 'Path printed' return class MyLaPlace(Pathfind): label = "My LaPlace" def FindPath(self, start, goal): # CAREFUL OF TABS vs. SPACES!!! # Example code fosr printing the graph... # illustrates how to access nodes, coordinates, etc #for i in range(self.graph.getNodeCount()): #print i #print i,"Node " + str(self.graph.nodeList[i].nid) + " at " + self.graph.nodeList[i].coordinateString() #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]) # Initialize our potential function err = [0] * len(self.graph.nodeList) npts = 8.0 for n in self.graph.nodeList: #n.state[0] = 1.0 #n.state[1] = 8-len(n.adjs) # first entry is value, next is number of walls n.state = [1, npts-len(n.adjs)] if n.nid == goal: n.state[0] = 0.011 else: err[n.nid] = 1.0 #if n.nid == start: # n.state[0] = 1.0 notvisited = [] # Everything but start and finish starts off not visited for n in range(len(self.graph.nodeList)): notvisited.append(self.graph.nodeList[n].nid) #notvisited = self.graph.getNodes()[:] #nodeList[:] del notvisited[start] del notvisited[goal] err_thresh = 0.001 palette = ['black','purple','blue','cyan','green','yellow','orange','red','pink','white'] #print err,max(err) #raw_input() #while max(err)>err_thresh: max_val = 1.0 while max_val==1.0: print max(err) #raw_input() colordots = [[]] * 10 for node in self.graph.nodeList: i = node.nid if i==goal: # or i==start: continue #new_val = self.graph.nodeList[i].state[1] new_val = 0.0 nn = 0 for n in range(len(self.graph.nodeList[i].adjs)): #print 'her it is: ',self.graph.nodeList[i].adjs #if self.graph.nodeList[i].adjs[n][1]<=1.0: nn = nn + 1 new_val = new_val + self.graph.nodeList[self.graph.nodeList[i].adjs[n][0]].state[0] new_val = (new_val + (npts - nn)) / npts err[i] = abs(self.graph.nodeList[i].state[0] - new_val) self.graph.nodeList[i].state[0] = new_val if i==start and new_val<1.0: max_val = new_val bucket = int((self.graph.nodeList[i].state[0]-0.01)*10) #print 'buck:',colordots,bucket,self.graph.nodeList[i].state[0], i temp = colordots[bucket][:] #print 'temp1',temp temp.append(i) #print 'temp2',temp colordots[bucket] = temp #print colordots, colordots[bucket] #colordots[0] = [25] #print colordots #raw_input() for n in [8,7,6,5,4,3,2,1,0]: #print n,':',colordots self.DrawList(colordots[n], clear=False, color=palette[n]) #raw_input() if True==False: for n in self.graph.nodeList: if n.state[0]<0.1: self.DrawList([i], clear=False, color='black') elif n.state[0]<0.2: self.DrawList([i], clear=False, color='purple') elif n.state[0]<0.3: self.DrawList([i], clear=False, color='blue') elif n.state[0]<0.4: self.DrawList([i], clear=False, color='cyan') elif n.state[0]<0.5: self.DrawList([i], clear=False, color='green') elif n.state[0]<0.6: self.DrawList([i], clear=False, color='yellow') elif n.state[0]<0.7: self.DrawList([i], clear=False, color='orange') elif n.state[0]<0.8: self.DrawList([i], clear=False, color='red') elif n.state[0]<0.9: self.DrawList([i], clear=False, color='pink') else: self.DrawList([i], clear=False, color='white') path = [start] cost = 0.0 i = start while i != goal: #for a in range(20): val = 2.0 prev = i for n in range(len(self.graph.nodeList[prev].adjs)): print prev, n if self.graph.nodeList[self.graph.nodeList[prev].adjs[n][0]].state[0] < val: val = self.graph.nodeList[self.graph.nodeList[prev].adjs[n][0]].state[0] i = self.graph.nodeList[prev].adjs[n][0] inc = self.graph.nodeList[prev].adjs[n][1] print prev, n, val, self.graph.nodeList[self.graph.nodeList[prev].adjs[n][0]].state[0], i, inc, self.graph.nodeList[prev].adjs[n][0] if i in path: print "local minima reached" break path.append(i) cost = cost + inc print path, cost [self.path, self.pathCost] = [path, cost] # Tell the main program about the winning path #[self.path, self.pathCost] = makeRRTPath(treei,lasti,treef,lastf,self.graph) # Print out our "winning" path #print self.path #print 'Path printed' return