def replanPaths(self, map, time): ''' Replans paths for all registered agents for map, considering all current observations :param map: (Map) map to plan on ''' #time = pygame.time.get_ticks() / 1000 clipTime = time - self.observationDuration self.observations = [ observation for observation in self.observations if observation[0][0] >= clipTime ] dataArray = [] stateArray = [] for observation in self.observations: (data, state) = observation dataArray.append(data) stateArray.append(state) nx = (map.sizeX - 1) * map.dX ny = (map.sizeY - 1) * map.dY x = np.linspace(0, nx, map.sizeX) y = np.linspace(0, ny, map.sizeY) X, Y = np.meshgrid(x, y) ell = np.array([5, 5, 5]) if (len(dataArray) > 0): updateMap = gpupdate.GPUpdate(self.initialMap._distribution, X, Y, np.matrix(dataArray), np.array(stateArray), time, ell, 1) updateMap = np.transpose(updateMap) map.updateMapDistribution(updateMap / updateMap.sum()) self.splitMapPaths = self.generatePrimitivePaths( map, self._splitMapAgents, self._tries) for splitMapIndex in range(len(self._splitMapAgents)): agentList = self._splitMapAgents[splitMapIndex] for i in range(len(agentList)): agentList[i].setNewPath(self.splitMapPaths[splitMapIndex][i]) self.infoMap = Map(map.sizeX, map.sizeY) self.infoMap.updateMapDistribution( self.generateCurrentInfoMap(map, 1.5)) # Save infoMap distribution '''file = open("saved data/info maps/infoMapDistributionData.txt", "a+") for row in self.infoMap._distribution: for element in row: file.write(str(element) + " ") file.write("-1 ") file.write('-2 ') file.close()''' infoMap = self.generateCurrentInfoMap(map, 1) erg = mathlib.calcErgodicity(map, infoMap) print(erg) # Save ergodicity values file = open("saved data/ergvals.txt", "a+") file.write(str(erg) + " ")
def bestMultiPath(self, m, n, len, tries, numAgent, oMap): minErg = -1 multiPath = [] for i in range(tries): multilist = self.multiPath(m, n, len, numAgent, oMap) pathDistrib = self.multiAgentInfoMap(multilist, numAgent, m) erg = mathlib.calcErgodicity(self.map, pathDistrib, 10) if minErg < 0 or erg < minErg: minErg = erg multiPath = multilist return multiPath
def bestMultiPathWithSpeed(self, m, n, time, tries, agents, omap): minErg = -1 multipath = [] for i in range(tries): tempmultipath = [] for j in range(len(agents)): agents[j].path = self.getPathWithObstacleWithSpeed( agents[j], m, n, time, omap) tempmultipath.append(agents[j].path) pathDistrib = self.multiAgentInfoMapWithSpeed( tempmultipath, agents, m) erg = mathlib.calcErgodicity(self.map, pathDistrib, 10) if minErg < 0 or erg < minErg: minErg = erg multipath = tempmultipath for agent in agents: agent.bestspeedlist = agent.speedlist return multipath
def getBestPath(self, startpoint, m, n, len, tries=3): """ Returns the best path out of multiple tries Args: startpoint ((int,int)): the point from which we generate the path m (Map): map to generate path on n (int): number of points to generate len (int): length of generated path tries (int): how many paths should be sampled """ minErg = -1 bestPath = np.array([]) for i in range(tries): list = self.closestPointPath(startpoint, m, n, len) pathDistrib = self.pathInfoMap( mathlib.makeAllLineWithSpeed(list, 3), self.map.getDistribution(), self.gaussianMap) pathDistrib = pathDistrib / pathDistrib.sum() erg = mathlib.calcErgodicity(self.map, pathDistrib, 10) if minErg < 0 or erg < minErg: minErg = erg bestPath = list return bestPath
def generatePrimitivePaths(self, map, splitMapAgents, tries): ''' Returns an array of primitive paths (an array of primitives) that take pathTime time for each agent. path at index i corresponds to agent i in agents array. Args: map (Map): Map to generate path on pathTime (float): time path traversal should take agents (Agent array): the agents to generate path for tries (int): how many tries should we do for the path (we pick best one using ergodicity calculation) ''' '''bestSplitMapPaths = [[],[],[]] for splitMapIndex in range(len(splitMapAgents)): minErg = -1 agentList = splitMapAgents[splitMapIndex] for trial in range(tries * len(agentList)): #print(minErg) paths = [] for i in range(len(agentList)): currentAgent = agentList[i] generator = self._generators[currentAgent.generatorIndex] inBounds = False path = PathPrimitive([]) while inBounds == False: (path, inBounds) = generator.generateRandomPrimitivePath(map, currentAgent) paths.append(path) infoMap = self.generateInfoMapFromPrimitivePaths(map, 5, agentList, paths) erg = mathlib.calcErgodicity(map, infoMap, splitMapIndex, 15) if minErg < 0 or erg < minErg: minErg = erg bestSplitMapPaths[splitMapIndex] = np.array(paths) #if (minErg != -1): # print(minErg) # printing final erg values: for i in range(len(splitMapAgents)): agentList = splitMapAgents[i] infoMap = self.generateInfoMapFromPrimitivePaths(map, 5, agentList, bestSplitMapPaths[i]) erg = mathlib.calcErgodicity(map, infoMap, i, 15) #print("Final erg " + str(erg)) if (erg > 0.0): print(erg) return np.array(bestSplitMapPaths)''' bestSplitMapPaths = [[], [], []] originalMap = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) tries = 1 lowestx, lowesty = map.getWorldSize() highestx = 0 highesty = 0 for splitMapIndex in range(len(splitMapAgents)): minErg = -1 agentList = splitMapAgents[splitMapIndex] #print("Number of agents: " + str(len(agentList))) for trial in range(tries * len(agentList)): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) #print("Map size: " + str(len(map._distribution)) + ", " + str(len(map._distribution[0]))) #print("Map sum = " + str(map._distribution.sum())) #for i in range(len(map._distribution)): # print("Line sum = " + str(map._distribution[i].sum())) for n in range(10): #print(minErg) paths = [] for i in range(len(agentList)): currentAgent = agentList[i] generator = self._generators[ currentAgent.generatorIndex] inBounds = False count = 0 path = PathPrimitive([]) while inBounds == False: count += 1 if (count % 100000 == 0): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) (path, inBounds) = generator.generateRandomPrimitivePath( map, currentAgent) paths.append(path) infoMap = self.generateInfoMapFromPrimitivePaths( originalMap, 5, agentList, paths) erg = mathlib.calcErgodicity(originalMap, infoMap, splitMapIndex, 15) if minErg < 0 or erg < minErg: minErg = erg bestSplitMapPaths[splitMapIndex] = np.array(paths) #lowestx = 100 #lowesty = 100 lowestx, lowesty = map.getWorldSize() highestx = 0 highesty = 0 for path in paths: for x in range(int(path.getTotalTime())): (currentx, currenty) = path.getPointAtTime(float(x)) #(currentx, currenty) = map.mapToWorld(currentx, currenty) if currentx > highestx: highestx = currentx if currentx < lowestx: lowestx = currentx if currenty > highesty: highesty = currenty if currenty < lowesty: lowesty = currenty lowestx, lowesty = map.worldToMap(lowestx, lowesty) highestx, highesty = map.worldToMap(highestx, highesty) if (len(map._distribution) <= highesty) or (len( map._distribution[0]) <= highestx): #print("flag") map = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) else: #Map stays unchanged if either dimension of new map is 0 if (len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)]) == 0): map = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) elif (len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)][0]) == 0): map = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) #else: # map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #Map stays unchanged if sum of probabilities in new map is 0.0 elif (originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)].sum() == 0.0): map = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) #else: # map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #elif(len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) > 0): #Map stays unchanged if new map dimensions is less than 5x5 elif ((len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)]) * len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)][0])) < 25): map = Map(map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) #else: # map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) elif (originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)].sum() == 1.0): for i in range( len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)])): if (originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)][i].sum() == 1.0): map = Map( map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) #change to original # Map stays unchanged if only one value in new map is non-zero else: flag = 0 for i in range( len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)])): if (originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)][i].sum() == 0.0): flag += 1 #figure out how to make this not hard coded if flag >= (len(originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)]) - 3): print("Number of all-zero rows" + str(flag)) map = Map( map.sizeX, map.sizeY, map.dX, map.dY, 50, map._distribution) #Change to originalMap else: map = Map( abs(int(highestx) - int(lowestx)), abs(int(highesty) - int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[ int(lowesty):int(highesty), int(lowestx):int(highestx)]) if (minErg != -1): print(minErg) if (minErg > 0.2): print(map.sizeX) #printing final erg values: for i in range(len(splitMapAgents)): agentList = splitMapAgents[i] infoMap = self.generateInfoMapFromPrimitivePaths( originalMap, 5, agentList, bestSplitMapPaths[i]) erg = mathlib.calcErgodicity(originalMap, infoMap, i, 15) #print("Final erg " + str(erg)) #if (erg > 0.0): #print(erg) return np.array(bestSplitMapPaths)
def generatePrimitivePaths(self, map, splitMapAgents, tries): ''' Returns an array of primitive paths (an array of primitives) that take pathTime time for each agent. path at index i corresponds to agent i in agents array. Args: map (Map): Map to generate path on pathTime (float): time path traversal should take agents (Agent array): the agents to generate path for tries (int): how many tries should we do for the path (we pick best one using ergodicity calculation) ''' '''Original planner''' bestSplitMapPaths = [[], [], []] for splitMapIndex in range(len(splitMapAgents)): minErg = -1 agentList = splitMapAgents[splitMapIndex] for trial in range(tries * len(agentList)): paths = [] #Finding fft of map map_fft = fftpack.fft2(map._distribution) #print("New trial") for i in range(len(agentList)): currentAgent = agentList[i] generator = self._generators[currentAgent.generatorIndex] inBounds = False path = PathPrimitive([]) reconstructedmap = currentAgent.distributeAgent(map_fft) reconstructedmap = Map(len(reconstructedmap), len(reconstructedmap[0]), map.dX, map.dY, 50, reconstructedmap) count = 0 while inBounds == False: count += 1 if (count > 7): (path, inBounds) = generator.generateRandomPrimitivePath( map, currentAgent) else: (path, inBounds) = generator.generateRandomPrimitivePath( reconstructedmap, currentAgent) paths.append(path) infoMap = self.generateInfoMapFromPrimitivePaths( map, 5, agentList, paths) erg = mathlib.calcErgodicity(map, infoMap, splitMapIndex, 15) if minErg < 0 or erg < minErg: minErg = erg bestSplitMapPaths[splitMapIndex] = np.array(paths) # Printing final erg values: '''for i in range(len(splitMapAgents)): agentList = splitMapAgents[i] infoMap = self.generateInfoMapFromPrimitivePaths(map, 5, agentList, bestSplitMapPaths[i]) erg = mathlib.calcErgodicity(map, infoMap, i, 15) #print("Final erg " + str(erg)) if (erg > 0.0): print(erg)''' return np.array(bestSplitMapPaths) '''Cross entropy planner''' '''bestSplitMapPaths = [[], [], []]
def generatePrimitivePaths(self, map, splitMapAgents, tries): ''' Returns an array of primitive paths (an array of primitives) that take pathTime time for each agent. path at index i corresponds to agent i in agents array. Args: map (Map): Map to generate path on pathTime (float): time path traversal should take agents (Agent array): the agents to generate path for tries (int): how many tries should we do for the path (we pick best one using ergodicity calculation) ''' '''bestSplitMapPaths = [[],[],[]] for splitMapIndex in range(len(splitMapAgents)): minErg = -1 agentList = splitMapAgents[splitMapIndex] tries = 1 for trial in range(tries * len(agentList)): paths = [] for i in range(len(agentList)): currentAgent = agentList[i] generator = self._generators[currentAgent.generatorIndex] inBounds = False path = PathPrimitive([]) while inBounds == False: (path, inBounds) = generator.generateRandomPrimitivePath(map, currentAgent) paths.append(path) infoMap = self.generateInfoMapFromPrimitivePaths(map, 5, agentList, paths) erg = mathlib.calcErgodicity(map, infoMap, splitMapIndex, 15) if minErg < 0 or erg < minErg: minErg = erg bestSplitMapPaths[splitMapIndex] = np.array(paths) print(minErg) return np.array(bestSplitMapPaths)''' bestSplitMapPaths = [[],[],[]] originalMap = Map(map.sizeX,map.sizeY,map.dX,map.dY,50,map._distribution) tries = 15 for splitMapIndex in range(len(splitMapAgents)): print("Start new") minErg = -1 agentList = splitMapAgents[splitMapIndex] lowestx = 100 highestx = 0 #print("Number of trials: " + str(tries)) print("Length of agent list: " + str(len(agentList))) for trial in range(tries * len(agentList)): #print("New trial") paths = [] for i in range(len(agentList)): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) #print("new agent " + str(i)) ''' print("Map info (x): " + str(len(map._distribution)) + ", " + str(map.sizeY)) print("Map info (y): " + str(len(map._distribution[0])) + ", " + str(map.sizeX)) print("Map slicing coordinates: " + str(lowestx) + ", " + str(highestx)) ''' currentAgent = agentList[i] generator = self._generators[currentAgent.generatorIndex] for n in range(1): inBounds = False count = 0 path = PathPrimitive([]) #print("Path finding level " + str(n)) while inBounds == False: count += 1 #print("Gets to here") if (count%500 == 0): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) #print("Overshoot " + str(count)) #print("Before finding path" + str(n)) (path, inBounds) = generator.generateRandomPrimitivePath(map, currentAgent) #print("Found path") #if (count%1000 == 0): # print("stuck here") lowestx = 100 lowesty = 100 highestx = 0 highesty = 0 for x in range(int(path.getTotalTime())): (currentx, currenty) = path.getPointAtTime(float(x)) if currentx > highestx: highestx = currentx if currentx < lowestx: lowestx = currentx if currenty > highesty: highesty = currenty if currenty < lowesty: lowesty = currenty ''' print("Map size: " + str(map.sizeX) + ", " + str(map.sizeY)) print("Starting coordinate is: " + str(int(lowestx)) + ", " + str(int(lowesty))) print("Test value before slice: " + str(map._distribution[0][0])) print("Sum of slice: " + str(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)].sum())) print("Coordinates for slice: lowx = " + str(lowestx) + ", highx = " + str(highestx) + ", lowy = " + str(lowesty) + ", highy = " + str(highesty)) ''' #Changes map back to original map if either dimension is 0 if (len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) == 0): #print("Size of map was 0") map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) elif (len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)][0]) == 0): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) else: map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #Changes map back to original map if sum of probabilities in map is 0.0 if (originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)].sum() == 0.0): map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) #print("Changed to original map") #print("Slice:" + str(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)])) else: map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #Changes map back to original map if only one value is none-zero #if (originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)].sum() >= 0.99999): flag = 0 for i in range(len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)])): if (originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)][i].sum() == 0.0): flag += 1 if flag >= (len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) - 3): #print("Only one none zero value in map") map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) else: map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #Changes map back to original map if map dimensions goes below 5x5 if(len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) > 0): if ((len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) * len(originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)][0])) < 25): #print("Map got too small") map = Map(originalMap.sizeX, originalMap.sizeY, originalMap.dX, originalMap.dY, 50, originalMap._distribution) else: map = Map(abs(int(highestx)-int(lowestx)), abs(int(highesty)-int(lowesty)), map.dX, map.dY, 50, originalMap._distribution[int(lowesty):int(highesty), int(lowestx):int(highestx)]) #print("Test value from distribution: " + str(map._distribution[0][0])) #print(len(map._distribution)) paths.append(path) infoMap = self.generateInfoMapFromPrimitivePaths(originalMap, 5, agentList, paths) erg = mathlib.calcErgodicity(originalMap, infoMap, splitMapIndex, 15) if minErg < 0 or erg < minErg: minErg = erg bestSplitMapPaths[splitMapIndex] = np.array(paths) print(minErg) return np.array(bestSplitMapPaths)