def buildObs(self,gen=True):
if(gen):
self.pz = Softmax();
#dining table
#self.pz.buildOrientedRecModel([-8.5,-2.3],90,1.17,0.69,5);
dining = self.yamlFile['dining table'];
self.pz.buildOrientedRecModel([dining['centroid_x'],dining['centroid_y']],dining['orientation']+90,dining['x_len'],dining['y_len'],5);
for i in range(0,len(self.pz.weights)):
self.pz.weights[i] = [0,0,self.pz.weights[i][0],self.pz.weights[i][1]];
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/"+self.fileNamePrefix + "OBS.npy","w");
np.save(f,self.pz);
self.pz2 = Softmax();
f = open("../models/"+self.fileNamePrefix + "OBS2.npy","w");
np.save(f,self.pz2);
else:
self.pz = np.load("../models/"+self.fileNamePrefix + "OBS.npy").tolist();
self.pz2 = np.load("../models/"+self.fileNamePrefix+ "OBS2.npy").tolist();
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 fullPipeline():
#np.random.seed(5)
soft = Softmax()
points = generateSketch(verbosity=1)
soft.buildPointsModel(points, steepness=5)
joint, pclass, plabs = labelClasses(soft, points)
# labelClasses_Example(soft, points)
[x, y, c] = soft.plot2D(low=[0, 0], high=[10, 10], vis=False)
plt.contourf(x, y, c, cmap="Blues")
plt.figure()
matProb = np.zeros(shape=(soft.size, 9))
labs = ['East', 'NorthEast', 'North', 'NorthWest',
'West', 'SouthWest', 'South', 'SouthEast']
# For every class
for i in range(0, len(pclass)):
# normalize across pclass
c = pclass[i]
for j in range(0, len(labs)):
matProb[i, j+1] = c[labs[j]]
matProb[0, :] = 0
# matProb[0, 0] = 1
plt.imshow(matProb, cmap='inferno')
plt.xticks([0, 1, 2, 3, 4, 5, 6, 7, 8, 9], ['Inside', 'East', 'NorthEast',
'North', 'NorthWest', 'West', 'SouthWest', 'South', 'SouthEast'], rotation=90)
plt.show()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#dining table
self.pz.buildOrientedRecModel([-8.5, -2.3], 90, 1.17, 0.69, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#Vern
#self.pz.buildOrientedRecModel([-2.475,1.06],270,0.5,0.5,5);
#Desk
#self.pz.buildOrientedRecModel([-5.5,-2.0],0,0.61,0.99,5);
#bookcase
#self.pz.buildOrientedRecModel([0,-1.1662],270,0.38,0.18,5);
#checkers
#self.pz.buildOrientedRecModel([2.04,2.16],270,0.5,0.5,5);
#Fridge
self.pz.buildOrientedRecModel([-9.1, 3.07], 315, 0.46, 0.46, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open(self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#filing cabinet
#self.pz2.buildOrientedRecModel([-3.8638,-1.3262],270,0.5,.37,5);
#chair
#self.pz2.buildOrientedRecModel([2.975,-2.435],90,0.46,0.41,5);
#cassini
#self.pz2.buildOrientedRecModel([1.38,3.475],270,0.05,0.56,5);
#mars
self.pz2.buildOrientedRecModel([-4.38, 3.475], 270, 0.05, 0.84, 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open(self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load(self.fileNamePrefix + "OBS.npy").tolist()
self.pz2 = np.load(self.fileNamePrefix + "OBS2.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#checkers
#self.pz.buildOrientedRecModel([2.04,2.16],270,0.5,0.5,5);
checkers = self.yamlFile['checkers table']
self.pz.buildOrientedRecModel(
[checkers['centroid_x'], checkers['centroid_y']],
checkers['orientation'] + 90, checkers['x_len'],
checkers['y_len'], 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#cassini
#self.pz2.buildOrientedRecModel([1.38,3.475],270,0.05,0.56,5);
cassini = self.yamlFile['cassini poster']
self.pz2.buildOrientedRecModel(
[cassini['centroid_x'], cassini['centroid_y']],
cassini['orientation'] + 90, cassini['x_len'],
cassini['y_len'], 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#Fridge
#self.pz.buildOrientedRecModel([-9.1,3.07],315,0.46,0.46,5);
fridge = self.yamlFile['fridge']
self.pz.buildOrientedRecModel(
[fridge['centroid_x'], fridge['centroid_y']],
fridge['orientation'] + 90, fridge['x_len'], fridge['y_len'],
5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#mars
#self.pz2.buildOrientedRecModel([-4.38,3.475],270,0.05,0.84,5);
mars = self.yamlFile['mars poster']
self.pz2.buildOrientedRecModel(
[mars['centroid_x'], mars['centroid_y']],
mars['orientation'] + 90, mars['x_len'], mars['y_len'], 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#bookcase
#self.pz.buildOrientedRecModel([0,-1.1662],270,0.38,0.18,5);
bookcase = self.yamlFile['bookcase']
self.pz.buildOrientedRecModel(
[bookcase['centroid_x'], bookcase['centroid_y']],
bookcase['orientation'] + 90, bookcase['x_len'],
bookcase['y_len'], 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#chair
#self.pz2.buildOrientedRecModel([2.975,-2.435],90,0.46,0.41,5);
chair = self.yamlFile['chair']
self.pz2.buildOrientedRecModel(
[chair['centroid_x'], chair['centroid_y']],
chair['orientation'] + 90, chair['x_len'], chair['y_len'], 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildObs(self,gen=True):
if(gen):
self.pz = Softmax();
#Desk
#self.pz.buildOrientedRecModel([-5.5,-2.0],0,0.61,0.99,5);
desk = self.yamlFile['desk'];
self.pz.buildOrientedRecModel([desk['centroid_x'],desk['centroid_y']],desk['orientation']+90,desk['x_len'],desk['y_len'],5);
for i in range(0,len(self.pz.weights)):
self.pz.weights[i] = [0,0,self.pz.weights[i][0],self.pz.weights[i][1]];
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/"+self.fileNamePrefix + "OBS.npy","w");
np.save(f,self.pz);
self.pz2 = Softmax();
#filing cabinet
#self.pz2.buildOrientedRecModel([-3.8638,-1.3262],270,0.5,.37,5);
filing = self.yamlFile['filing cabinet'];
self.pz2.buildOrientedRecModel([filing['centroid_x'],float(filing['centroid_y'])],filing['orientation']+90,filing['x_len'],filing['y_len'],5);
for i in range(0,len(self.pz2.weights)):
self.pz2.weights[i] = [0,0,self.pz2.weights[i][0],self.pz2.weights[i][1]];
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/"+self.fileNamePrefix + "OBS2.npy","w");
np.save(f,self.pz2);
else:
self.pz = np.load("../models/"+self.fileNamePrefix + "OBS.npy").tolist();
self.pz2 = np.load("../models/"+self.fileNamePrefix+ "OBS2.npy").tolist();
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 = [-150,-100,0]; bias = [590,400,0]; softClass = 0; low = 0; high = 5; res = 100; #Define Likelihood Model pz = Softmax(weight,bias); #pz.plot1D(high = 5,low = 0); return pz;
def __init__(self, params, points, seed=int(1)):
if (seed is not None):
np.random.seed(seed)
# self.name = params['name']
# self.centroid = params['centroid']
self.points = points
self.inflated = self.inflatePoints(params)
self.labels = [
'East', 'NorthEast', 'North', 'NorthWest', 'West', 'SouthWest',
'South', 'SouthEast'
]
self.sm = Softmax()
self.sm.buildPointsModel(self.points, steepness=params['steepness'])
self.sm_inf = Softmax()
self.sm_inf.buildPointsModel(self.inflated,
steepness=params['steepness'])
[self.joint, self.con_class, self.con_label] = self.labelClasses()
def makeModel():
#weight = [-10,0];
#bias = [10,0];
weight = [-3, -2, -1, 0]
bias = [6, 5, 3, 0]
a = Softmax(weight, bias)
return a
def buildObs(self, gen=True):
#cardinal + 1 model
#left,right,up,down,near
if (gen):
weight = [[0, 1], [-1, 1], [1, 1], [0, 2], [0, 0]]
bias = [1, 0, 0, 0, 0]
steep = 0.2
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
self.pz = Softmax(weight, bias)
print('Plotting Observation Model')
self.pz.plot2D(low=[-10, -10], high=[10, 10], vis=True)
f = open("../models/obs/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
else:
self.pz = np.load("../models/obs/" + self.fileNamePrefix +
"OBS.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#Desk
self.pz.buildOrientedRecModel([-5.5, -2.0], 0, 0.61, 0.99, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#filing cabinet
self.pz2.buildOrientedRecModel([-3.8638, -1.3262], 270, 0.5, .37,
5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildObs(self, gen=True):
#cardinal + 1 model
#left,right,up,down,near
if (gen):
self.pz = Softmax()
self.pz.buildRectangleModel([[2, 2], [3, 4]], 1)
print('Plotting Observation Model')
self.pz.plot2D(low=[0, 0], high=[10, 5], vis=True)
f = open(self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
self.pz2.buildRectangleModel([[.75, 2.75], [1.25, 3.25]], 2)
f = open(self.fileNamePrefix + "2OBS.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load(self.fileNamePrefix + "OBS.npy").tolist()
self.pz2 = np.load(self.fileNamePrefix + "2OBS.npy").tolist()
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#checkers
self.pz.buildOrientedRecModel([2.04, 2.16], 270, 0.5, 0.5, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#cassini
self.pz2.buildOrientedRecModel([1.38, 3.475], 270, 0.05, 0.56, 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
def buildRecFromCentroidOrient():
centroid = [5, 4]
orient = 0
length = 2
width = 4
theta1 = orient * math.pi / 180
h = math.sqrt((width / 2) * (width / 2) + (length / 2) * (length / 2))
theta2 = math.asin((width / 2) / h)
s1 = h * math.sin(theta1 + theta2)
s2 = h * math.cos(theta1 + theta2)
s3 = h * math.sin(theta1 - theta2)
s4 = h * math.cos(theta1 - theta2)
pz = Softmax()
points = []
points = [[centroid[0] + s2, centroid[1] + s1],
[centroid[0] + s4, centroid[1] + s3],
[centroid[0] - s2, centroid[1] - s1],
[centroid[0] - s4, centroid[1] - s3]]
for p in points:
plt.scatter(p[0], p[1])
plt.show()
pz.buildPointsModel(points, steepness=5)
pz.plot2D(low=[0, 0], high=[10, 10])
def buildObs(self,gen=True):
#cardinal + 1 model
#left,right,up,down,near
if(gen):
self.pz = Softmax();
self.pz.buildOrientedRecModel([4,4.75],270,.5,2,5);
for i in range(0,len(self.pz.weights)):
self.pz.weights[i] = [0,0,self.pz.weights[i][0],self.pz.weights[i][1]];
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open(self.fileNamePrefix + "OBS.npy","w");
np.save(f,self.pz);
else:
self.pz = np.load(self.fileNamePrefix + "OBS.npy").tolist();
def measurementUpdate(meas, sm, s, curs, goals):
origLen = len(s)
s = np.array(s)
curs = np.array(curs)
goals = np.array(goals)
sm = Softmax()
# sm.buildRectangleModel([[1,5],[3,7]],steepness=7);
sm.buildOrientedRecModel([2000, -2000], 0, 1, 1, steepness=7)
weights = [sm.pointEvalND(meas, s[i]) for i in range(0, len(s))]
weights /= np.sum(weights)
csum = np.cumsum(weights)
csum[-1] = 1
indexes = np.searchsorted(csum, np.random.random(len(s)))
s[:] = s[indexes]
curs[:] = curs[indexes]
goals[:] = goals[indexes]
return s, curs, goals
def buildObs(self,gen=True):
if(gen):
self.pz = Softmax();
#Vern
self.pz.buildOrientedRecModel([-2.475,1.06],270,0.5,0.5,5);
for i in range(0,len(self.pz.weights)):
self.pz.weights[i] = [0,0,self.pz.weights[i][0],self.pz.weights[i][1]];
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/"+self.fileNamePrefix + "OBS.npy","w");
np.save(f,self.pz);
self.pz2 = Softmax();
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/"+self.fileNamePrefix + "OBS2.npy","w");
np.save(f,self.pz2);
else:
self.pz = np.load("../models/"+self.fileNamePrefix + "OBS.npy").tolist();
self.pz2 = np.load("../models/"+self.fileNamePrefix+ "OBS2.npy").tolist();
def stretch1DModel():
steep = 5
weight = (np.array([-3, -2, -1, 0]) * steep).tolist()
bias = (np.array([6, 5, 3, 0]) * steep).tolist()
low = 0
high = 5
res = 100
#Define Likelihood Model
a = Softmax(weight, bias)
a.plot1D(low=low, high=high)
#weight = (np.array([[-1,0],[0,0]])*steep).tolist();
#bias = (np.array([3,0])*steep).tolist();
for i in range(0, len(weight)):
weight[i] = [weight[i], 0]
b = Softmax(weight, bias)
b.plot2D(low=[0, 0], high=[5, 5])
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 buildTriView():
pose = [2, 1, 165]
l = 3
#Without Cutting
triPoints = [
[pose[0], pose[1]],
[
pose[0] + l * math.cos(2 * -0.261799 + math.radians(pose[2])),
pose[1] + l * math.sin(2 * -0.261799 + math.radians(pose[2]))
],
[
pose[0] + l * math.cos(2 * 0.261799 + math.radians(pose[2])),
pose[1] + l * math.sin(2 * 0.261799 + math.radians(pose[2]))
]
]
#With Cutting
lshort = 0.5
triPoints = [
[
pose[0] + lshort * math.cos(2 * 0.261799 + math.radians(pose[2])),
pose[1] + lshort * math.sin(2 * 0.261799 + math.radians(pose[2]))
],
[
pose[0] + lshort * math.cos(2 * -0.261799 + math.radians(pose[2])),
pose[1] + lshort * math.sin(2 * -0.261799 + math.radians(pose[2]))
],
[
pose[0] + l * math.cos(2 * -0.261799 + math.radians(pose[2])),
pose[1] + l * math.sin(2 * -0.261799 + math.radians(pose[2]))
],
[
pose[0] + l * math.cos(2 * 0.261799 + math.radians(pose[2])),
pose[1] + l * math.sin(2 * 0.261799 + math.radians(pose[2]))
]
]
pz = Softmax()
pz.buildPointsModel(triPoints, steepness=10)
pz.plot2D(low=[-10, -10], high=[10, 10])
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()
pairedPoints = drawShape(sketch)
print(pairedPoints)
#Get N Vertices of the shape
cHull = ConvexHull(pairedPoints)
vertices = fitSimplePolyToHull(cHull, pairedPoints, N=5)
#vertices = fitBestPolyToHull(cHull,pairedPoints);
#Show Vertices
plt.scatter([vertices[i][0] for i in range(0, len(vertices))],
[vertices[i][1] for i in range(0, len(vertices))])
plt.show()
#Make softmax model
pz = Softmax()
pz.buildPointsModel(vertices, steepness=5)
#Update belief
post = pz.runVBND(prior, 4)
#display
fig, axarr = plt.subplots(3)
[xprior, yprior, cprior] = prior.plot2D(low=[0, 0],
high=[10, 10],
vis=False)
[xobs, yobs, cobs] = pz.plot2D(low=[0, 0],
high=[10, 10],
delta=0.1,
vis=False)
[xpost, ypost, cpost] = post.plot2D(low=[0, 0], high=[10, 10], vis=False)
class ModelSpec:
def __init__(self):
self.fileNamePrefix = 'D4QuestSoftmax'
#Problem specific
def buildTransition(self):
#self.bounds = [[0,10],[0,5],[0,10],[0,5]];
#Hallway
#self.bounds = [[-9.5,4],[-1,1.4],[-9.5,4],[-1,1.4]];
#Dining Room
#self.bounds = [[-9.5,-7],[-3.33,-1],[-9.5,-7],[-3.33,-1]]
#Study
#self.bounds = [[-7,-2],[-3.33,-1],[-7,-2],[-3.33,-1]];
#Library
#self.bounds = [[-2,4],[-3.33,-1],[-2,4],[-3.33,-1]];
#Billiard
#self.bounds = [[0,4],[1.4,3.68],[0,4],[1.4,3.68]];
#Kitchen
self.bounds = [[-9.5, 0], [1.4, 3.68], [-9.5, 0], [1.4, 3.68]]
self.delAVar = (np.identity(4) * 1).tolist()
self.delAVar[0][0] = 0.00001
self.delAVar[1][1] = 0.00001
#self.delA = [[-0.5,0],[0.5,0],[0,-0.5],[0,0.5],[0,0],[-0.5,-0.5],[0.5,-0.5],[-0.5,0.5],[0.5,0.5]];
delta = 1
self.delA = [[-delta, 0, 0, 0], [delta, 0, 0, 0], [0, delta, 0, 0],
[0, -delta, 0, 0], [0, 0, 0, 0]]
self.discount = 0.95
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#Vern
#self.pz.buildOrientedRecModel([-2.475,1.06],270,0.5,0.5,5);
#Desk
#self.pz.buildOrientedRecModel([-5.5,-2.0],0,0.61,0.99,5);
#bookcase
#self.pz.buildOrientedRecModel([0,-1.1662],270,0.38,0.18,5);
#checkers
#self.pz.buildOrientedRecModel([2.04,2.16],270,0.5,0.5,5);
#Fridge
self.pz.buildOrientedRecModel([-9.1, 3.07], 315, 0.46, 0.46, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open(self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#filing cabinet
#self.pz2.buildOrientedRecModel([-3.8638,-1.3262],270,0.5,.37,5);
#chair
#self.pz2.buildOrientedRecModel([2.975,-2.435],90,0.46,0.41,5);
#cassini
#self.pz2.buildOrientedRecModel([1.38,3.475],270,0.05,0.56,5);
#mars
self.pz2.buildOrientedRecModel([-4.38, 3.475], 270, 0.05, 0.84, 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open(self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load(self.fileNamePrefix + "OBS.npy").tolist()
self.pz2 = np.load(self.fileNamePrefix + "OBS2.npy").tolist()
#Problem Specific
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()
#Need gaussians along the borders for positive and negative rewards
for i in range(0, len(self.r)):
for x1 in range(
int(np.floor(self.bounds[0][0])) - 1,
int(np.ceil(self.bounds[0][1])) + 1):
for y1 in range(
int(np.floor(self.bounds[1][0])) - 1,
int(np.ceil(self.bounds[1][1])) + 1):
for x2 in range(
int(np.floor(self.bounds[2][0])) - 1,
int(np.ceil(self.bounds[2][1])) + 1):
for y2 in range(
int(np.floor(self.bounds[3][0])) - 1,
int(np.ceil(self.bounds[3][1])) + 1):
if (np.sqrt((x1 - x2)**2 + (y1 - y2)**2) < 1):
self.r[i].addG(
Gaussian(
np.array(([x1, y1, x2, y2]) -
np.array(self.delA[i])).
tolist(), var, 10))
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()
class ModelSpec:
def __init__(self):
self.fileNamePrefix = 'D4QuestBilliardSoftmax'
#Problem specific
def buildTransition(self):
#Billiard
self.bounds = [[0, 4], [1.4, 3.68], [0, 4], [1.4, 3.68]]
self.delAVar = (np.identity(4) * 1).tolist()
self.delAVar[0][0] = 0.001
self.delAVar[1][1] = 0.001
#self.delA = [[-0.5,0],[0.5,0],[0,-0.5],[0,0.5],[0,0],[-0.5,-0.5],[0.5,-0.5],[-0.5,0.5],[0.5,0.5]];
delta = 1
self.delA = [[-delta, 0, 0, 0], [delta, 0, 0, 0], [0, delta, 0, 0],
[0, -delta, 0, 0], [0, 0, 0, 0]]
self.discount = 0.95
def buildObs(self, gen=True):
if (gen):
self.pz = Softmax()
#checkers
self.pz.buildOrientedRecModel([2.04, 2.16], 270, 0.5, 0.5, 5)
for i in range(0, len(self.pz.weights)):
self.pz.weights[i] = [
0, 0, self.pz.weights[i][0], self.pz.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS.npy", "w")
np.save(f, self.pz)
self.pz2 = Softmax()
#cassini
self.pz2.buildOrientedRecModel([1.38, 3.475], 270, 0.05, 0.56, 5)
for i in range(0, len(self.pz2.weights)):
self.pz2.weights[i] = [
0, 0, self.pz2.weights[i][0], self.pz2.weights[i][1]
]
#print('Plotting Observation Model');
#self.pz.plot2D(low=[0,0],high=[10,5],vis=True);
f = open("../models/" + self.fileNamePrefix + "OBS2.npy", "w")
np.save(f, self.pz2)
else:
self.pz = np.load("../models/" + self.fileNamePrefix +
"OBS.npy").tolist()
self.pz2 = np.load("../models/" + self.fileNamePrefix +
"OBS2.npy").tolist()
#Problem Specific
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 obs2models(self, obs, pose):
"""Map received observation to the appropriate softmax model and class.
Observation may be a str type with a pushed observation or a list with
question and answer.
"""
print(obs)
sign = None
model = None
model_name = None
room_num = None
class_idx = None
# check if observation is statement (str) or question (list)
if type(obs) is str:
# obs = obs.split()
if 'not' in obs:
sign = False
else:
sign = True
else:
sign = obs[1]
obs = obs[0]
# find map object mentioned in statement
for obj in self.map_.objects:
if re.search(obj, obs.lower()):
model = self.map_.objects[obj].softmax
model_name = self.map_.objects[obj].name
for i, room in enumerate(self.map_.rooms):
if obj in self.map_.rooms[room][
'objects']: # potential for matching issues if obj is 'the <obj>', as only '<obj>' will be found in room['objects']
room_num = i + 1
print self.map_.rooms[room]['objects']
print room_num
break
# if observation is relative to the cop
if re.search('cop', obs.lower()):
model = Softmax()
model.buildOrientedRecModel((pose[0], pose[1]),
pose[2] * 180 / np.pi,
0.5,
0.5,
steepness=2)
room_num = 1
for i in range(0, len(model.weights)):
model.weights[i] = [
0, 0, model.weights[i][0], model.weights[i][1]
]
# if no model is found, try looking for room mentioned in observation
if model is None:
for room in self.map_.rooms:
if re.search(room, obs.lower()):
model = self.map_.rooms[room]['softmax']
room_num = 0
break
# find softmax class index
if re.search('inside', obs.lower()) or re.search('in', obs.lower()):
class_idx = 0
if re.search('front', obs.lower()):
class_idx = 1
elif re.search('right', obs.lower()):
class_idx = 2
elif re.search('behind', obs.lower()):
class_idx = 3
elif re.search('left', obs.lower()):
class_idx = 4
# elif 'near' in obs:
# class_idx = 5
print(room_num, model, model_name, class_idx, sign)
return room_num, model, class_idx, sign
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