def buildReward(self, gen=True):
if (gen):
self.r = GM()
var = [[1, 0, .7, 0], [0, 1, 0, .7], [.7, 0, 1, 0], [0, .7, 0, 1]]
for i in range(-2, 8):
for j in range(-2, 8):
self.r.addG(Gaussian([i, j, i, j], var, 5.6))
for i in range(-2, 8):
for j in range(-2, 8):
for k in range(-2, 8):
for l in range(-2, 8):
if (abs(i - j) >= 2 or abs(k - l) >= 2):
self.r.addG(Gaussian([i, j, k, l], var, -1))
print('Plotting Reward Model')
self.plotAllSlices(self.r, title='Uncondensed Reward')
print('Condensing Reward Model')
self.r.condense(50)
print('Plotting Condensed Reward Model')
self.plotAllSlices(self.r, title='Condensed Reward')
#f = open("../models/rewardModel4DIntercept.npy","w");
#np.save(f,self.r);
file = 'models/rewardModel4DIntercept'
self.r.printGMArrayToFile([self.r], file)
else:
#self.r = np.load("../models/rewardModel4DIntercept.npy").tolist();
file = 'models/rewardModel4DIntercept'
tmp = GM()
self.r = tmp.readGMArray4D(file)[0]
def buildAltObs(self, gen=True):
#A front back left right center model
#0:center
#1-4: left,right,down,up
if (gen):
self.pz = [0] * 5
for i in range(0, 5):
self.pz[i] = GM()
var = [[.7, 0, 0, 0], [0, .7, 0, 0], [0, 0, .7, 0], [0, 0, 0, .7]]
for i in range(-1, 7):
for j in range(-1, 7):
self.pz[0].addG(Gaussian([i, j, i, j], var, 1))
for i in range(-1, 7):
for j in range(-1, 7):
for k in range(-1, 7):
for l in range(-1, 7):
if (i - k > 0):
self.pz[1].addG(Gaussian([i, j, k, l], var, 1))
if (i - k < 0):
self.pz[2].addG(Gaussian([i, j, k, l], var, 1))
if (j - l > 0):
self.pz[3].addG(Gaussian([i, j, k, l], var, 1))
if (j - l < 0):
self.pz[4].addG(Gaussian([i, j, k, l], var, 1))
print('Plotting Observation Models')
for i in range(0, len(self.pz)):
self.plotAllSlices(self.pz[i], title='Uncondensed Observation')
print('Condensing Observation Models')
for i in range(0, len(self.pz)):
self.pz[i] = self.pz[i].kmeansCondensationN(
50, lowInit=[-1, -1, -1, -1], highInit=[7, 7, 7, 7])
print('Plotting Condensed Observation Models')
for i in range(0, len(self.pz)):
self.plotAllSlices(self.pz[i], title='Condensed Observation')
#f = open("../models/obsModel4DIntercept.npy","w");
#np.save(f,self.pz);
file = 'models/obsAltModel4DIntercept'
self.pz[0].printGMArrayToFile(self.pz, file)
else:
file = 'models/obsModel4DIntercept'
tmp = GM()
self.pz = tmp.readGMArray4D(file)
def buildObs(self, gen=True):
if (gen):
self.pz = [GM(), GM()]
var = [[1, 0, .7, 0], [0, 1, 0, .7], [.7, 0, 1, 0], [0, .7, 0, 1]]
for i in range(-2, 8):
for j in range(-2, 8):
self.pz[0].addG(Gaussian([i, j, i, j], var, 1))
for i in range(-2, 8):
for j in range(-2, 8):
for k in range(-2, 8):
for l in range(-2, 8):
if (abs(i - k) >= 2 or abs(j - l) >= 2):
self.pz[1].addG(Gaussian([i, j, k, l], var, 1))
print('Plotting Observation Models')
self.plotAllSlices(self.pz[0], title='Uncondensed Detection')
self.plotAllSlices(self.pz[1], title='Uncondensed Non-Detect')
print('Condensing Observation Models')
self.pz[0].condense(20)
self.pz[1] = self.pz[1].kmeansCondensationN(
45, lowInit=[-1, -1, -1, -1], highInit=[7, 7, 7, 7])
print('Plotting Condensed Observation Models')
self.plotAllSlices(self.pz[0], title='Condensed Detection')
self.plotAllSlices(self.pz[1], title='Condensed Non-Detect')
#f = open("../models/obsModel4DIntercept.npy","w");
#np.save(f,self.pz);
file = '../models/obsModel4DIntercept'
self.pz[0].printGMArrayToFile(self.pz, file)
else:
file = '../models/obsModel4DIntercept'
tmp = GM()
self.pz = tmp.readGMArray4D(file)
def MDPValueIteration(self, gen=True):
if (gen):
#Intialize Value function
self.ValueFunc = copy.deepcopy(self.r)
for g in self.ValueFunc.Gs:
g.weight = -1000
comparision = GM()
comparision.addG(
Gaussian(
[1, 0, 0, 0],
[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]],
1))
uniform = GM()
for i in range(0, 5):
for j in range(0, 5):
for k in range(0, 5):
for l in range(0, 5):
uniform.addG(
Gaussian([i, j, k, l],
[[4, 0, 0, 0], [0, 4, 0, 0],
[0, 0, 4, 0], [0, 0, 0, 4]], 1))
count = 0
#until convergence
while (not self.ValueFunc.comp(comparision) and count < 30):
print(count)
comparision = copy.deepcopy(self.ValueFunc)
count += 1
#print(count);
maxVal = -10000000
maxGM = GM()
for a in range(0, 2):
suma = GM()
for g in self.ValueFunc.Gs:
mean = (np.matrix(g.mean) -
np.matrix(self.delA[a])).tolist()
var = (np.matrix(g.var) +
np.matrix(self.delAVar)).tolist()
suma.addG(Gaussian(mean, var, g.weight))
suma.addGM(self.r)
tmpVal = self.continuousDot(uniform, suma)
if (tmpVal > maxVal):
maxVal = tmpVal
maxGM = copy.deepcopy(suma)
maxGM.scalerMultiply(self.discount)
maxGM = maxGM.kmeansCondensationN(20)
self.ValueFunc = copy.deepcopy(maxGM)
#self.ValueFunc.display();
#self.ValueFunc.plot2D();
print("MDP Value Iteration Complete")
#f = open("../policies/MDP4DIntercept.npy","w");
#np.save(f,self.ValueFunc);
file = "policies/MDP4DIntercept"
self.ValueFunc.printGMArrayToFile([self.ValueFunc], file)
else:
#self.ValueFunc = np.load("../policies/MDP4DIntercept.npy").tolist();
file = "policies/MDP4DIntercept"
tmp = GM()
self.ValueFunc = tmp.readGMArray4D(file)[0]
