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 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()
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)
axarr[0].contourf(xprior, yprior, cprior, cmap='viridis')
axarr[1].contourf(xobs, yobs, cobs, cmap='inferno')
axarr[2].contourf(xpost, ypost, cpost, cmap='viridis')
plt.show()
#pz.plot2D(low=[0,0],high=[10,10],delta=0.1);
# fig,ax = plt.subplots();
# ax.contourf(xprior,yprior,cprior,cmap='viridis');
# ax.set_xlabel('X/East Location (m)');
# ax.set_ylabel('Y/West Location (m)');
# fig,ax = plt.subplots();
class Sketch:
def __init__(self, params, seed=None):
if(seed is not None):
np.random.seed(seed)
self.name = params['name']
self.centroid = params['centroid']
if(params['points'] is not None):
self.points = params['points'];
else:
self.points = self.generateSketch(params)
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 displayClasses(self, show=True):
fig = plt.figure()
[x, y, c] = self.sm.plot2D(low=[0, 0], high=[10, 10], vis=False)
[x_inf, y_inf, c_inf] = self.sm_inf.plot2D(
low=[0, 0], high=[10, 10], vis=False)
c_inf = np.array(c_inf)
c_inf[c_inf == 0] = 10
c_inf[c_inf < 10] = 0
plt.contourf(x_inf, y_inf, c_inf, cmap="Reds", alpha=1)
plt.contourf(x, y, c, cmap="Blues", alpha=0.5)
cid = fig.canvas.mpl_connect(
'button_press_event', self.onclick_classes)
if(show):
plt.show()
def onclick_classes(self, event):
# print('%s click: button=%d, x=%d, y=%d, xdata=%f, ydata=%f' %
# ('double' if event.dblclick else 'single', event.button,
# event.x, event.y, event.xdata, event.ydata))
print("Point Selected: [{:.2f},{:.2f}]".format(
event.xdata, event.ydata))
point = [event.xdata, event.ydata]
if(event.dblclick):
class_test = np.zeros(shape=(self.sm.size))
for i in range(0, len(class_test)):
class_test[i] = self.sm.pointEvalND(i, point)
# print(class_test)
ans = {}
for l in self.labels:
ans[l] = 0
for i in range(0, len(class_test)):
te = self.con_label[i]
ans[l] += self.con_label[i][l]*class_test[i]
suma = sum(ans.values())
for k in ans.keys():
ans[k] /= suma
print("Outputing all label probabilities:")
for k, v in ans.items():
if(v > 0.009):
print("P: {:0.2f}, L: {}".format(v, k))
else:
# Find just most likely class
# Check all softmax classes
# Check if it's near
near = ""
near_test = np.zeros(shape=(self.sm_inf.size))
for i in range(0, len(near_test)):
near_test[i] = self.sm_inf.pointEvalND(i, point)
if(np.argmax(near_test) == 0):
near = 'Near'
# Check other classes
class_test = np.zeros(shape=(self.sm.size))
for i in range(0, len(class_test)):
class_test[i] = self.sm.pointEvalND(i, point)
best = np.argmax(class_test)
if(best == 0):
near = ""
best_lab = "Inside"
else:
te = self.con_label[best]
best_lab = max(te, key=te.get)
print("Most Likely Class: {}".format(near + " " + best_lab))
print("")
def giveMostLikelyClass(self,point):
near = ""
near_test = np.zeros(shape=(self.sm_inf.size))
for i in range(0, len(near_test)):
near_test[i] = self.sm_inf.pointEvalND(i, point)
if(np.argmax(near_test) == 0):
near = 'Near'
# Check other classes
class_test = np.zeros(shape=(self.sm.size))
for i in range(0, len(class_test)):
class_test[i] = self.sm.pointEvalND(i, point)
best = np.argmax(class_test)
if(best == 0):
near = ""
best_lab = "Inside"
else:
te = self.con_label[best]
best_lab = max(te, key=te.get)
res = near + " " + best_lab;
return res
def giveProbabilities(self,point):
class_test = np.zeros(shape=(self.sm.size))
for i in range(1, len(class_test)):
class_test[i] = self.sm.pointEvalND(i, point)
# print(class_test)
ans = {}
for l in self.labels:
ans[l] = 0
for i in range(1, len(class_test)):
te = self.con_label[i]
ans[l] += self.con_label[i][l]*class_test[i]
ans['Inside'] = self.sm.pointEvalND(0,point);
suma = sum(ans.values())
for k in ans.keys():
ans[k] /= suma
return ans;
def giveNearProb(self,point):
near_test = np.zeros(shape=(self.sm_inf.size))
for i in range(0, len(near_test)):
near_test[i] = self.sm_inf.pointEvalND(i, point)
return near_test;
def answerQuestion(self,point,label,thresh = .8):
# res = self.giveMostLikelyClass(point)
# if(res == label):
##################################################
#TODO: Integrate Camera Positions
##################################################
probs = self.giveProbabilities(point);
maxi = max(probs.values());
for k in probs.keys():
probs[k] /= maxi;
if(label == "Near"):
nearProb = self.giveNearProb(point);
if(np.argmax(nearProb) == 0):
return 'Yes'
else:
return 'No'
elif("Near" in label):
spl = label.split();
nearProb = self.giveNearProb(point);
if(probs[spl[1]] > thresh and np.argmax(nearProb) == 0):
return 'Yes'
else:
return 'No'
elif(probs[label] > thresh):
return 'Yes';
else:
return 'No';
def displayProbTables(self, show=True):
plt.figure()
matProb = np.zeros(shape=(self.sm.size, len(self.labels)+1))
# For every class
for i in range(0, len(self.con_label)):
# normalize across self.con_label
c = self.con_label[i]
for j in range(0, len(self.labels)):
matProb[i, j+1] = c[self.labels[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.title("Conditional p(label | class)")
plt.ylabel("Softmax Class")
plt.colorbar()
plt.figure()
matProb = np.zeros(shape=(self.sm.size, len(self.labels)+1))
# For every class
for i in range(0, len(self.con_class)):
# normalize across self.con_class
c = self.con_class[i]
for j in range(0, len(self.labels)):
matProb[i, j+1] = c[self.labels[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.title("Conditional p(class | label)")
plt.ylabel("Softmax Class")
plt.colorbar()
if(show):
plt.show()
def displayPoints(self, show=True):
plt.figure()
plt.scatter(self.points[:, 0], self.points[:, 1])
if(show):
plt.show()
def generateSketch(self, params):
# given a point, spread rays out in random directions with semi random distance, and a random number of points
# Tuning Paramters
####################################
centroid = params['centroid']
dist_nom = params['dist_nom']
dist_noise = params['dist_noise']
angle_noise = params['angle_noise']
pois_mean = params['pois_mean']
####################################
# Fixed Parameters
####################################
# Has to be at least a triangle
pois_min = 3
####################################
# Chose Number of Points from Poisson Distribution
numVerts = np.random.poisson(pois_mean)+pois_min
# Debugging
# numVerts = min(numVerts, 5)
points = []
# Note: Angles must preserve order
totalAngle = 0
# Precompute angles
allAngles = np.zeros(numVerts)
for i in range(0, numVerts):
totalAngle += 2*np.pi/numVerts + np.random.normal(0, angle_noise)
allAngles[i] = totalAngle
# Normalize and expand to 2pi
allAngles /= totalAngle
allAngles *= 1.999*np.pi
for i in range(0, numVerts):
h = dist_nom + np.random.normal(0, dist_noise)
points.append([h*np.cos(allAngles[i])+centroid[0],
h*np.sin(allAngles[i])+centroid[1]])
points = np.array(points)
cHull = ConvexHull(points)
points = points[cHull.vertices]
return points
def inflatePoints(self, params):
# Tuning Paramters
####################################
area_multiplier = params['area_multiplier']
####################################
ske = Polygon(self.points)
ske2 = affinity.scale(ske, xfact=np.sqrt(
area_multiplier), yfact=np.sqrt(area_multiplier))
inflation = np.array(ske2.exterior.coords.xy).T
return inflation
def findLabels(self, point, centroid):
# point must first be normalized with respect to centroid
pprime = [point[0] - centroid.x, point[1] - centroid.y]
ang = np.arctan2(pprime[1], pprime[0])*180/np.pi
if(ang < 0):
ang += 360
allLabels = []
# Off-Axial
if(ang < 90):
allLabels.append("NorthEast")
elif(ang < 180):
allLabels.append("NorthWest")
elif(ang < 270):
allLabels.append("SouthWest")
elif(ang <= 360):
allLabels.append("SouthEast")
# On-Axial
if(ang < 45):
allLabels.append("East")
elif(ang < 135):
allLabels.append("North")
elif(ang < 225):
allLabels.append("West")
elif(ang < 315):
allLabels.append("South")
elif(ang < 360):
allLabels.append("East")
return allLabels
def labelClasses(self):
# Take a ring of points, around centroid of object
# For each point, identify it's cardinal direction, can be multiple levels
# For each class, add together eval at points into direction labels
# Normalize direction labels, and you have a soft classification of each class
# Maybe threshold during normalization
# Returns:
# Joint probability dirLabs
# Conditional p(class|labs)
# Conditional p(labs|class)
ske = Polygon(self.points)
# print(ske.centroid)
cent = ske.centroid
hfactor = 3
ra = hfactor*np.sqrt(ske.area/np.pi)
# Direction Percentages
dirLabs = []
for i in range(0, len(self.points)+1):
dirLabs.append({"West": 0, "East": 0, "North": 0, "South": 0,
"SouthWest": 0, "NorthWest": 0, "NorthEast": 0, "SouthEast": 0})
numDegrees = 360
testPoints = []
for i in range(0, numDegrees):
testPoints.append([ra*np.cos((i/numDegrees)*360 * np.pi/180)+cent.x,
ra*np.sin((i/numDegrees)*360 * np.pi/180)+cent.y])
# for i in range(0,10000):
# testPoints.append([np.random.random()*10,np.random.random()*10])
testPoints = np.array(testPoints)
suma = 0
# For each point
for t in testPoints:
# Find its labels
labs = self.findLabels(t, cent)
# Now apply to each class
for c in range(0, self.sm.size):
# eval
tmp = self.sm.pointEvalND(c, t)
for l in labs:
# add to dirLabs[c][l]
dirLabs[c][l] += tmp
suma += tmp
# Normalize dirlabs to obtain p(class,label)
for c in dirLabs:
for k in c.keys():
c[k] /= suma
# print(c)
cond_classes = deepcopy(dirLabs)
labs = self.labels
# For every class
for i in range(0, len(cond_classes)):
# normalize across labels
c = cond_classes[i]
suma = 0
for k, v in c.items():
suma += v
for key in c.keys():
c[key] /= suma
cond_labs = deepcopy(dirLabs)
for l in labs:
suma = 0
for c in cond_labs:
suma += c[l]
for c in cond_labs:
c[l] /= suma
return dirLabs, cond_classes, cond_labs
class ModelSpec:
def __init__(self):
self.fileNamePrefix = 'D2QuestSoftmax'
self.walls = []
#Problem specific
def buildTransition(self):
self.bounds = [[0, 10], [0, 5]]
self.delAVar = (np.identity(2) * 0.25).tolist()
#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 = 0.5
self.delA = [[-delta, 0], [delta, 0], [0, delta], [0, -delta], [0, 0]]
self.discount = 0.95
#set wall line segments
#self.walls = [[[2.5,-1],[2.5,2]],[[0,0],[0,5]],[[0,5],[5,5]],[[5,0],[5,5]],[[0,0],[5,0]]];
#self.walls = [];
#Problem Specific
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()
#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(2) * .25).tolist()
#Positive Rewards
for i in range(0, len(self.r)):
self.r[i].addG(
Gaussian([1 - self.delA[i][0], 3 - self.delA[i][1]], var,
30))
#Negative Rewards
for i in range(0, len(self.r)):
self.r[i].addG(
Gaussian([2.5 - self.delA[i][0], 2.5 - self.delA[i][1]],
var, -50))
self.r[i].addG(
Gaussian([2.5 - self.delA[i][0], 3 - self.delA[i][1]], var,
-50))
self.r[i].addG(
Gaussian([2.5 - self.delA[i][0], 3.5 - self.delA[i][1]],
var, -50))
print('Plotting Reward Model')
for i in range(0, len(self.r)):
self.r[i].plot2D(high=[10, 5],
low=[0, 0],
xlabel='Robot X',
ylabel='Robot Y',
title='Reward for action: ' + str(i))
print('Condensing Reward Model')
for i in range(0, len(self.r)):
self.r[i] = self.r[i].kmeansCondensationN(k=5)
print('Plotting Condensed Reward Model')
for i in range(0, len(self.r)):
#self.r[i].plot2D(xlabel = 'Robot X',ylabel = 'Robot Y',title = 'Reward for action: ' + str(i));
[x, y, c] = self.r[i].plot2D(high=[10, 5],
low=[0, 0],
vis=False)
minim = np.amin(c)
maxim = np.amax(c)
#print(minim,maxim);
levels = np.linspace(minim, maxim)
plt.contourf(x,
y,
c,
levels=levels,
vmin=minim,
vmax=maxim,
cmap='viridis')
plt.title('Reward for action: ' + str(i))
plt.xlabel('Robot X')
plt.ylabel('Robot Y')
plt.pause(0.5)
f = open(self.fileNamePrefix + "REW.npy", "w")
np.save(f, self.r)
else:
self.r = np.load(self.fileNamePrefix + "REW.npy").tolist()
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 slice3DModel():
steep = 1
dims = 3
#Trapezoidal Pyramid Specs
numClasses = 7
boundries = [[1, 0], [2, 0], [3, 0], [4, 0], [5, 0], [6, 0]]
B = np.matrix([
0, 0, -1, -2, -1, 0, .5, -2, 0, 1, .5, -2, 1, 0, .5, -2, 0, -1, .5, -2,
0, 0, 1, -1
]).T
'''
#Octohedron Specs
numClasses = 9;
boundries = [];
for i in range(1,numClasses):
boundries.append([i,0]);
B = np.matrix([-1,-1,0.5,-1,-1,1,0.5,-1,1,1,0.5,-1,1,-1,0.5,-1,-1,-1,-0.5,-1,-1,1,-0.5,-1,1,1,-0.5,-1,1,-1,-0.5,-1]).T;
'''
M = np.zeros(shape=(len(boundries) * (dims + 1), numClasses * (dims + 1)))
for j in range(0, len(boundries)):
for i in range(0, dims + 1):
M[(dims + 1) * j + i, (dims + 1) * boundries[j][1] + i] = -1
M[(dims + 1) * j + i, (dims + 1) * boundries[j][0] + i] = 1
A = np.hstack((M, B))
Theta = linalg.lstsq(M, B)[0].tolist()
weight = []
bias = []
for i in range(0, len(Theta) // (dims + 1)):
weight.append([
Theta[(dims + 1) * i][0], Theta[(dims + 1) * i + 1][0],
Theta[(dims + 1) * i + 2][0]
])
bias.append(Theta[(dims + 1) * i + dims][0])
steep = 10
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
pz = Softmax(weight, bias)
print('Plotting Observation Model')
#pz.plot2D(low=[2.5,2.5],high=[3.5,3.5],delta = 0.1,vis=True);
#pz.plot2D(low=[0,0],high=[10,5],delta = 0.1,vis=True);
#pz.plot2D(low=[-5,-5],high=[5,5],delta = 0.1,vis=True);
pz2 = Softmax(deepcopy(weight), deepcopy(bias))
pz3 = Softmax(deepcopy(weight), deepcopy(bias))
pz4 = Softmax(deepcopy(weight), deepcopy(bias))
for i in range(0, len(pz2.weights)):
pz2.weights[i] = [pz2.weights[i][0], pz2.weights[i][2]]
for i in range(0, len(pz3.weights)):
pz3.weights[i] = [pz3.weights[i][1], pz3.weights[i][2]]
for i in range(0, len(pz4.weights)):
pz4.weights[i] = [pz4.weights[i][0], pz4.weights[i][1]]
fig = plt.figure()
[x, y, c] = pz2.plot2D(low=[-5, -5], high=[5, 5], vis=False)
plt.contourf(x, y, c)
plt.xlabel('X Axis')
plt.ylabel('Z Axis')
plt.title('Slice Across Y Axis')
fig = plt.figure()
[x, y, c] = pz3.plot2D(low=[-5, -5], high=[5, 5], vis=False)
plt.contourf(x, y, c)
plt.xlabel('Y Axis')
plt.ylabel('Z Axis')
plt.title('Slice Across X axis')
fig = plt.figure()
[x, y, c] = pz4.plot2D(low=[-5, -5], high=[5, 5], vis=False)
plt.contourf(x, y, c)
plt.xlabel('X Axis')
plt.ylabel('Y Axis')
plt.title('Slice Across Z Axis')
fig = plt.figure()
ax = fig.add_subplot(111, projection='3d')
ax.set_xlabel('X Axis')
ax.set_ylabel('Y Axis')
ax.set_zlabel('Z Axis')
ax.set_xlim([-5, 5])
ax.set_ylim([-5, 5])
ax.set_zlim([-5, 5])
ax.set_title("3D Scatter of Softmax Class Dominance Regions")
for clas in range(1, numClasses):
shapeEdgesX = []
shapeEdgesY = []
shapeEdgesZ = []
#-5 to 5 on all dims
data = np.zeros(shape=(21, 21, 21))
for i in range(0, 21):
for j in range(0, 21):
for k in range(0, 21):
data[i][j][k] = pz.pointEvalND(clas,
[(i - 10) / 2, (j - 10) / 2,
(k - 10) / 2])
if (data[i][j][k] > 0.1):
shapeEdgesX.append((i - 10) / 2)
shapeEdgesY.append((j - 10) / 2)
shapeEdgesZ.append((k - 10) / 2)
ax.scatter(shapeEdgesX, shapeEdgesY, shapeEdgesZ)
plt.show()
def buildPointsModel():
#Ok, so the idea here is that given some points you can
#find the normal for each edge.
dims = 2
#points = [[2,2],[2,4],[3,4],[3,2]];
#points = [[-2,-2],[-2,-1],[0,1],[2,-1],[2,-2]];
#points = [[1,1],[1,2],[3,2],[6,1],[4,-1]];
points = [[1, 1], [3, 5], [4, 1], [3, 0], [4, -2]]
pointsx = [p[0] for p in points]
pointsy = [p[1] for p in points]
centroid = [sum(pointsx) / len(points),
sum(pointsy) / len(points)]
#for each point to the next, find the normal between them.
B = []
for i in range(0, len(points)):
p1 = points[i]
if (i == len(points) - 1):
p2 = points[0]
else:
p2 = points[i + 1]
mid = []
for i in range(0, len(p1)):
mid.append((p1[i] + p2[i]) / 2)
H = np.matrix([[p1[0], p1[1], 1], [p2[0], p2[1], 1],
[mid[0], mid[1], 1]])
#print(H);
#print(nullspace(H).T[0]);
#print("");
Hnull = (nullspace(H)).tolist()
distMed1 = distance(mid[0] + Hnull[0][0], mid[1] + Hnull[1][0],
centroid[0], centroid[1])
distMed2 = distance(mid[0] - Hnull[0][0], mid[1] - Hnull[1][0],
centroid[0], centroid[1])
if (distMed1 < distMed2):
Hnull[0][0] = -Hnull[0][0]
Hnull[1][0] = -Hnull[1][0]
Hnull[2][0] = -Hnull[2][0]
for j in Hnull:
B.append(j[0])
B = np.matrix(B).T
numClasses = len(points) + 1
boundries = []
for i in range(1, numClasses):
boundries.append([i, 0])
#boundries = [[1,0],[2,0],[3,0],[4,0],[5,0]];
M = np.zeros(shape=(len(boundries) * (dims + 1), numClasses * (dims + 1)))
for j in range(0, len(boundries)):
for i in range(0, dims + 1):
M[(dims + 1) * j + i, (dims + 1) * boundries[j][1] + i] = -1
M[(dims + 1) * j + i, (dims + 1) * boundries[j][0] + 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) // (dims + 1)):
weight.append([Theta[(dims + 1) * i][0], Theta[(dims + 1) * i + 1][0]])
bias.append(Theta[(dims + 1) * i + dims][0])
steep = 5
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
pz = Softmax(weight, bias)
print('Plotting Observation Model')
#pz.plot2D(low=[2.5,2.5],high=[3.5,3.5],delta = 0.1,vis=True);
#pz.plot2D(low=[0,0],high=[10,5],delta = 0.1,vis=True);
#pz.plot2D(low=[-5,-5],high=[5,5],delta = 0.1,vis=True);
pz.plot2D(low=[-10, -10], high=[10, 10], delta=0.1, vis=True)
def buildGeneralModel():
dims = 2
'''
#Triangle Specs
numClasses = 4;
boundries = [[1,0],[2,0],[3,0]];
B = np.matrix([-1,1,-1,1,1,-1,0,-1,-1]).T;
'''
'''
#Rectangle Specs
numClasses = 4;
boundries = [[1,0],[2,0],[3,0],[4,0]];
recBounds = [[2,2],[3,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;
B = np.matrix([0.44721359549995826, -2.220446049250313e-16, -0.8944271909999157, -0.0, 0.24253562503633294, -0.9701425001453319, 0.316227766016838, -5.551115123125783e-17, -0.948683298050514, 0.0, -0.447213595499958, 0.8944271909999159]).T;
'''
#Pentagon Specs
numClasses = 6
boundries = [[1, 0], [2, 0], [3, 0], [4, 0], [5, 0]]
B = np.matrix([-1, 1, -2, 1, 1, -2, 1, 0, -1, 0, -1, -2, -1, 0, -1]).T
'''
#Hexagon Specs
numClasses = 7;
boundries = [[1,0],[2,0],[3,0],[4,0],[5,0],[6,0]];
B = np.matrix([-1,1,-1,0,2,-1,1,1,-1,1,-1,-1,0,-2,-1,-1,-1,-1]).T
'''
'''
#Octogon Specs
numClasses = 9;
boundries = [[1,0],[2,0],[3,0],[4,0],[5,0],[6,0],[7,0],[8,0]];
B = np.matrix([-1,1,-1,0,2,-1,1,1,-1,1,0,-1,1,-1,-1,0,-2,-1,-1,-1,-1,-1,0,-1]).T;
'''
M = np.zeros(shape=(len(boundries) * (dims + 1), numClasses * (dims + 1)))
for j in range(0, len(boundries)):
for i in range(0, dims + 1):
M[(dims + 1) * j + i, (dims + 1) * boundries[j][1] + i] = -1
M[(dims + 1) * j + i, (dims + 1) * boundries[j][0] + 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) // (dims + 1)):
weight.append([Theta[(dims + 1) * i][0], Theta[(dims + 1) * i + 1][0]])
bias.append(Theta[(dims + 1) * i + dims][0])
steep = 5
weight = (np.array(weight) * steep).tolist()
bias = (np.array(bias) * steep).tolist()
pz = Softmax(weight, bias)
print('Plotting Observation Model')
#pz.plot2D(low=[2.5,2.5],high=[3.5,3.5],delta = 0.1,vis=True);
#pz.plot2D(low=[0,0],high=[10,5],delta = 0.1,vis=True);
pz.plot2D(low=[-5, -5], high=[5, 5], delta=0.1, vis=True)
class ModelSpec:
def __init__(self):
self.fileNamePrefix = 'D2DiffsSoftmax'
self.walls = []
#Problem specific
def buildTransition(self):
self.bounds = [[-10, 10], [-10, 10]]
self.delAVar = (np.identity(2) * 0.25).tolist()
#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 = 0.5
self.delA = [[-delta, 0], [delta, 0], [0, delta], [0, -delta], [0, 0]]
self.discount = 0.95
#set wall line segments
#self.walls = [[[2.5,-1],[2.5,2]],[[0,0],[0,5]],[[0,5],[5,5]],[[5,0],[5,5]],[[0,0],[5,0]]];
#self.walls = [];
#Problem Specific
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()
#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(2) * .5).tolist()
for i in range(0, len(self.r)):
self.r[i].addG(
Gaussian([-self.delA[i][0], -self.delA[i][1]], var, 10))
print('Plotting Reward Model')
for i in range(0, len(self.r)):
self.r[i].plot2D(high=[10, 10],
low=[-10, -10],
xlabel='Robot X',
ylabel='Robot Y',
title='Reward for action: ' + str(i))
print('Condensing Reward Model')
for i in range(0, len(self.r)):
self.r[i] = self.r[i].kmeansCondensationN(k=5)
print('Plotting Condensed Reward Model')
for i in range(0, len(self.r)):
#self.r[i].plot2D(xlabel = 'Robot X',ylabel = 'Robot Y',title = 'Reward for action: ' + str(i));
[x, y, c] = self.r[i].plot2D(high=[10, 10],
low=[-10, -10],
vis=False)
minim = np.amin(c)
maxim = np.amax(c)
#print(minim,maxim);
levels = np.linspace(minim, maxim)
plt.contourf(x,
y,
c,
levels=levels,
vmin=minim,
vmax=maxim,
cmap='viridis')
plt.title('Reward for action: ' + str(i))
plt.xlabel('Robot X')
plt.ylabel('Robot Y')
plt.show()
f = open("../models/rew/" + self.fileNamePrefix + "REW.npy", "w")
np.save(f, self.r)
else:
self.r = np.load("../models/rew/" + self.fileNamePrefix +
"REW.npy").tolist()
