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)
