def preComputeAlsSoftmaxFactored(self): G = self.Gamma; #for each alpha, each movement, each question, each question answer, each view cone als1 = np.zeros(shape = (len(G),len(self.delA),3,2)).tolist(); #questions left, right, in front of, behind for j in range(0,len(G)): for am in range(0,len(self.delA)): for aq in range(0,3): for oq in range(0,2): als1[j][am][aq][oq] = GM(); #get observation from question #If 0, multimodal alObs = GM(); if(oq == 0): for h in range(1,5): if(h!=aq and h!=3): alObs.addGM(self.pz.runVBND(G[j],h)); elif(oq == 1): if(aq==2): alObs.addGM(self.pz.runVBND(G[j],aq+2)); else: alObs.addGM(self.pz.runVBND(G[j],aq+1)); for k in alObs.Gs: mean = (np.matrix(k.mean) - np.matrix(self.delA[am])).tolist(); var = (np.matrix(k.var) + np.matrix(self.delAVar)).tolist(); weight = k.weight; als1[j][am][aq][oq].addG(Gaussian(mean,var,weight)); self.preAls = als1;
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 backup(als, modes, delA, delAVar, pz, r, maxMix, b):
newAls = [[[0 for i in range(0, len(pz))] for j in range(0, len(delA[0]))]
for k in range(0, len(als))]
for i in range(0, len(als)):
for j in range(0, len(delA[0])):
for k in range(0, len(pz)):
newAls[i][j][k] = GM()
for h in modes:
tmpGM = als[i].GMProduct(pz[j])
mean = tmpGM.getMeans()
for l in range(0, len(mean)):
mean[l][0] -= delA[modes.index(h)][j]
mean[l] = mean[l][0]
var = tmpGM.getVars()
for l in range(0, len(var)):
var[l][0][0] += delAVar
var[l] = var[l][0][0]
weights = tmpGM.getWeights()
tmpGM2 = GM()
for l in range(0, len(mean)):
tmpGM2.addG(Gaussian(mean[l], var[l], weights[l]))
#tmpGM2 = GM(mean,var,tmpGM.getWeights());
newAls[i][j][k].addGM(tmpGM2.GMProduct(h))
bestVal = -10000000000
bestAct = 0
bestGM = []
for a in range(0, len(delA[0])):
suma = GM()
for o in range(0, len(pz)):
suma.addGM(newAls[np.argmax([
continuousDot(newAls[j][a][o], b)
for j in range(0, len(newAls))
])][a][o])
suma.scalerMultiply(0.9)
suma.addGM(r[a])
for g in suma.Gs:
if (isinstance(g.mean, list)):
g.mean = g.mean[0]
g.var = g.var[0][0]
suma = suma.kmeansCondensationN(k=maxMix, lowInit=-20, highInit=20)
tmp = continuousDot(suma, b)
#print(a,tmp);
if (tmp > bestVal):
bestAct = a
bestGM = copy.deepcopy(suma)
bestVal = tmp
bestGM.action = bestAct
return bestGM
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 generate(self, s, a):
#sprime = np.random.choice([i for i in range(0,self.model.N)],p=self.model.px[a][s]);
#tmpGM = GM((np.array(s) + np.array(self.model.delA)).T.tolist(),self.model.delAVar,1);
tmpGM = GM()
tmpGM.addG(
Gaussian((np.array(s) + np.array(self.model.delA[a])).tolist(),
self.model.delAVar, 1))
sprime = tmpGM.sample(1)[0]
ztrial = [0] * len(self.model.pz)
for i in range(0, len(self.model.pz)):
ztrial[i] = self.model.pz[i].pointEval(sprime)
z = ztrial.index(max(ztrial))
reward = self.model.r[a].pointEval(s)
'''
if(a == 0 and s > 13):
reward = 10;
elif(a==1 and s<13):
reward = 10;
elif(a == 2 and s==13):
reward = 100;
else:
reward = -10;
'''
return [sprime, z, reward]
def assignRooms(self, belief):
#1. partition means into separate GMs, 1 for each room
allBels = []
allBounds = []
for room in self.map_.rooms:
tmp = GM()
tmpw = 0
allBounds.append([
self.map_.rooms[room]['min_x'], self.map_.rooms[room]['min_y'],
self.map_.rooms[room]['max_x'], self.map_.rooms[room]['max_y']
])
for g in belief:
m = [g.mean[2], g.mean[3]]
# if mean is inside the room
if (m[0] < self.map_.rooms[room]['max_x']
and m[0] > self.map_.rooms[room]['min_x']
and m[1] < self.map_.rooms[room]['max_y']
and m[1] > self.map_.rooms[room]['min_y']):
tmp.addG(deepcopy(g))
tmpw += g.weight
if (tmp.size == 0):
centx = (self.map_.rooms[room]['max_x'] +
self.map_.rooms[room]['min_x']) / 2
centy = (self.map_.rooms[room]['max_y'] +
self.map_.rooms[room]['min_y']) / 2
var = np.identity(4).tolist()
tmp.addG(Gaussian([0, 0, centx, centy], var, 0.0001))
allBels.append(tmp)
return allBels
def beliefUpdateSoftmax(self,b,a,o): btmp = GM(); btmp1 = GM(); for j in b.Gs: mean = (np.matrix(j.mean) + np.matrix(self.delA[a])).tolist()[0]; var = (np.matrix(j.var) + np.matrix(self.delAVar)).tolist(); weight = j.weight; btmp1.addG(Gaussian(mean,var,weight)); btmp = self.pz2.runVBND(btmp1,o); #btmp.condense(maxMix); btmp = btmp.kmeansCondensationN(self.maxMix); btmp.normalizeWeights(); return btmp;
def rehydrate_msg(weights_flat, means_flat, variances_flat):
'''
This method takes 'flattened' weights, means, and variances of a belief
represented by a gaussian mixture, reconstructs these into the appropriate
1xn vector, a list of 1xn vectors, and a list of nxn matrices respectively
into one dimensional vectors, and then uses these to constrct a gaussian
mixture class instance.
Inputs:
- weights_flat: array of weights
- means_flat: array of means
- variances_flat: array of variances
Outputs:
- belief: gaussian mixture representation of belief
'''
if len(weights_flat) == 0:
belief = None
return belief
else:
n = len(weights_flat)
r = len(means_flat) / n
n = len(weights_flat)
means_inflate = [[] for x in xrange(n)]
variances_inflate = [[[] for x in xrange(r)] for x in xrange(n)]
for i in range(0, n):
for j in range(0, r):
means_inflate[i].append(means_flat[i * r + j])
for k in range(0, r):
variances_inflate[i][j].append(
variances_flat[(i * (r * r)) + (j * r) + k])
belief = GM(means_inflate, variances_inflate, weights_flat)
return belief
def beliefUpdate(b, a, o, pz):
btmp = GM()
adelA = [-1, 1, 0]
adelAVar = 0.5
for obs in pz[o].Gs:
for bel in b.Gs:
sj = np.matrix(bel.mean).T
si = np.matrix(obs.mean).T
delA = np.matrix(adelA[a]).T
sigi = np.matrix(obs.var)
sigj = np.matrix(bel.var)
delAVar = np.matrix(adelAVar)
weight = obs.weight * bel.weight
weight = weight * mvn.pdf(
(sj + delA).T.tolist()[0],
si.T.tolist()[0], np.add(sigi, sigj, delAVar))
var = (sigi.I + (sigj + delAVar).I).I
mean = var * (sigi.I * si + (sigj + delAVar).I * (sj + delA))
weight = weight.tolist()
mean = mean.T.tolist()[0]
var = var.tolist()
btmp.addG(Gaussian(mean, var, weight))
btmp.normalizeWeights()
btmp.condense(1)
btmp.normalizeWeights()
return btmp
def backupFactored(self,b): G = self.Gamma; R = self.r; pz = self.pz; als1 = self.preAls; bestVal = -10000000000; bestAct= [0,0]; bestGM = []; for am in range(0,len(self.delA)): for aq in range(0,8): suma = GM(); for oq in range(0,2): suma.addGM(als1[np.argmax([self.continuousDot(als1[j][am][aq][oq],b) for j in range(0,len(als1))])][am][aq][oq]); suma.scalerMultiply(self.discount); suma.addGM(R[am]); tmp = self.continuousDot(suma,b); #print(a,tmp); if(tmp > bestVal): bestAct = [am,aq]; bestGM = copy.deepcopy(suma); bestVal = tmp; bestGM.action = bestAct; return bestGM;
def backup(self, b):
G = self.Gamma
R = self.r
pz = self.pz
als1 = self.preAls
#one alpha for each belief, so one per backup
bestVal = -10000000000
bestAct = 0
bestGM = []
for a in range(0, len(self.delA)):
suma = GM()
for o in range(0, len(pz)):
suma.addGM(als1[np.argmax([
self.continuousDot(als1[j][a][o], b)
for j in range(0, len(als1))
])][a][o])
suma.scalerMultiply(self.discount)
suma.addGM(R)
tmp = self.continuousDot(suma, b)
#print(a,tmp);
if (tmp > bestVal):
bestAct = a
bestGM = suma
bestVal = tmp
bestGM.action = bestAct
return bestGM
def stateLWISUpdate(self):
cp=self.prevPoses[-1];
prev = self.prevPoses[-2];
theta = np.arctan2([cp[1]-prev[1]],[cp[0]-prev[0]]);
#print(theta);
radius = self.ROBOT_VIEW_RADIUS;
points = [[cp[0]-radius,cp[1]-radius],[cp[0]+radius,cp[1]-radius],[cp[0]+radius,cp[1]+radius],[cp[0]-radius,cp[1]+radius]];
soft = Softmax()
soft.buildPointsModel(points,steepness=1);
#soft.buildTriView(pose = [cp[0],cp[1],theta],length=10,steepness=5);
change = False;
post = GM();
for g in self.belief:
if(distance(cp,g.mean) > self.ROBOT_VIEW_RADIUS+5):
post.addG(g);
else:
change = True;
tmp = soft.lwisUpdate(g,0,20,inverse=True);
#self.bounds = {'low':[0,0],'high':[437,754]}
tmp.mean[0] = max(self.bounds['low'][0]+1,tmp.mean[0]);
tmp.mean[1] = max(self.bounds['low'][1]+1,tmp.mean[1]);
tmp.mean[0] = min(self.bounds['high'][0]-1,tmp.mean[0]);
tmp.mean[1] = min(self.bounds['high'][1]-1,tmp.mean[1]);
post.addG(tmp);
self.belief = post;
self.belief.normalizeWeights();
return change;
def transformCartToPol(bcart, offset=[0, 0]):
bpol = GM()
for g in bcart:
m = g.mean
m1 = [0, 0]
m1[0] = m[0] - offset[0]
m1[1] = m[1] - offset[1]
mPrime = [np.sqrt(m1[0]**2 + m1[1]**2),
np.arctan2(m1[1], m1[0])]
if (m1[0]**2 + m1[1]**2 == 0):
m1[0] = 0.0001
m1[1] = 0.0001
J11 = m1[0] / np.sqrt(m1[0]**2 + m1[1]**2)
J12 = m1[1] / np.sqrt(m1[0]**2 + m1[1]**2)
J21 = -m1[1] / (m1[0]**2 + m1[1]**2)
J22 = m1[0] / (m1[0]**2 + m1[1]**2)
JCarPol = np.matrix([[J11, J12], [J21, J22]])
var = np.matrix(g.var)
varPrime = (JCarPol * var * JCarPol.T).tolist()
bpol.addG(Gaussian(mPrime, varPrime, g.weight))
return bpol
def buildReward(self,gen = True): if(gen): self.r = [0]*len(self.delA); for i in range(0,len(self.r)): self.r[i] = GM(); var = (np.identity(4)*1).tolist(); #Need gaussians along the borders for positive and negative rewards for i in range(0,len(self.r)): for x1 in range(self.bounds[0][0],self.bounds[0][1]): for y1 in range(self.bounds[1][0],self.bounds[1][1]): for x2 in range(self.bounds[2][0],self.bounds[2][1]): for y2 in range(self.bounds[3][0],self.bounds[3][1]): if(math.sqrt((x1-x2)**2 + (y1-y2)**2) < 0.5): self.r[i].addG(Gaussian([x1,y1,x2,y2],var,1)); for r in self.r: r.display(); f = open(self.fileNamePrefix + "REW.npy","w"); np.save(f,self.r); else: self.r = np.load(self.fileNamePrefix + "REW.npy").tolist();
def beliefUpdate(self, b, a, o, mod):
btmp = GM()
for obs in mod.pz[o].Gs:
for bel in b.Gs:
sj = np.matrix(bel.mean).T
si = np.matrix(obs.mean).T
delA = np.matrix(mod.delA[a]).T
sigi = np.matrix(obs.var)
sigj = np.matrix(bel.var)
delAVar = np.matrix(mod.delAVar)
weight = obs.weight * bel.weight
weight = weight * mvn.pdf(
(sj + delA).T.tolist()[0],
si.T.tolist()[0], np.add(sigi, sigj, delAVar))
var = (sigi.I + (sigj + delAVar).I).I
mean = var * (sigi.I * si + (sigj + delAVar).I * (sj + delA))
weight = weight.tolist()
mean = mean.T.tolist()[0]
var = var.tolist()
btmp.addG(Gaussian(mean, var, weight))
btmp.normalizeWeights()
btmp = btmp.kmeansCondensationN(1)
#btmp.condense(maxMix);
btmp.normalizeWeights()
return btmp
def buildObs():
pz = [0] * 3
for i in range(0, 3):
pz[i] = GM()
for i in range(-5, 6):
if (i * 4 <= 0):
pz[0].addNewG(i * 4, 4, .2)
elif (i * 4 >= 8):
pz[1].addNewG(i * 4, 4, .2)
else:
pz[2].addNewG(i * 4, 4, .2)
#for i in range(0,3):
#pz[i].plot(low=-20,high=20);
'''
weight = [-30,-20,-10,0];
bias = [60,50,30,0];
softClass = 0;
low = 0;
high = 5;
res = 100;
#Define Likelihood Model
pz = Softmax(weight,bias);
#pz.plot1D();
'''
return pz
def preComputeAls(self): G = self.Gamma; R = self.r; pz = self.pz; als1 = [[[0 for i in range(0,len(pz))] for j in range(0,len(self.delA))] for k in range(0,len(G))]; for j in range(0,len(G)): for a in range(0,len(self.delA)): for o in range(0,len(pz)): als1[j][a][o] = GM(); for k in range(0,G[j].size): for l in range(0,pz[o].size): #get weights wk,wl, and del weight = G[j].Gs[k].weight*pz[o].Gs[l].weight*mvn.pdf(pz[o].Gs[l].mean,G[j].Gs[k].mean,(np.matrix(G[j].Gs[k].var)+np.matrix(pz[o].Gs[l].var)).tolist()); #get sig and ss sigtmp = (np.matrix(G[j].Gs[k].var).I + np.matrix(pz[o].Gs[l].var)).tolist(); sig = np.matrix(sigtmp).I.tolist(); sstmp = np.matrix(G[j].Gs[k].var).I*np.transpose(np.matrix(G[j].Gs[k].mean)) + np.matrix(pz[o].Gs[l].var).I*np.transpose(np.matrix(pz[o].Gs[l].mean)); ss = np.dot(sig,sstmp).tolist(); smean = (np.transpose(np.matrix(ss)) - np.matrix(self.delA[a])).tolist(); sigvar = (np.matrix(sig)+np.matrix(self.delAVar)).tolist(); als1[j][a][o].addG(Gaussian(smean[0],sigvar,weight)); self.preAls = als1;
def beliefUpdate(self, b, a, o, maxMix=10):
btmp = GM()
for i in self.pz[o].Gs:
for j in b.Gs:
tmp = mvn.pdf(
np.add(np.matrix(j.mean),
np.matrix(self.delA[a])).tolist(), i.mean,
self.covAdd(self.covAdd(i.var, j.var), self.delAVar))
#print(i.weight,j.weight,tmp);
w = i.weight * j.weight * tmp.tolist()
sig = (np.add(
np.matrix(i.var).I,
np.matrix(self.covAdd(j.var, self.delAVar)).I)).I.tolist()
#sstmp = np.matrix(i.var).I*np.transpose(i.mean) + np.matrix(self.covAdd(j.var + self.delAVar)).I*np.transpose(np.add(np.matrix(j.mean),np.matrix(delA[a])));
sstmp1 = np.matrix(i.var).I * np.transpose(np.matrix(i.mean))
sstmp2 = np.matrix(self.covAdd(j.var, self.delAVar)).I
sstmp21 = np.add(np.matrix(j.mean), np.matrix(self.delA[a]))
sstmp3 = sstmp1 + sstmp2 * np.transpose(sstmp21)
smean = np.transpose(sig * sstmp3).tolist()[0]
btmp.addG(Gaussian(smean, sig, w))
btmp = btmp.kmeansCondensationN(maxMix)
#btmp.condense(maxMix);
btmp.normalizeWeights()
return btmp
def backup(self,b): G = self.Gamma; R = self.r; pz = self.pz; if(self.useSoft): obslen = pz.size; else: obslen = len(pz); als1 = self.preAls; bestVal = -10000000000; bestAct= 0; bestGM = []; for a in range(0,len(self.delA)): suma = GM(); for o in range(0,obslen): suma.addGM(als1[np.argmax([self.continuousDot(als1[j][a][o],b) for j in range(0,len(als1))])][a][o]); suma.scalerMultiply(self.discount); suma.addGM(R[a]); tmp = self.continuousDot(suma,b); #print(a,tmp); if(tmp > bestVal): bestAct = a; bestGM = copy.deepcopy(suma); bestVal = tmp; bestGM.action = bestAct; return bestGM;
def runVB(self,prior,softClassNum): #For the one dimensional case only post = GM(); weight = self.weights; bias = self.bias; alpha = self.alpha; zeta_c = self.zeta_c; for g in prior.Gs: prevLogCHat = -1000; count = 0; while(count < 100000): count = count+1; [mean,var,yc,yc2] = self.Estep(weight,bias,g.mean,g.var,alpha,zeta_c,softClassNum = softClassNum); [zeta_c,alpha] = self.Mstep(len(weight),yc,yc2,zeta_c,alpha,steps = 100); logCHat = self.calcCHat(g.mean,g.var,mean,var,alpha,zeta_c,yc,yc2,mod=softClassNum); if(abs(prevLogCHat - logCHat) < 0.00001): break; else: prevLogCHat = logCHat; post.addG(Gaussian(mean,var,g.weight*np.exp(logCHat).tolist()[0][0])) return post;
def test_discrete_handling():
variances = [[[5.320099817666392, 0.6070740587168416],
[0.6070740587168416, 2.91511327850999]],
[[2.4071373546900015, 0.22786169765608927],
[0.22786169765608927, 2.4736622912606343]],
[[3.065924316519349, 0.9318565801601824],
[0.9318565801601824, 3.269782301092388]]]
means = [[2.3804013106244195, -5.959479465161133],
[0.4759797740245023, -2.7971912319791215],
[-3.137589475680539, -5.78236605039465]]
weights = [0.2931922485762746, 0.33288265886527635, 0.3739250925584491]
mix = GM(means, variances, weights)
delta = 0.1
bounds = [-9.6, -3.6, 4, 3.6]
discrete_mix = mix.discretize2D(low=[bounds[0], bounds[1]],
high=[bounds[2], bounds[3]],
delta=delta)
flat_belief = discrete_dehydrate(discrete_mix)
# print(flat_belief)
shapes = [
int((bounds[2] - bounds[0]) / delta),
int((bounds[3] - bounds[1]) / delta)
]
print(shapes)
unflat_belief = discrete_rehydrate(flat_belief, shapes)
print(discrete_mix == unflat_belief)
def testRectangleModel():
pz = Softmax();
pz.buildRectangleModel([[2,2],[3,4]],1);
#print('Plotting Observation Model');
#pz.plot2D(low=[0,0],high=[10,5],vis=True);
prior = GM();
for i in range(0,10):
for j in range(0,5):
prior.addG(Gaussian([i,j],[[1,0],[0,1]],1));
# prior.addG(Gaussian([4,3],[[1,0],[0,1]],1));
# prior.addG(Gaussian([7,2],[[4,1],[1,4]],3))
prior.normalizeWeights();
dela = 0.1;
x, y = np.mgrid[0:10:dela, 0:5:dela]
fig,axarr = plt.subplots(6);
axarr[0].contourf(x,y,prior.discretize2D(low=[0,0],high=[10,5],delta=dela));
axarr[0].set_title('Prior');
titles = ['Inside','Left','Right','Up','Down'];
for i in range(0,5):
post = pz.runVBND(prior,i);
c = post.discretize2D(low=[0,0],high=[10,5],delta=dela);
axarr[i+1].contourf(x,y,c,cmap='viridis');
axarr[i+1].set_title('Post: ' + titles[i]);
plt.show();
def beliefUpdate(self,b,a,o): btmp = GM(); for obs in self.pz[o].Gs: for bel in b.Gs: sj = np.matrix(bel.mean).T; si = np.matrix(obs.mean).T; delA = np.matrix(self.delA[a]).T; sigi = np.matrix(obs.var); sigj = np.matrix(bel.var); delAVar = np.matrix(self.delAVar); weight = obs.weight*bel.weight; weight = weight*mvn.pdf((sj+delA).T.tolist()[0],si.T.tolist()[0],np.add(sigi,sigj,delAVar)); var = (sigi.I + (sigj+delAVar).I).I; mean = var*(sigi.I*si + (sigj+delAVar).I*(sj+delA)); weight = weight.tolist(); mean = mean.T.tolist()[0]; var = var.tolist(); btmp.addG(Gaussian(mean,var,weight)); btmp.normalizeWeights(); btmp = btmp.kmeansCondensationN(self.maxMix); #btmp.condense(maxMix); btmp.normalizeWeights(); return btmp;
def backup(self, b, als1):
G = self.Gamma
R = self.rew
numActs = 3
bestVal = -10000000000
bestAct = 0
bestGM = []
for a in range(0, numActs):
suma = GM()
suma.addGM(als1[np.argmax([
self.continuousDot(als1[j][a], b) for j in range(0, len(als1))
])][a])
suma.scalarMultiply(self.discount)
suma.addGM(R)
tmp = self.continuousDot(suma, b)
# print(a,tmp);
if (tmp > bestVal):
bestAct = a
bestGM = deepcopy(suma)
bestVal = tmp
bestGM.action = bestAct
return bestGM
def buildReward(self,gen = True):
if(gen):
self.r = [0]*len(self.delA);
for i in range(0,len(self.r)):
self.r[i] = GM();
var = (np.identity(4)*5).tolist();
cutFactor = 3;
for i in range(0,len(self.r)):
for x1 in range(int(np.floor(self.bounds[0][0]/cutFactor))-1,int(np.ceil(self.bounds[0][1]/cutFactor))+1):
for y1 in range(int(np.floor(self.bounds[1][0]/cutFactor))-1,int(np.ceil(self.bounds[1][1]/cutFactor))+1):
for x2 in range(int(np.floor(self.bounds[2][0]/cutFactor))-1,int(np.ceil(self.bounds[2][1]/cutFactor))+1):
for y2 in range(int(np.floor(self.bounds[3][0]/cutFactor))-1,int(np.ceil(self.bounds[3][1]/cutFactor))+1):
if(np.sqrt((x1-x2)**2 + (y1-y2)**2) < 1):
self.r[i].addG(Gaussian(np.array(([x1*cutFactor,y1*cutFactor,x2*cutFactor,y2*cutFactor])-np.array(self.delA[i])).tolist(),var,100));
# for r in self.r:
# r.display();
f = open("../models/"+self.fileNamePrefix + "REW.npy","w");
np.save(f,self.r);
else:
self.r = np.load("../models/"+self.fileNamePrefix + "REW.npy").tolist();
def precomputeAls(self):
G = self.Gamma
rew = self.rew
numActs = 3
als1 = [[0 for j in range(0, numActs)] for k in range(0, len(G))]
for j in range(0, len(G)):
for a in range(0, numActs):
als1[j][a] = GM()
for k in range(0, G[j].size):
weight = G[j][k].weight / self.A
D = ((self.C**-1 * self.Q * self.C**-1)**-1 +
G[j][k].var**-1)**-1
E = self.C**-1 * self.Q * self.C**-1
mean = self.A**-1 * \
(D*(E**-1*self.C*G[j][k].mean + G[j]
[k].var**-1*G[j][k].mean)-self.B*self.acts[a])
var = self.A**-1 * (
D * (E**-1 *
(E *
(D**-1 *
(D + self.R[a]) * D**-1) * E + self.Q + self.C *
G[j][k].var * self.C) * E**-1) * D) * self.A**-1
als1[j][a].addG(Gaussian(mean, var, weight))
return als1
def testMakeMap():
translator = POMDPTranslator()
b = GM()
b.addG(Gaussian([3, 2, 1, 0],
np.identity(4).tolist(), 1))
translator.makeBeliefMap(b, [0, 0, 0])
def testGeneralModel():
pz = Softmax()
pz.buildGeneralModel(2, 4, [[1, 0], [2, 0], [3, 0]],
np.matrix([-1, 1, -1, 1, 1, -1, 0, -1, -1]).T)
#print('Plotting Observation Model');
#pz.plot2D(low=[0,0],high=[10,5],vis=True);
prior = GM()
for i in range(0, 10):
for j in range(0, 5):
prior.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1))
# prior.addG(Gaussian([4,3],[[1,0],[0,1]],1));
# prior.addG(Gaussian([7,2],[[4,1],[1,4]],3))
prior.normalizeWeights()
dela = 0.1
x, y = np.mgrid[0:10:dela, 0:5:dela]
fig, axarr = plt.subplots(5)
axarr[0].contourf(x, y,
prior.discretize2D(low=[0, 0], high=[10, 5], delta=dela))
axarr[0].set_title('Prior')
titles = ['Inside', 'Left', 'Right', 'Down']
for i in range(0, 4):
post = pz.runVBND(prior, i)
c = post.discretize2D(low=[0, 0], high=[10, 5], delta=dela)
axarr[i + 1].contourf(x, y, c, cmap='viridis')
axarr[i + 1].set_title('Post: ' + titles[i])
plt.show()
def buildObs():
'''
pz = [GM()];
for i in range(-5,6):
pz[0].addNewG(i*4,4,4);
#pz[0].plot(low=-20,high = 20);
return pz;
'''
pz = [0] * 3
for i in range(0, 3):
pz[i] = GM()
for i in range(-5, 6):
if (i * 4 <= 0):
pz[0].addNewG(i * 4, 4, .2)
elif (i * 4 >= 8):
pz[1].addNewG(i * 4, 4, .2)
else:
pz[2].addNewG(i * 4, 4, .2)
#for i in range(0,3):
#pz[i].plot(low=-20,high=20);
return pz
def buildModels(self, dataSet, num_tar=5,saveFileName='hmm_train.npy'):
histModels = {}
warnings.filterwarnings("ignore")
for i in tqdm(range(num_tar),ncols=100):
allTypeData = []
allTypeLengths = []
currentSet = dataSet[i]
for j in range(len(currentSet)):
allTypeData.append(currentSet[j])
allTypeLengths.append(len(currentSet[j]))
allTypeData=np.concatenate(allTypeData)
allTypeData=np.reshape(allTypeData,(-1,1))
model = hmm.GaussianHMM(n_components=4).fit(allTypeData,allTypeLengths)
model_store = {}
model_store['transition'] = model.transmat_.tolist()
model_store['prior'] = model.startprob_.tolist()
means = model.means_.tolist()
var = model.covars_.tolist()
obs = []
for j in range(len(means)):
obs.append(GM(means[j],var[j],1))
model_store['obs'] = obs
histModels['Cumuliform'+str(i)] = model_store
if num_tar==10:
np.save('hmm_train_10.npy',histModels)
else:
np.save(saveFileName,histModels)
