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(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 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 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 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 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 createRandomMixture(size, dims=2):
testMix = GM()
for i in range(0, size):
testMix.addG(sampleWishart(dims))
testMix.normalizeWeights()
#testMix.clean();
return testMix
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 simulate(cop_pose=[0, 0, -15.3], steps=10):
translator = POMDPTranslator()
b = GM()
b.addG(Gaussian([3, 2, -2, 2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([3, 2, -8, -2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([3, 2, -4, -2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([0, 0, 2, 2], (np.identity(4) * 6).tolist(), 1))
b.normalizeWeights()
#cop_pose = [0,0,-15.3];
rob_pose = [-5, 0, 0]
for count in range(0, steps):
[b, cop_pose, qs] = translator.getNextPose(b, None, [cop_pose])
print(distance(cop_pose[0], cop_pose[1], rob_pose[0], rob_pose[1]))
def testInvertedSoftmaxModels():
b = GM()
b.addG(Gaussian([2, 2], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([4, 2], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([2, 4], [[1, 0], [0, 1]], 1))
b.addG(Gaussian([3, 3], [[1, 0], [0, 1]], 1))
b.normalizeWeights()
b.plot2D()
pz = Softmax()
pz.buildOrientedRecModel([2, 2], 0, 1, 1, 5)
#pz.plot2D();
startTime = time.clock()
b2 = GM()
for i in range(1, 5):
b2.addGM(pz.runVBND(b, i))
print(time.clock() - startTime)
b2.plot2D()
startTime = time.clock()
b3 = GM()
b3.addGM(b)
tmpB = pz.runVBND(b, 0)
tmpB.normalizeWeights()
tmpB.scalerMultiply(-1)
b3.addGM(tmpB)
tmpBWeights = b3.getWeights()
mi = min(b3.getWeights())
#print(mi);
for g in b3.Gs:
g.weight = g.weight - mi
b3.normalizeWeights()
print(time.clock() - startTime)
#b3.display();
b3.plot2D()
def separateAndNormalize(mix):
#Takes a mixture and returns two mixtures
#and two anti-normalizing constants
posMix = GM()
negMix = GM()
posSum = 0
negSum = 0
for g in mix:
if (g.weight < 0):
negSum += g.weight
negMix.addG(deepcopy(g))
else:
posSum += g.weight
posMix.addG(deepcopy(g))
posMix.normalizeWeights()
negMix.normalizeWeights()
return [posMix, negMix, posSum, negSum]
def testGetNextPose():
translator = POMDPTranslator()
b = GM()
b.addG(Gaussian([3, 2, -2, 2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([3, 2, -8, -2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([3, 2, -4, -2],
np.identity(4).tolist(), 1))
b.addG(Gaussian([0, 0, 2, 2], (np.identity(4) * 6).tolist(), 1))
b.normalizeWeights()
#for i in range(-8,3):
#for j in range(-1,2):
#b.addG(Gaussian([3,2,i,j],np.identity(4).tolist(),1));
translator.cutGMTo2D(b, dims=[2, 3]).plot2D(low=[-9.6, -3.6],
high=[4, 3.6])
[bnew, goal_pose, qs] = translator.getNextPose(b, None, [[0, 0, -15.3]])
bnew = translator.cutGMTo2D(bnew, dims=[2, 3])
bnew.plot2D(low=[-9.6, -3.6], high=[4, 3.6])
#print(qs)
'''
def testLWIS():
pz = Softmax()
pose = [0, 0, 0]
pz.buildTriView(pose, length=2, steepness=10)
prior = GM()
#prior.addG(Gaussian([1,0],[[1,0],[0,1]],1));
for i in range(0, 100):
prior.addG(
Gaussian([np.random.random() * 4 - 2,
np.random.random() * 4 - 2], [[0.1, 0], [0, 0.1]], 1))
prior.normalizeWeights()
post = GM()
for g in prior:
post.addG(pz.lwisUpdate(g, 0, 500, inverse=True))
#post.display();
[x1, y1, c1] = prior.plot2D(low=[-5, -5], high=[5, 5], vis=False)
[x3, y3, c3] = pz.plot2D(low=[-5, -5], high=[5, 5], vis=False)
[x2, y2, c2] = post.plot2D(low=[-5, -5], high=[5, 5], vis=False)
diffs = c2 - c1
print(np.amax(c2))
print(np.amax(diffs))
print(np.amin(diffs))
fig, axarr = plt.subplots(4)
axarr[0].contourf(x1, y1, c1)
axarr[0].set_title('Prior')
axarr[1].contourf(x3, y3, c3)
axarr[1].set_title('Likelihood')
axarr[2].contourf(x2, y2, c2)
axarr[2].set_title('Posterior')
axarr[3].contourf(x2, y2, diffs)
axarr[3].set_title('Diffs')
plt.show()
def beliefUpdate(modes,delA,delAVar,pz,bels,a,o,cond = -1): #Initialize btmp = GM(); for d in bels.Gs: for h in modes: for f in h.Gs: for l in pz[o].Gs: C1 = 1/(1/f.var + 1/d.var); c1 = C1*((1/f.var)*f.mean + (1/d.var)*d.mean); C2 = C1 + delAVar; c2 = c1+delA[modes.index(h)][a]; weight = d.weight*f.weight*l.weight*mvn.pdf(l.mean,c2,l.var+C2); var = 1/((1/l.var)+(1/C2)); mean = var*((1/l.var)*l.mean + (1/C2)*c2); g = Gaussian(mean,var,weight); btmp.addG(g); btmp.normalizeWeights(); if(cond != -1): btmp = btmp.kmeansCondensationN(k=cond,lowInit = -20,highInit=20); #btmp.condense(cond); for g in btmp: while(isinstance(g.var,list)): g.var = g.var[0]; btmp.display(); return btmp;
def makeInitialBelief():
bel = GM()
numMix = 100
varscale = .2
for i in range(0, numMix):
w = np.random.random()
mean = [np.random.random() * 8,
np.random.random() * 8]
tmp = np.random.random() * varscale
var = [[np.random.random() * 1 + varscale, 0],
[0, np.random.random() * 1 + varscale]]
bel.addG(Gaussian(mean, var, w))
bel.addG(Gaussian([4, 7], [[1, 0], [0, 1]], 4))
bel.normalizeWeights()
[x, y, belView] = bel.plot2D(low=[0, 0], high=[10, 10], vis=False)
plt.contourf(x, y, belView)
plt.savefig('../img/testBel.png')
plt.cla()
plt.clf()
plt.close()
return bel
def stateObsUpdate(self,name,relation,pos="Is"): if(name == 'You'): #Take Cops Position, builid box around it cp=self.copPose; points = [[cp[0]-5,cp[1]-5],[cp[0]+5,cp[1]-5],[cp[0]+5,cp[1]+5],[cp[0]-5,cp[1]+5]]; soft = Softmax() soft.buildPointsModel(points,steepness=3); else: soft = self.sketches[name]; softClass = self.spatialRealtions[relation]; if(pos=="Is"): self.belief = soft.runVBND(self.belief,softClass); self.belief.normalizeWeights(); else: tmp = GM(); for i in range(0,5): if(i!=softClass): tmp.addGM(soft.runVBND(self.belief,i)); tmp.normalizeWeights(); self.belief=tmp; if(self.belief.size > self.MAX_BELIEF_SIZE): self.belief.condense(self.MAX_BELIEF_SIZE); self.belief.normalizeWeights()
def testMakeNear():
pzIn = Softmax()
pzOut = Softmax()
cent = [4, 4]
orient = 0
nearness = 2
lengthIn = 3
lengthOut = lengthIn + nearness
widthIn = 2
widthOut = widthIn + nearness
pzIn.buildOrientedRecModel(cent, orient, lengthIn, widthIn, steepness=10)
pzOut.buildOrientedRecModel(cent,
orient,
lengthOut,
widthOut,
steepness=10)
#pzIn.plot2D(low=[0,0],high=[10,10]);
#pzOut.plot2D(low=[0,0],high=[10,10]);
b = GM()
for i in range(0, 10):
for j in range(0, 10):
b.addG(Gaussian([i, j], [[1, 0], [0, 1]], 1))
b.normalizeWeights()
b1 = GM()
for i in range(1, 5):
b1.addGM(pzIn.runVBND(b, i))
b1.normalizeWeights()
b2 = GM()
b2.addGM(pzOut.runVBND(b1, 0))
b2.normalizeWeights()
fig, axarr = plt.subplots(3)
[x, y, c] = b.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[0].contourf(x, y, c)
[x, y, c] = b1.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[1].contourf(x, y, c)
[x, y, c] = b2.plot2D(low=[0, 0], high=[10, 10], vis=False)
axarr[2].contourf(x, y, c)
plt.show()
def testMakeNear(): pzIn = Softmax(); pzOut = Softmax(); cent = [3.5,3.5]; orient = 0; nearness = 2; lengthIn = 3; lengthOut = lengthIn+nearness; widthIn = 2; widthOut = widthIn+nearness; pzIn.buildOrientedRecModel(cent,orient,lengthIn,widthIn,steepness=10); pzOut.buildOrientedRecModel(cent,orient,lengthOut,widthOut,steepness=10); #pzIn.plot2D(low=[0,0],high=[10,10]); #pzOut.plot2D(low=[0,0],high=[10,10]); b = GM(); for i in range(0,11): for j in range(0,11): b.addG(Gaussian([i,j],[[1,0],[0,1]],1)); b.normalizeWeights(); b1 = GM(); for i in range(1,5): b1.addGM(pzIn.runVBND(b,i)); b1.normalizeWeights(); b2 = GM(); b2.addGM(pzOut.runVBND(b1,0)); b2.normalizeWeights(); fig,axarr = plt.subplots(3); [x,y,c] = b.plot2D(low=[0,0],high=[10,10],vis=False); axarr[0].contourf(x,y,c); [x,y,c] = b1.plot2D(low=[0,0],high=[10,10],vis=False); axarr[1].contourf(x,y,c); [x,y,c] = b2.plot2D(low=[0,0],high=[10,10],vis=False); axarr[2].contourf(x,y,c); plt.show();
def buildRadialSoftmaxModels():
dims = 2
#Target Model
steep = 2
weight = (np.array([-2, -1, 0]) * steep).tolist()
bias = (np.array([6, 4, 0]) * steep).tolist()
for i in range(0, len(weight)):
weight[i] = [weight[i], 0]
pz = Softmax(weight, bias)
obsOffset = [-7, -4]
observation = 2
#pz.plot2D(low=[0,0],high=[1,6.28]);
'''
H = np.matrix([[2,math.pi*2,1],[2,math.pi*3/4,1],[2,math.pi/2,1]]);
print(nullspace(H));
#Modified Target Model
#def buildGeneralModel(self,dims,numClasses,boundries,B,steepness=1):
B = np.matrix([0.447,0,-0.8944,0.447,0,-0.894]).T;
boundries = [[1,0],[2,0]];
pz = Softmax();
pz.buildGeneralModel(2,3,boundries,B,steepness=2);
'''
'''
cent = [0,0];
length = 3;
width = 2;
orient = 0;
pz = Softmax();
pz.buildOrientedRecModel(cent,orient,length,width,steepness=5);
'''
print('Plotting Observation Model')
[xobs, yobs, domObs] = plot2DPolar(pz,
low=[-10, -10],
high=[10, 10],
delta=0.1,
offset=obsOffset,
vis=False)
[xobsPol, yobsPol, domObsPol] = pz.plot2D(low=[0, -3.14],
high=[10, 3.14],
delta=0.1,
vis=False)
# fig = plt.figure()
# ax = fig.gca(projection='3d');
# colors = ['b','g','r','c','m','y','k','w','b','g'];
# for i in range(0,len(model)):
# ax.plot_surface(x,y,model[i],color = colors[i]);
# plt.show();
#pz.plot2D(low=[-10,-10],high=[10,10]);
scaling = o1
bcart = GM()
for i in range(-10, 11):
for j in range(-10, 11):
# if(i != 0 or j != 0):
bcart.addG(Gaussian([i, j], [[scaling, 0], [0, scaling]], 1))
bcart.normalizeWeights()
[xpri, ypri, cpri] = bcart.plot2D(low=[-10, -10], high=[10, 10], vis=False)
bpol = transformCartToPol(bcart, obsOffset)
for i in range(0, 3):
bpolPrime = pz.runVBND(bpol, i)
bcartPrime = transformPolToCart(bpolPrime, obsOffset)
bcartPrime.normalizeWeights()
[xpos, ypos, cpos] = bcartPrime.plot2D(low=[-10, -10],
high=[10, 10],
vis=False)
fig, axarr = plt.subplots(3)
axarr[0].contourf(xpri, ypri, cpri)
axarr[0].set_ylabel("Prior")
axarr[1].contourf(xobs, yobs, domObs)
axarr[1].set_ylabel("Observation: " + str(i))
axarr[2].contourf(xpos, ypos, cpos)
axarr[2].set_ylabel("Posterior")
plt.show()
fig, axarr = plt.subplots(1, 2)
axarr[0].contourf(xobs, yobs, domObs)
axarr[1].contourf(xobsPol, yobsPol, domObsPol)
axarr[0].set_title("Cartesian Observations")
axarr[1].set_title("Polar Observations")
axarr[0].set_xlabel("X")
axarr[0].set_ylabel("Y")
axarr[1].set_xlabel("Radius (r)")
axarr[1].set_ylabel("Angle (theta)")
plt.show()
bTestCart = GM()
bTestCart.addG(Gaussian([1, 2], [[1, 0], [0, 1]], .25))
bTestCart.addG(Gaussian([-3, 1], [[3, 0], [0, 1]], .25))
bTestCart.addG(Gaussian([1, -4], [[1, 0], [0, 2]], .25))
bTestCart.addG(Gaussian([-3, -3], [[2, 1.2], [1.2, 2]], .25))
bTestPol = transformCartToPol(bTestCart, [0, 0])
[xTestCart, yTestCart, cTestCart] = bTestCart.plot2D(low=[-10, -10],
high=[10, 10],
vis=False)
[xTestPol, yTestPol, cTestPol] = bTestPol.plot2D(low=[0, -3.14],
high=[10, 3.14],
vis=False)
fig, axarr = plt.subplots(1, 2)
axarr[0].contourf(xTestCart, yTestCart, cTestCart)
axarr[1].contourf(xTestPol, yTestPol, cTestPol)
axarr[0].set_title("Cartesian Gaussians")
axarr[1].set_title("Polar Gaussians")
axarr[0].set_xlabel("X")
axarr[0].set_ylabel("Y")
axarr[1].set_xlabel("Radius (r)")
axarr[1].set_ylabel("Angle (theta)")
plt.show()
'''
def getNextPose(self, belief, obs=None, copPoses=None):
belief.display()
#Late hack to save everything
self.allBels.append(belief)
# f =open(os.path.dirname(__file__) + self.fileName,'w');
# print('^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^')
# print('Saving Beliefs at:');
# print(f);
# print('^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^')
# print(os.getcwd());
# np.save(f,self.allBels);
print('GETTING NEW POSE')
#0. update belief
newBel = self.beliefUpdate(belief, obs, copPoses)
#1. partition means into separate GMs, 1 for each room
allBels = []
weightSums = []
for room in self.map_.rooms:
tmp = GM()
tmpw = 0
for g in belief:
m = [g.mean[2], g.mean[3]]
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
tmp.normalizeWeights()
allBels.append(tmp)
weightSums.append(tmpw)
#2. find action from upper level pomdp
[room, questsHigh, weightsHigh] = self.getUpperAction(weightSums)
# print(questsHigh);
# print(weightsHigh);
questHighConversion = [5, 4, 0, 2, 3, 1]
questHighNew = []
for i in range(0, len(questsHigh)):
questHighNew.append(questHighConversion[int(questsHigh[i])])
questsHigh = questHighNew
questHighNew = []
for i in range(0, len(questsHigh)):
questHighNew.append([questsHigh[i], 0])
questsHigh = questHighNew
#3. find position and questions from lower level pomdp for that room
# DO NOT COMMENT THESE LINES !!!!!!!!!!!!!!
roomConversion = [5, 4, 0, 2, 3, 1]
room_conv = roomConversion[room]
# room_conv = room;
[movement, questsLow,
weightsLow] = self.getLowerAction(belief, room_conv)
# TODO: Fake Questions and goal pose
# goal_pose = allBels[room].findMAPN();
# goal_pose = [goal_pose[2],goal_pose[3]];
pose = copPoses[-1]
roomCount = 0
copRoom = 7
for room in self.map_.rooms:
if (pose[0] < self.map_.rooms[room]['max_x']
and pose[0] > self.map_.rooms[room]['min_x']
and pose[1] < self.map_.rooms[room]['max_y']
and pose[1] > self.map_.rooms[room]['min_y']):
copRoom = self.rooms_map_inv[room]
break
roomCount += 1
if (copRoom == room_conv):
dela = 1.0
displacement = [dela, 0, 0]
if (movement == 0):
displacement = [-dela, 0, 0]
elif (movement == 1):
displacement = [dela, 0, 0]
elif (movement == 2):
displacement = [0, dela, 0]
elif (movement == 3):
displacement = [0, -dela, 0]
goal_pose = np.array(copPoses[-1]) + np.array(displacement)
goal_pose = goal_pose.tolist()
else:
xpose = (self.map_.rooms[self.rooms_map[room_conv]]['max_x'] +
self.map_.rooms[self.rooms_map[room_conv]]['min_x']) / 2
ypose = (self.map_.rooms[self.rooms_map[room_conv]]['max_y'] +
self.map_.rooms[self.rooms_map[room_conv]]['min_y']) / 2
goal_pose = [xpose, ypose, 0]
for i in range(0, len(questsLow)):
questsLow[i] = [room_conv, questsLow[i] + 1]
#questsLow = [18,43,21,33,58];
#weightsLow = [24,54,23,48,53];
#questsLow = [];
#weightsLow = [];
suma = sum(weightsLow)
for i in range(0, len(weightsLow)):
weightsLow[i] = weightsLow[i] / suma
#4. weight questions accordingly
h = []
for i in range(0, len(questsHigh)):
h.append([weightsHigh[i], questsHigh[i]])
for i in range(0, len(questsLow)):
h.append([weightsLow[i], questsLow[i]])
# print('H: {}'.format(h))
h = sorted(h, key=self.getKey, reverse=True)
questsFull = []
weightsFull = []
print("HHHHHHHHHHHHHHHHHHHHHHHHHHHHHH")
for i in h:
questsFull.append(i[1])
weightsFull.append(i[0])
# print(len(self.question_list))
# print(questsFull)
# print(len(questsFull))
#5. use questioner function to publish questions
#questions = self.getQuestionStrings(questsFull);
questions = []
for i in range(0, len(questsFull)):
#print questsFull
#print i
#print questsFull[i][0]
#print questsFull[i][1]
try:
questions.append(
self.question_list[questsFull[i][0]][questsFull[i][1]])
except:
continue
#5.1 Make new belief map with goal_pose on it
pose = copPoses[-1]
#print("MAP COP POSE TO PLOT: {}".format(pose))
self.makeBeliefMap(newBel, pose, goal_pose)
#Save the belief map, save the observations that led to it
#For belief map, copy the tmp belief file
#For the observations, grab the strings
#6. return new belief and goal pose
return [newBel, goal_pose, [questions, questsFull]]
def oldbeliefUpdate(self, belief, responses=None, copPoses=None):
print('UPDATING BELIEF')
#1. partition means into separate GMs, 1 for each room
allBels = []
allBounds = []
copBounds = []
weightSums = []
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
tmp.normalizeWeights()
allBels.append(tmp)
weightSums.append(tmpw)
pose = copPoses[-1]
roomCount = 0
copBounds = 0
for room in self.map_.rooms:
if (pose[0] < self.map_.rooms[room]['max_x']
and pose[0] > self.map_.rooms[room]['min_x']
and pose[1] < self.map_.rooms[room]['max_y']
and pose[1] > self.map_.rooms[room]['min_y']):
copBounds = self.rooms_map_inv[room]
roomCount += 1
viewCone = Softmax()
viewCone.buildTriView(pose, length=1, steepness=10)
for i in range(0, len(viewCone.weights)):
viewCone.weights[i] = [
0, 0, viewCone.weights[i][0], viewCone.weights[i][1]
]
#Only update room that cop is in with view cone update
#Make sure to renormalize that room
# newerBelief = GM();
# for i in range(1,5):
# tmpBel = viewCone.runVBND(allBels[copBounds],i);
# newerBelief.addGM(tmpBel);
# allBels[copBounds] = newerBelief;
#Update all rooms
for i in range(0, len(allBels)):
newerBelief = GM()
for j in range(1, 5):
tmpBel = viewCone.runVBND(allBels[i], j)
if (j == 1):
tmpBel.scalerMultiply(.8)
newerBelief.addGM(tmpBel)
allBels[i] = newerBelief
for i in range(0, len(allBels)):
allBels[i].normalizeWeights()
#allBels[copBounds].normalizeWeights();
print('allBels LENGTH: {}'.format(len(allBels)))
#2. use queued observations to update appropriate rooms GM
if (responses is not None):
for res in responses:
roomNum = res[0]
mod = res[1]
clas = res[2]
sign = res[3]
if (roomNum == 0):
#apply to all
for i in range(0, len(allBels)):
if (sign == True):
allBels[i] = mod.runVBND(allBels[i], 0)
else:
tmp = GM()
for j in range(1, mod.size):
tmp.addGM(mod.runVBND(allBels[i], j))
allBels[i] = tmp
# else:
# print('ROOM NUM: {}'.format(roomNum))
# #apply to roomNum-1;
# if(sign == True):
# allBels[roomNum-1] = mod.runVBND(allBels[roomNum-1],clas);
# else:
# tmp = GM();
# for i in range(1,mod.size):
# if(i!=clas):
# tmp.addGM(mod.runVBND(allBels[roomNum-1],i));
# allBels[roomNum-1] = tmp;
else:
print('ROOM NUM: {}'.format(roomNum))
#apply to all rooms
for i in range(0, len(allBels)):
if (sign == True):
allBels[i] = mod.runVBND(allBels[i], clas)
else:
tmp = GM()
for j in range(1, mod.size):
if (j != clas):
tmp.addGM(mod.runVBND(allBels[i], j))
allBels[i] = tmp
#2.5. Make sure all GMs stay within their rooms bounds:
#Also condense each mixture
for gm in allBels:
for g in gm:
g.mean[2] = max(g.mean[2],
allBounds[allBels.index(gm)][0] - 0.01)
g.mean[2] = min(g.mean[2],
allBounds[allBels.index(gm)][2] + 0.01)
g.mean[3] = max(g.mean[3],
allBounds[allBels.index(gm)][1] - 0.01)
g.mean[3] = min(g.mean[3],
allBounds[allBels.index(gm)][3] + 0.01)
for i in range(0, len(allBels)):
allBels[i].condense(15)
# allBels[i] = allBels[i].kmeansCondensationN(6)
#3. recombine beliefs
newBelief = GM()
for g in allBels:
g.scalerMultiply(weightSums[allBels.index(g)])
newBelief.addGM(g)
newBelief.normalizeWeights()
#4. fix cops position in belief
for g in newBelief:
g.mean = [copPoses[0][0], copPoses[0][1], g.mean[2], g.mean[3]]
g.var[0][0] = 0.1
g.var[0][1] = 0
g.var[1][0] = 0
g.var[1][1] = 0.1
#5. add uncertainty for robber position
for g in newBelief:
g.var[2][2] += 0
g.var[3][3] += 0
# newBelief.normalizeWeights();
if copPoses is not None:
pose = copPoses[len(copPoses) - 1]
print("MAP COP POSE TO PLOT: {}".format(pose))
self.makeBeliefMap(newBelief, pose)
return newBelief
def beliefUpdate(self, belief, responses=None, copPoses=None):
# #Create Cop View Cone
# pose = copPoses[-1];
# viewCone = Softmax();
# viewCone.buildTriView(pose,length=1,steepness=10);
# for i in range(0,len(viewCone.weights)):
# viewCone.weights[i] = [0,0,viewCone.weights[i][0],viewCone.weights[i][1]];
#Update Cop View Cone
# newerBelief = GM();
# for j in range(1,5):
# tmpBel = viewCone.runVBND(belief,j);
# if(j==1):
# tmpBel.scalerMultiply(.4);
# newerBelief.addGM(tmpBel);
#Dont Update Cop View Cone
#newerBelief = belief
#4. update cops position to current position
for g in belief:
g.mean = [copPoses[-1][0], copPoses[-1][1], g.mean[2], g.mean[3]]
g.var[0][0] = 0.1
g.var[0][1] = 0
g.var[1][0] = 0
g.var[1][1] = 0.1
#5. update belief with robber dynamics
for g in belief:
g.var[2][2] += 0.03
g.var[3][3] += 0.03
#Distance Cutoff
#How many standard deviations away from the cop should gaussians be updated with view cone?
distCut = 2
#Update Cop View Cone Using LWIS
newerBelief = GM()
for pose in copPoses:
#Create Cop View Cone
#pose = copPoses[-1];
viewCone = Softmax()
viewCone.buildTriView(pose, length=1, steepness=10)
for i in range(0, len(viewCone.weights)):
viewCone.weights[i] = [
0, 0, viewCone.weights[i][0], viewCone.weights[i][1]
]
for g in belief:
#If the gaussian is suffciently close to the pose
#based on mahalanobis distance.
#Logic: M-dist basically says how many standard devs away the point is from the mean
#If it's more than distCut, it should be left alone
gprime = Gaussian()
gprime.mean = [g.mean[2], g.mean[3]]
gprime.var = [[g.var[2][2], g.var[2][3]],
[g.var[3][2], g.var[3][3]]]
gprime.weight = g.weight
#print(gprime.mean,gprime.mahalanobisDistance([pose[0]-np.cos(pose[2])*.5,pose[1]-np.sin(pose[2])*.5]));
if (gprime.mahalanobisDistance([
pose[0] - np.cos(pose[2]) * .5,
pose[1] - np.sin(pose[2]) * .5
]) <= distCut):
newG = viewCone.lwisUpdate(g, 0, 500, inverse=True)
newerBelief.addG(newG)
else:
newerBelief.addG(g)
#after each bayes update for the view cone, re-normalize
#Just to be sure, it never hurts to check
newerBelief.normalizeWeights()
# newerBelief= belief
#Update From Responses
if (responses is not None):
for res in responses:
roomNum = res[0]
mod = res[1]
clas = res[2]
sign = res[3]
if (roomNum == 0):
#apply to all
if (sign == True):
newerBelief = mod.runVBND(newerBelief, 0)
else:
tmp = GM()
for j in range(1, mod.size):
tmp.addGM(mod.runVBND(newerBelief, j))
newerBelief = tmp
else:
print('ROOM NUM: {}'.format(roomNum))
#apply to all rooms
if (sign == True):
newerBelief = mod.runVBND(newerBelief, clas)
else:
tmp = GM()
for j in range(1, mod.size):
if (j != clas):
tmp.addGM(mod.runVBND(newerBelief, j))
newerBelief = tmp
#Each response recieves a full bayes update, so we need to normalize each time
newerBelief.normalizeWeights()
#Condense the belief
newerBelief.condense(15)
print("*********************")
print(newerBelief.size)
print("*********************")
#Make sure there is a belief in each room
#A bit of a hack, but if this isn't here the lower level query fails
# for room in self.map_.rooms:
# 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();
# newerBelief.addG(Gaussian([0,0,centx,centy],var,0.00001));
#3. recombine beliefs (if we're still doing that sort of thing)
newBelief = newerBelief
newBelief.normalizeWeights()
#Moved to before observation updates
# #4. update cops position to current position
# for g in newBelief:
# g.mean = [copPoses[-1][0],copPoses[-1][1],g.mean[2],g.mean[3]];
# g.var[0][0] = 0.1;
# g.var[0][1] = 0;
# g.var[1][0] = 0;
# g.var[1][1] = 0.1;
# #5. update belief with robber dynamics
# for g in newBelief:
# g.var[2][2] += 0.05;
# g.var[3][3] += 0.05;
print("*********************")
print(newBelief.size)
print("*********************")
return newBelief
def runFactoredSim(self):
numSteps = 50
initialPose = [8,2];
b = GM();
'''
mean = [0]*len(self.delA[0]);
var = [[0 for k in range(0,len(self.delA[0]))] for j in range(0,len(self.delA[0]))];
for k in range(0,len(self.delA[0])):
mean[k] = random.random()*(self.bounds[k][1]-self.bounds[k][0]) + self.bounds[k][0];
var[k][k] = random.random()*10;
b.addG(Gaussian(mean,var,0.5));
'''
b.addG(Gaussian([6,3],[[4,0],[0,4]],6));
b.addG(Gaussian([.8,3.2],[[1,0],[0,1]],1));
b.addG(Gaussian([3,4.5],[[1,0],[0,1]],1));
b.addG(Gaussian([3,1.5],[[1,0],[0,1]],1));
b.normalizeWeights();
#Setup data gathering
x = initialPose;
allX = [];
allX.append(x);
allXInd = [0]*len(self.delA[0]);
for i in range(0,len(self.delA[0])):
allXInd[i] = [x[i]];
reward = 0;
allReward = [0];
allB = [];
allB.append(b);
allAct = [];
fig,ax = plt.subplots();
#Simulate
for count in range(0,numSteps):
if(self.exitFlag):
break;
if(self.distance(1,3,x[0],x[1]) < 1):
print('Robber Found');
break;
plt.cla();
[xxx,yyy,ccc] = b.plot2D(low=[0,0],high=[10,5],vis=False);
plt.gca().add_patch(Rectangle([2,2],1,2,fc='sandybrown'));
plt.gca().add_patch(Circle([1,3],0.15,fc='r'));
ax.contourf(xxx,yyy,ccc);
'''
#Get action
if(greedy):
act = self.getGreedyAction(b);
elif(belGen):
act = random.randint(0,len(self.delA)-1);
else:
act = self.getAction(b);
'''
act = self.getAction(b);
#Take action
x = np.random.multivariate_normal(np.array(x)-np.array(self.delA[act[0]]),self.delAVar,size =1)[0].tolist();
#bound the movement
for i in range(0,len(x)):
x[i] = max(self.bounds[i][0],x[i]);
x[i] = min(self.bounds[i][1],x[i]);
'''
#Get observation and update belief
if(not self.useSoft):
ztrial = [0]*len(self.pz);
for i in range(0,len(self.pz)):
ztrial[i] = self.pz[i].pointEval(x);
z = ztrial.index(max(ztrial));
b = self.beliefUpdate(b,act,z);
else:
ztrial = [0]*self.pz.size;
for i in range(0,self.pz.size):
ztrial[i] = self.pz.pointEval2D(i,x);
z = ztrial.index(max(ztrial));
b = self.beliefUpdateSoftmax(b,act,z);
'''
#fig,axarr = plt.subplots(2);
#[x,y,c] = b.plot2D(low=[0,0],high=[10,5],)
'''
if(self.distance(1,3,x[0],x[1]) > 1):
for i in range(1,4):
b = self.pz2.runVBND(b,i);
b = self.beliefUpdateSoftmax(b,act[0],4);
else:
'''
b = self.beliefUpdateSoftmax(b,act[0],2);
#Handle Question response
questionHandles = ['left of','right of', 'in front of', 'behind'];
question = 'Am I ' + questionHandles[act[1]] + ' the table?';
ax.scatter(x[0],x[1],c='k');
plt.pause(0.5);
humanInput = raw_input(question);
if('y' in humanInput or 'Y' in humanInput):
b = self.pz.runVBND(b,act[1]+1);
else:
for i in range(0,4):
if(i!=act[1]):
b = self.pz.runVBND(b,i+1);
b.normalizeWeights();
#save data
allB.append(b);
allX.append(x);
allAct.append(act);
for i in range(0,len(x)):
allXInd[i].append(x[i]);
reward += self.r[act[0]].pointEval(x);
allReward.append(reward);
allAct.append(-1);
#print("Simulation Complete. Accumulated Reward: " + str(reward));
return [allB,allX,allXInd,allAct,allReward];
def buildRectangleModel():
#Specify the lower left and upper right points
recBounds = [[2, 2], [3, 4]]
#recBounds = [[1,1],[8,4]];
B = np.matrix([
-1, 0, recBounds[0][0], 1, 0, -recBounds[1][0], 0, 1, -recBounds[1][1],
0, -1, recBounds[0][1]
]).T
M = np.zeros(shape=(12, 15))
#Boundry: Left|Near
rowSB = 0
classNum1 = 1
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Right|Near
rowSB = 1
classNum1 = 2
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Up|Near
rowSB = 2
classNum1 = 3
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
#Boundry: Down|Near
rowSB = 3
classNum1 = 4
classNum2 = 0
for i in range(0, 3):
M[3 * rowSB + i, 3 * classNum2 + i] = -1
M[3 * rowSB + i, 3 * classNum1 + i] = 1
A = np.hstack((M, B))
#print(np.linalg.matrix_rank(A))
#print(np.linalg.matrix_rank(M))
Theta = linalg.lstsq(M, B)[0].tolist()
weight = []
bias = []
for i in range(0, len(Theta) // 3):
weight.append([Theta[3 * i][0], Theta[3 * i + 1][0]])
bias.append(Theta[3 * i + 2][0])
steep = 1
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
pz = Softmax(weight, bias)
#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 getNextPose(self, belief, obs=None, copPoses=None):
#print('GETTING NEW POSE')
#0. update belief
newBel = self.beliefUpdate(belief, obs, copPoses)
#1. partition means into separate GMs, 1 for each room
allBels = []
weightSums = []
for room in self.map2.rooms:
tmp = GM()
tmpw = 0
for g in belief:
m = [g.mean[2], g.mean[3]]
if (m[0] <= self.map2.rooms[room]['upper_r'][0]
and m[0] >= self.map2.rooms[room]['lower_l'][0]
and m[1] <= self.map2.rooms[room]['upper_r'][1]
and m[1] >= self.map2.rooms[room]['lower_l'][1]):
tmp.addG(deepcopy(g))
tmpw += g.weight
tmp.normalizeWeights()
allBels.append(tmp)
weightSums.append(tmpw)
#2. find action from upper level pomdp
[room, questsHigh, weightsHigh] = self.getUpperAction(weightSums)
#print("Room: " + str(room));
# print(questsHigh);
# print(weightsHigh);
questHighConversion = [5, 4, 0, 2, 3, 1]
questHighNew = []
for i in range(0, len(questsHigh)):
questHighNew.append(questHighConversion[int(questsHigh[i])])
questsHigh = questHighNew
questHighNew = []
for i in range(0, len(questsHigh)):
questHighNew.append([questsHigh[i], 0])
questsHigh = questHighNew
#3. find position and questions from lower level pomdp for that room
roomConversion = [5, 4, 0, 2, 3, 1]
room_conv = roomConversion[room]
[movement, questsLow,
weightsLow] = self.getLowerAction(belief, room_conv)
# TODO: Fake Questions and goal pose
# goal_pose = allBels[room].findMAPN();
# goal_pose = [goal_pose[2],goal_pose[3]];
pose = copPoses[-1]
roomCount = 0
copRoom = 7
for room in self.map2.rooms:
if (pose[0] < self.map2.rooms[room]['upper_r'][0]
and pose[0] > self.map2.rooms[room]['lower_l'][0]
and pose[1] < self.map2.rooms[room]['upper_r'][1]
and pose[1] > self.map2.rooms[room]['lower_l'][1]):
copRoom = self.rooms_map_inv[room]
break
roomCount += 1
if (copRoom == room_conv):
displacement = [0, 0, 0]
dela = 1.0
if (movement == 0):
displacement = [-dela, 0, 0]
elif (movement == 1):
displacement = [dela, 0, 0]
elif (movement == 2):
displacement = [0, dela, 0]
elif (movement == 3):
displacement = [0, -dela, 0]
goal_pose = np.array(copPoses[-1]) + np.array(displacement)
goal_pose = goal_pose.tolist()
else:
# tmp = allBels[room_conv].findMAPN();
# xpose = tmp[0];
# ypose = tmp[1];
xpose = (
self.map2.rooms[self.rooms_map[room_conv]]['upper_r'][0] +
self.map2.rooms[self.rooms_map[room_conv]]['lower_l'][0]) / 2
ypose = (
self.map2.rooms[self.rooms_map[room_conv]]['upper_r'][1] +
self.map2.rooms[self.rooms_map[room_conv]]['lower_l'][1]) / 2
goal_pose = [xpose, ypose, 0]
for i in range(0, len(questsLow)):
questsLow[i] = [room_conv, questsLow[i] + 1]
#questsLow = [18,43,21,33,58];
#weightsLow = [24,54,23,48,53];
#questsLow = [];
#weightsLow = [];
suma = sum(weightsLow)
for i in range(0, len(weightsLow)):
weightsLow[i] = weightsLow[i] / suma
#4. weight questions accordingly
h = []
for i in range(0, len(questsHigh)):
h.append([weightsHigh[i], questsHigh[i]])
for i in range(0, len(questsLow)):
h.append([weightsLow[i], questsLow[i]])
# print('H: {}'.format(h))
h = sorted(h, key=self.getKey, reverse=True)
questsFull = []
weightsFull = []
for i in h:
questsFull.append(i[1])
weightsFull.append(i[0])
#5. use questioner function to publish questions
#questions = self.getQuestionStrings(questsFull);
questions = []
for i in range(0, len(questsFull)):
# print questsFull
# print i
# print questsFull[i][0]
# print questsFull[i][1]
questions.append(
self.question_list[questsFull[i][0]][questsFull[i][1]])
#6. return new belief and goal pose
return [newBel, goal_pose, [questions, questsFull]]
class Model:
def __init__(self,params,size = [437,754],trueModel = False,):
self.truth = trueModel
#Cop Pose
self.copPose = params['Model']['copInitPose'];
self.ROBOT_VIEW_RADIUS = params['Model']['robotViewRadius'];
self.ROBOT_SIZE_RADIUS = params['Model']['robotSizeRadius'];
self.ROBOT_NOMINAL_SPEED = params['Model']['robotNominalSpeed'];
self.TARGET_SIZE_RADIUS = params['Model']['targetSizeRadius'];
self.MAX_BELIEF_SIZE = params['Model']['numRandBel'];
self.BREADCRUMB_TRAIL_LENGTH = params['Model']['breadCrumbLength'];
self.history = {'beliefs':[],'positions':[],'sketches':{},'humanObs':[]};
belModel = params['Model']['belNum']
#Make Target or Belief
if(not self.truth):
if(belModel == 'None'):
self.belief = GM();
for i in range(0,self.MAX_BELIEF_SIZE):
self.belief.addNewG([np.random.randint(0,437),np.random.randint(0,754)],[[2000+500*np.random.normal(),0],[0,2000+500*np.random.normal()]],np.random.random());
self.belief.normalizeWeights();
else:
self.belief = np.load("../models/beliefs{}.npy".format(belModel))[0]
self.robPose = params['Model']['targetInitPose'];
self.bounds = {'low':[0,0],'high':[437,754]}
self.setupTransitionLayer();
self.setupCostLayer();
#TODO: Spatial Relations don't always map correctly, fix it....
#self.spatialRealtions = {'Near':0,'South of':4,'West of':1,'North of':2,'East of':3};
self.spatialRealtions = {'Near':0,'South of':1,'West of':2,'North of':3,'East of':4};
self.sketches = {};
self.prevPoses = [];
def setupCostLayer(self):
self.costLayer = np.zeros(shape=(self.bounds['high'][0],self.bounds['high'][1]));
x = self.robPose[0]
y = self.robPose[1];
for i in range(self.bounds['low'][0],self.bounds['high'][0]):
for j in range(self.bounds['low'][1],self.bounds['high'][1]):
if(not (i==x and y==j)):
self.costLayer[i,j] = 1/np.sqrt((x-i)*(x-i) + (y-j)*(y-j));
def setupTransitionLayer(self):
self.transitionLayer = np.zeros(shape=(self.bounds['high'][0],self.bounds['high'][1]));
if(self.truth):
self.transitionLayer = np.load('../models/trueTransitions.npy');
def transitionEval(self,x):
if(x[0] > self.bounds['low'][0] and x[1] > self.bounds['low'][1] and x[0] < self.bounds['high'][0] and x[1] < self.bounds['high'][1]):
return self.transitionLayer[x[0],x[1]];
else:
return -1e10;
def costEval(self,x):
if(x[0] > self.bounds['low'][0] and x[1] > self.bounds['low'][1] and x[0] < self.bounds['high'][0] and x[1] < self.bounds['high'][1]):
return self.rewardLayer[x[0],x[1]];
else:
return 0;
def distance(self,x,y):
return np.sqrt((x[0]-y[0])**2 + (x[1]-y[1])**2);
def makeSketch(self,vertices,name):
pz = Softmax();
vertices.sort(key=lambda x: x[1])
pz.buildPointsModel(vertices,steepness=2);
self.sketches[name] = pz;
def stateObsUpdate(self,name,relation,pos="Is"):
if(name == 'You'):
#Take Cops Position, builid box around it
cp=self.copPose;
points = [[cp[0]-5,cp[1]-5],[cp[0]+5,cp[1]-5],[cp[0]+5,cp[1]+5],[cp[0]-5,cp[1]+5]];
soft = Softmax()
soft.buildPointsModel(points,steepness=3);
else:
soft = self.sketches[name];
softClass = self.spatialRealtions[relation];
if(pos=="Is"):
self.belief = soft.runVBND(self.belief,softClass);
self.belief.normalizeWeights();
else:
tmp = GM();
for i in range(0,5):
if(i!=softClass):
tmp.addGM(soft.runVBND(self.belief,i));
tmp.normalizeWeights();
self.belief=tmp;
if(self.belief.size > self.MAX_BELIEF_SIZE):
self.belief.condense(self.MAX_BELIEF_SIZE);
self.belief.normalizeWeights()
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;
from gaussianMixtures import GM,Gaussian
import numpy as np;
numBels = 10;
for j in range(0,numBels):
belief = GM();
belief.addNewG([400,200],[[1000,0],[0,1000]],.25);
for i in range(0,20):
belief.addNewG([np.random.randint(0,437),np.random.randint(0,754)],[[2000+500*np.random.normal(),0],[0,2000+500*np.random.normal()]],np.random.random());
belief.normalizeWeights();
np.save("../models/beliefs{}.npy".format(j),[belief]);
