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)
'''
'''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)