return Gamma
def getAction(b, Gamma):
act = Gamma[np.argmax([continuousDot(j, b) for j in Gamma])].action
return act
#Build models
[delA, modes, delAVar] = buildTransitions()
pz = buildObs()
r = buildRewards()
#Make belief
b = GM()
b.addNewG(-15, 1, 1)
act = 2
obs = 2
maxMix = 10
B = []
'''
B.append(b);
b = GM();
b.addNewG(4,1,1);
B.append(b);
b = GM();
b.addNewG(15,1,1);
B.append(b);
'''
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;
return Gamma; def getAction(b,Gamma): act = Gamma[np.argmax([continuousDot(j,b) for j in Gamma])].action; return act; #Build models [delA,modes,delAVar] = buildTransitions(); pz = buildObs(); r = buildRewards(); #Make belief b = GM(); b.addNewG(-15,1,1); act = 2; obs = 2; maxMix = 10; B = []; ''' B.append(b); b = GM(); b.addNewG(4,1,1); B.append(b); b = GM(); b.addNewG(15,1,1); B.append(b);
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]);
