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]);