def plotXYEstimErrorOfSpeed(what, rs,foldername = "None", targetSize = "All"):
plt.figure(1, figsize=(16,9))
if what == "OPTI" and targetSize == "All":
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/Log/"
plotEstimErrorOfSpeed(name, ax)
#makeInitPlot(rs)
ax.set_xlabel("Velocity (m/s)")
ax.set_ylabel("Estimation error (m)")
ax.set_title("Estimation error function of velocity for target " + str(rs.target_size[i]))
imageFolder = rs.OPTIpath + "/ImageBank/"
else:
if what == "OPTI":
name = rs.OPTIpath + targetSize + "/" + foldername + "/Log/"
imageFolder = rs.OPTIpath + "/ImageBank/"
elif what == "Brent":
name = BrentTrajectoriesFolder
imageFolder = "ImageBank/"
else:
name = rs.path + foldername + "/Log/"
imageFolder = rs.path + "/ImageBank/"
plotEstimErrorOfSpeed(name, plt)
#makeInitPlot(rs)
plt.xlabel("Velocity (m/s)")
plt.ylabel("Estimation error (m)")
plt.title("Estimation error function of velocity for " + what)
checkIfFolderExists(imageFolder)
plt.savefig("ImageBank/"+what+'_estimError'+foldername+'.svg', bbox_inches='tight')
plt.show(block = True)
def printEpisode(self):
#cost, time = self.runMultiProcessTrajectories(self.rs.numberOfRepeatEachTraj)
#cost, time = self.allTraj(self.rs.numberOfRepeatEachTraj)
#print("Progression :"+str(self.progress))
cost, time = self.progress(self.rs.numberOfRepeatEachTraj)
#cost, time = self.ligneTraj(self.rs.numberOfRepeatEachTraj)
#cost, time = self.OneTraj(self.rs.XTarget+ 0.1*np.cos(-np.pi/2), self.rs.YTarget+ 0.1*np.sin(-np.pi/2))
costfoldername = self.foldername + "CostCMAES/"
checkIfFolderExists(costfoldername)
costFile = open(costfoldername + "ddpgCost.log", "a")
timeFile = open(costfoldername + "ddpgTime.log", "a")
costFile.write(str(cost) + "\n")
timeFile.write(str(time) + "\n")
costFile.close()
timeFile.close()
"""
if(cost>0.8 and self.progress <4):
self.progress+=1
self.max=0
"""
if (cost > self.max):
self.max = cost
writeArray(self.actor.linear_parameters(), self.foldername, "Best",
".theta")
def plotVelocityProfile(what, rs, foldername = "None"):
arm = ArmType[rs.arm]()
plt.figure(1, figsize=(16,9))
if what == "OPTI":
for i in range(4):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/Log/"
makeVelocityData(rs,arm,name,ax)
ax.set_xlabel("time (s)")
ax.set_ylabel("Instantaneous velocity (m/s)")
ax.set_title(str("Velocity profiles for target " + str(rs.target_size[i])))
imageFolder = rs.OPTIpath + "/ImageBank/"
else:
if what == "Brent":
name = BrentTrajectoriesFolder
imageFolder="ImageBank/"
else:
name = rs.path + foldername + "/Log/"
imageFolder=rs.path +"/ImageBank/"
makeVelocityData(rs,arm,name,plt)
plt.xlabel("time (s)")
plt.ylabel("Instantaneous velocity (m/s)")
plt.title("Velocity profiles for " + what)
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+'velo.svg', bbox_inches='tight')
plt.show(block = True)
def plotDDPGCostProgress(rs):
plt.figure(1, figsize=(16,9))
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/Cost/ddpgCost.log"
nameProgress = rs.OPTIpath + str(rs.target_size[i]) + "/Cost/ddpgProg.log"
data = np.loadtxt(name)
#progress = np.loadtxt(nameProgress)
x,w = [],[]
for j in range(len(data)):
x.append(j)
w.append(data[j])
ax.plot(x, w, c = 'b')
"""
for j in progress.shape[0]:
ax.plot(progress[j],np.random.uniform(-0.8,1.2,100),'r')
"""
ax.set_title(str("Cost Target " + str(rs.target_size[i])))
imageFolder = rs.OPTIpath + "/ImageBank/"
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+rs.thetaFile+"DDPGcostProgress.svg")
plt.show(block = True)
def launchCMAESMissing(rs, save, gamma, point):
'''
Run cmaes for a specific target size
Input: -target_size, size of the target, float
-setuFile, file of setup, string
-save: do we use a previous Best.theta file? True = Yes, False = use current controller, None = random controller
'''
rs.gamma = gamma/10.0
pos=point[0]
x=point[1][0]
y=point[1][1]
target_size = point[2]
print("Starting the CMAES Optimization for gamma = "+ str(gamma) + ",target " + str(target_size) + " for point "+ str(pos)+" !")
foldername = rs.OPTIpathMissing + "gamma" + str(gamma) + "/"+str(target_size)+"/"+str(pos)+"/"
thetaname = foldername + "Best"
if save:
checkIfFolderExists(foldername)
copyfile(rs.OPTIpathMissing + "gamma" + str(gamma) + "/" + str(target_size)+"/" + "Best.theta",foldername + "Best.theta")
elif save==None:
thetaname=None
#Initializes all the classes used to generate trajectory
exp = Experiments(rs, target_size, False, foldername, thetaname,rs.popsizeCmaes,rs.period)
theta = exp.tm.controller.getTheta()
thetaCMA = theta.flatten()
#run the optimization (cmaes)
cma.fmin(partial(exp.runTrajectoriesCMAESOnePointMulti, x, y), thetaCMA, rs.sigmaCmaes, options={'maxiter':rs.maxIterCmaes, 'popsize':rs.popsizeCmaes, 'CMA_diagonal':True, 'verb_log':0, 'verb_disp':0,'termination_callback':term()})
print("End of optimization for gamma = "+ str(gamma) + ",target " + str(target_size) + " for point "+ str(pos)+" !")
def plotCMAESTimeProgress(rs):
plt.figure(1, figsize=(16,9))
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/Cost/cmaesTime.log"
data = np.loadtxt(name)
x,w,m,b = [],[],[],[]
for j in range(len(data)):
x.append(j)
w.append(data[j][0])
m.append(data[j][1])
b.append(data[j][2])
ax.plot(x, w, c = 'b')
ax.plot(x, m, c = 'g')
ax.plot(x, b, c = 'r')
ax.set_title(str("Time Target " + str(rs.target_size[i])))
imageFolder = rs.OPTIpath + "/ImageBank/"
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+"timeProgress.svg")
plt.show(block = True)
def copyRegressiontoCMAES(rs, name, target_size):
cmaname = rs.OPTIpath + str(target_size) + "/"
checkIfFolderExists(cmaname)
savenametheta = rs.path + name + ".theta"
copyfile(savenametheta, cmaname + name + ".theta")
if(rs.regression=="RBFN"):
savenamestruct = rs.path + name + ".struct"
copyfile(savenamestruct, cmaname + name + ".struct")
def copyRegressiontoCMAES(rs, name, target_size):
cmaname = rs.OPTIpath + str(target_size) + "/"
checkIfFolderExists(cmaname)
savenametheta = rs.path + name + ".theta"
copyfile(savenametheta, cmaname + name + ".theta")
if (rs.regression == "RBFN"):
savenamestruct = rs.path + name + ".struct"
copyfile(savenamestruct, cmaname + name + ".struct")
def plotMuscularActivations(what, rs, foldername = "None", targetSize = "0.05"):
'''
plots the muscular activations from a folder
input: -foldername: the folder where the data lies
-what: get from Brent, rbfn or from cmaes controllers
'''
plt.figure(1, figsize=(16,9))
if what == "OPTI":
name = rs.OPTIpath + targetSize + "/" + foldername + "/Log/"
imageFolder = rs.OPTIpath + "/ImageBank/"
elif what == "Brent":
name = BrentTrajectoriesFolder
imageFolder = "ImageBank/"
else:
name = rs.path + foldername + "/Log/"
imageFolder = rs.path + "/ImageBank/"
U = getNoiselessCommandData(name)
checkIfFolderExists(imageFolder)
for key, el1 in U.items():
t = []
u1, u2, u3, u4, u5, u6 = [], [], [], [], [], []
if rd.random()<0.01 or what != "Brent":
for i in range(len(el1)):
t.append(i)
u1.append(el1[i][0])
u2.append(el1[i][1])
u3.append(el1[i][2])
u4.append(el1[i][3])
u5.append(el1[i][4])
u6.append(el1[i][5])
plt.plot(t, u1, label = "U1")
plt.plot(t, u2, label = "U2")
plt.plot(t, u3, label = "U3")
plt.plot(t, u4, label = "U4")
plt.plot(t, u5, label = "U5")
plt.plot(t, u6, label = "U6")
plt.legend(loc = 0)
plt.xlabel("time")
plt.ylabel("U")
plt.title("Muscular Activations for " + what)
plt.savefig(imageFolder+what+"_muscu" + key +foldername + ".svg", bbox_inches='tight')
print key
val = raw_input('1 to see data, anything otherwise: ')
val = int(val)
if val == 1:
print el1
#plt.clf()
plt.show(block = True)
def launchCMAESForSpecificTargetSizeAndSpecificPoint(target_size,
rs,
save,
point,
noise=None):
'''
Run cmaes for a specific target size
Input: -target_size, size of the target, float
-setuFile, file of setup, string
-save: do we use a previous Best.theta file? True = Yes, False = use current controller, None = random controller
noise: noise on muscle, if None, defalt noise from muscle setup, float
'''
pos = point[0]
x = point[1][0]
y = point[1][1]
print("Starting the CMAES Optimization for target " + str(target_size) +
" for point " + str(pos) + " !")
foldername = rs.OPTIpath + str(target_size) + "/" + str(pos) + "/"
thetaname = foldername + "Best"
if save:
checkIfFolderExists(foldername)
copyfile(rs.OPTIpath + str(target_size) + "/" + "Best.theta",
foldername + "Best.theta")
elif save == None:
thetaname = None
#Initializes all the class used to generate trajectory
exp = Experiments(rs, target_size, False, foldername, thetaname,
rs.popsizeCmaes, rs.period)
if (noise != None):
exp.setNoise(noise)
theta = exp.tm.controller.getTheta()
thetaCMA = theta.flatten()
#run the optimization (cmaes)
cma.fmin(partial(exp.runTrajectoriesCMAESOnePoint, x, y),
thetaCMA,
rs.sigmaCmaes,
options={
'maxiter': rs.maxIterCmaes,
'popsize': rs.popsizeCmaes,
'CMA_diagonal': True,
'verb_log': 0,
'verb_disp': 0,
'termination_callback': term()
})
print("End of optimization for target " + str(target_size) +
" for point " + str(pos) + " !")
def launchCMAESMissing(rs, save, gamma, point):
'''
Run cmaes for a specific target size
Input: -target_size, size of the target, float
-setuFile, file of setup, string
-save: do we use a previous Best.theta file? True = Yes, False = use current controller, None = random controller
'''
rs.gamma = gamma / 10.0
pos = point[0]
x = point[1][0]
y = point[1][1]
target_size = point[2]
print("Starting the CMAES Optimization for gamma = " + str(gamma) +
",target " + str(target_size) + " for point " + str(pos) + " !")
foldername = rs.OPTIpathMissing + "gamma" + str(gamma) + "/" + str(
target_size) + "/" + str(pos) + "/"
thetaname = foldername + "Best"
if save:
checkIfFolderExists(foldername)
copyfile(
rs.OPTIpathMissing + "gamma" + str(gamma) + "/" +
str(target_size) + "/" + "Best.theta", foldername + "Best.theta")
elif save == None:
thetaname = None
#Initializes all the classes used to generate trajectory
exp = Experiments(rs, target_size, False, foldername, thetaname,
rs.popsizeCmaes, rs.period)
theta = exp.tm.controller.getTheta()
thetaCMA = theta.flatten()
#run the optimization (cmaes)
cma.fmin(partial(exp.runTrajectoriesCMAESOnePointMulti, x, y),
thetaCMA,
rs.sigmaCmaes,
options={
'maxiter': rs.maxIterCmaes,
'popsize': rs.popsizeCmaes,
'CMA_diagonal': True,
'verb_log': 0,
'verb_disp': 0,
'termination_callback': term()
})
print("End of optimization for gamma = " + str(gamma) + ",target " +
str(target_size) + " for point " + str(pos) + " !")
def plotXYPositions(what, rs, foldername = "None", targetSize = "All", plotEstim=False, zoom=False):
#plt.ion()
plotName="trajectories"
if zoom==True:
plotName+="zoom"
plt.figure(1, figsize=(16,9))
if (what == "OPTI") and targetSize == "All":
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
if zoom==True:
ax.set_xlim([-rs.target_size[i],rs.target_size[i]])
scale=rs.target_size[i]*18/16
ax.set_ylim([rs.YTarget-3*scale/4,rs.YTarget+scale/4])
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/Log/"
ax.plot([rs.XTarget-rs.target_size[i]/2, rs.XTarget+rs.target_size[i]/2], [rs.YTarget, rs.YTarget], color="r", linewidth=4.0)
plotPos(name, ax, plotEstim,rs)
#makeInitPlot(rs)
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_title("XY Positions for target " + str(rs.target_size[i]))
imageFolder = rs.OPTIpath + "/ImageBank/"
else:
plt.plot([rs.XTarget-float(targetSize)/2, rs.XTarget+float(targetSize)/2], [rs.YTarget, rs.YTarget], color="r", linewidth=4.0)
if (what == "OPTI"):
name = rs.OPTIpath + targetSize + "/" + foldername + "/Log/"
imageFolder =rs.OPTIpath + targetSize + "/ImageBank/"
elif what == "Brent":
name = BrentTrajectoriesFolder
imageFolder ="ImageBank/"
else:
name = rs.path + foldername + "/Log/"
imageFolder =rs.path + "/ImageBank/"
plotPos(name, plt, plotEstim, rs)
#makeInitPlot(rs)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("XY Positions for " + what)
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+plotName+'.pdf', bbox_inches='tight')
plt.show(block = True)
def plotScattergram(what,foldername,rs):
data = {}
plt.figure(1, figsize=(16,9))
if what=="OPTI":
for i in range(len(rs.target_size)):
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/finalX/"
tmp = getLastXData(name)
tabx = []
for el in tmp.values():
for j in range(len(el)):
tabx.append(el[j])
data[rs.target_size[i]] = tabx
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((4,1), (i,0))
ax.set_xlim([-0.03,0.03])
ax.hist(data[rs.target_size[i]], 100)
ax.axvline(x=rs.target_size[i]/2, c = 'r', linewidth = 3)
ax.axvline(x=-rs.target_size[i]/2, c = 'r', linewidth = 3)
ax.set_title(str("Hit Dispersion for Target " + str(rs.target_size[i])))
imageFolder = rs.OPTIpath + "/ImageBank/"
elif what=="RBFN":
name = rs.path + foldername + "/finalX/"
tmp = getLastXData(name)
tabx = []
for el in tmp.values():
for j in range(len(el)):
tabx.append(el[j])
plt.hist(tabx, 20)
for i in range(len(rs.target_size)):
plt.plot([-rs.target_size[i]/2, -rs.target_size[i]]/2, [0, 20], c = 'r', linewidth = 3)
plt.plot([rs.target_size[i]/2, rs.target_size[i]]/2, [0, 20], c = 'r', linewidth = 3)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("Hit Dispersion for "+rs.regression)
imageFolder = rs.path + "/ImageBank/"
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+what+"_hitdisp"+foldername+".svg", bbox_inches='tight')
plt.show(block = True)
def plotArticularPositions(what, rs,foldername = "None"):
plt.figure(1, figsize=(16,9))
if what == "OPTI":
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/Log/"
state = getStateData(name)
for _,v in state.items():
Q1, Q2 = [], []
for j in range(len(v)):
Q1.append(v[j][2])
Q2.append(v[j][3])
ax.plot(Q1,Q2, c ='b')
ax.set_xlabel("Q1 (rad)")
ax.set_ylabel("Q2 (rad)")
ax.set_title("Articular positions for " + str(rs.target_size[i]))
imageFolder = rs.OPTIpath + "/ImageBank/"
else :
if what == "Brent":
name = BrentTrajectoriesFolder
imageFolder = "ImageBank/"
else:
name = rs.path + foldername + "/Log/"
imageFolder = rs.path + "/ImageBank/"
state = getStateData(name)
for _,v in state.items():
if rd.random()<0.06 or what != "Brent":
Q1, Q2 = [], []
for j in range(len(v)):
Q1.append(v[j][2])
Q2.append(v[j][3])
plt.plot(Q1,Q2, c ='b')
plt.xlabel("Q1 (rad)")
plt.ylabel("Q2 (rad)")
plt.title("Articular positions for " + what)
checkIfFolderExists(imageFolder)
plt.savefig("ImageBank/"+what+'_articular'+foldername+'.svg', bbox_inches='tight')
plt.show(block = True)
def plotHitDispersion(foldername,sizeT, rs):
name = rs.OPTIpath + sizeT + "/" + foldername + "/finalX/"
data = getLastXData(name)
tabx, taby = [], []
for el in data.values():
for j in range(len(el)):
tabx.append(el[j])
taby.append(rs.YTarget)
plt.figure(1, figsize=(16,9))
plt.plot([-rs.target_size[0]/2, rs.target_size[0]/2], [rs.YTarget, rs.YTarget], c = 'r')
plt.scatter([-rs.target_size[0]/2, rs.target_size[0]/2], [rs.YTarget, rs.YTarget], marker=u'|', s = 100)
plt.scatter(tabx, taby, c = 'b')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
imageFolder = rs.OPTIpath + sizeT +"/ImageBank/"
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+"hitDispersion.svg", bbox_inches='tight')
plt.show(block = True)
def plotFittsLaw(foldername, rs, rbfn = False):
plt.figure(1, figsize=(16,9))
timeDistWidth = []
for i in range(len(rs.target_size)):
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/TrajTime/"
trajTimes = getTrajTimeData(name)
for _, v in trajTimes.items():
for j in range(len(v)):
distance = rs.getDistanceToTarget(v[j][0],v[j][1])
trajtime = v[j][2]
size = rs.target_size[i]
timeDistWidth.append((distance, size, trajtime))
MT, DI = [], []
for el in timeDistWidth:
MT.append(el[2])
DI.append(np.log2(el[0]/el[1]))
slope, intercept, r_value, _, _ = stats.linregress(DI,MT)
yLR = slope * np.asarray(DI) + intercept
for el in timeDistWidth:
if el[0]<=0.15:
plt.scatter(np.log2(el[0]/el[1]), el[2], c ='blue')
elif el[0]<=0.28:
plt.scatter(np.log2(el[0]/el[1]), el[2], c ='green')
else:
plt.scatter(np.log2(el[0]/el[1]), el[2], c ='red')
plt.plot(DI, yLR)
plt.title("a = " + str(slope) + " b = " + str(intercept) + " r^2 = " + str(r_value**2))
plt.xlabel("log(D/W)/log(2)")
plt.ylabel("Movement time (s)")
imageFolder = rs.OPTIpath + "/ImageBank/"
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+"Fitts.svg", bbox_inches='tight')
plt.show(block = True)
def launchCMAESForSpecificTargetSizeAndSpecificPoint(target_size, rs, save, point, noise=None):
'''
Run cmaes for a specific target size
Input: -target_size, size of the target, float
-setuFile, file of setup, string
-save: do we use a previous Best.theta file? True = Yes, False = use current controller, None = random controller
noise: noise on muscle, if None, defalt noise from muscle setup, float
'''
pos=point[0]
x=point[1][0]
y=point[1][1]
print("Starting the CMAES Optimization for target " + str(target_size) + " for point "+ str(pos)+" !")
foldername = rs.OPTIpath + str(target_size)+"/"+str(pos)+"/"
thetaname = foldername + "Best"
if save:
checkIfFolderExists(foldername)
copyfile(rs.OPTIpath + str(target_size)+"/" + "Best.theta",foldername + "Best.theta")
elif save==None:
thetaname=None
#Initializes all the class used to generate trajectory
exp = Experiments(rs, target_size, False, foldername, thetaname,rs.popsizeCmaes,rs.period)
if(noise!=None):
exp.setNoise(noise)
theta = exp.tm.controller.getTheta()
thetaCMA = theta.flatten()
#run the optimization (cmaes)
cma.fmin(partial(exp.runTrajectoriesCMAESOnePoint, x, y), thetaCMA, rs.sigmaCmaes, options={'maxiter':rs.maxIterCmaes, 'popsize':rs.popsizeCmaes, 'CMA_diagonal':True, 'verb_log':0, 'verb_disp':0,'termination_callback':term()})
print("End of optimization for target " + str(target_size) + " for point "+ str(pos)+" !")
def printEpisode(self):
#cost, time = self.runMultiProcessTrajectories(self.rs.numberOfRepeatEachTraj)
#cost, time = self.allTraj(self.rs.numberOfRepeatEachTraj)
#print("Progression :"+str(self.progress))
cost, time = self.progress(self.rs.numberOfRepeatEachTraj)
#cost, time = self.ligneTraj(self.rs.numberOfRepeatEachTraj)
#cost, time = self.OneTraj(self.rs.XTarget+ 0.1*np.cos(-np.pi/2), self.rs.YTarget+ 0.1*np.sin(-np.pi/2))
costfoldername = self.foldername+"CostCMAES/"
checkIfFolderExists(costfoldername)
costFile = open(costfoldername+"ddpgCost.log","a")
timeFile = open(costfoldername+"ddpgTime.log","a")
costFile.write(str(cost)+"\n")
timeFile.write(str(time)+"\n")
costFile.close()
timeFile.close()
"""
if(cost>0.8 and self.progress <4):
self.progress+=1
self.max=0
"""
if(cost>self.max):
self.max = cost
writeArray(self.actor.linear_parameters(),self.foldername, "Best", ".theta")
def copyRegressiontoCMAES(rs, name, size):
cmaname = rs.CMAESpath + str(size) + "/"
checkIfFolderExists(cmaname)
savenametheta = rs.path + name + ".theta"
copyfile(savenametheta, cmaname + name + ".theta")
def plotTimeColorMap(what, rs, foldername = "None", targetSize = "All"):
'''
Cette fonction permet d'afficher le profil de temps des trajectoires
Entrees: -what: choix des donnees a afficher
'''
fig = plt.figure(1, figsize=(16,9))
if what == "OPTI" and targetSize == "All":
for i in range(len(rs.target_size)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.OPTIpath + str(rs.target_size[i]) + "/" + foldername + "/TrajTime/"
times = getTrajTimeData(name)
x0 = []
y0 = []
time = []
for _, v in times.items():
for j in range(len(v)):
x0.append(v[j][0])
y0.append(v[j][1])
time.append(v[j][2])
xi = np.linspace(-0.4,0.4,100)
yi = np.linspace(0.12,0.58,100)
zi = griddata(x0, y0, time, xi, yi)
t1 = ax.scatter(x0, y0, c=time, marker=u'o', s=50, cmap=cm.get_cmap('RdYlBu'))
ax.scatter(rs.XTarget, rs.YTarget, c ='g', marker='v', s=200)
ax.contourf(xi, yi, zi, 15, cmap=cm.get_cmap('RdYlBu'))
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_title(str("Time map for target " + str(rs.target_size[i])))
fig.colorbar(t1, shrink=0.5, aspect=5)
t1 = ax.scatter(x0, y0, c='b', marker=u'o', s=20)
imageFolder = rs.OPTIpath + "/ImageBank/"
else:
if what == "OPTI":
name = rs.OPTIpath + targetSize + "/" + foldername + "/TrajTime/"
imageFolder = rs.OPTIpath + "/ImageBank/"
elif what == "Brent":
name = BrentTrajectoriesFolder
imageFolder = "ImageBank/"
else:
name = rs.path + foldername + "/TrajTime/"
imageFolder = rs.path + "/ImageBank/"
times = getTrajTimeData(name)
x0 = []
y0 = []
time = []
for _, v in times.items():
for j in range(len(v)):
x0.append(v[j][0])
y0.append(v[j][1])
time.append(v[j][2])
xi = np.linspace(-0.4,0.4,100)
yi = np.linspace(0.12,0.58,100)
zi = griddata(x0, y0, time, xi, yi)
t1 = plt.scatter(x0, y0, c=time, marker=u'o', s=50, cmap=cm.get_cmap('RdYlBu'))
plt.scatter(rs.XTarget, rs.YTarget, c ='g', marker='v', s=200)
plt.contourf(xi, yi, zi, 15, cmap=cm.get_cmap('RdYlBu'))
fig.colorbar(t1, shrink=0.5, aspect=5)
plt.scatter(x0, y0, c='b', marker=u'o', s=20)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
checkIfFolderExists(imageFolder)
plt.savefig(imageFolder+'timemap.svg', bbox_inches='tight')
plt.show(block = True)
def copyRegressiontoCMAES(rs, name, size):
cmaname = rs.CMAESpath + str(size) + "/"
checkIfFolderExists(cmaname)
savenametheta = rs.path + name + ".theta"
copyfile(savenametheta, cmaname + name + ".theta")
def runTrajectories(self,theta, fonction):
'''
Generates all the trajectories of the experimental setup and return the mean cost. This function is used by cmaes to optimize the controller.
Input: -theta: vector of parameters, one dimension normalized numpy array
Ouput: -meanAll: the mean of the cost of all trajectories generated, float
'''
#c = Chrono()
self.initTheta(theta)
#print "theta avant appel :", theta
#compute all the trajectories x times each, x = numberOfRepeat
meanCost, meanTime = fonction(self.numberOfRepeat)
#cma.plot()
#opt = cma.CMAOptions()
#print "CMAES options :", opt
#c.stop()
#print("Indiv #: ", self.call, "\n Cost: ", meanCost)
if (self.call==0):
self.localBestCost = meanCost
self.localWorstCost = meanCost
self.localBestTime = meanTime
self.localWorstTime = meanTime
self.periodMeanCost = 0.0
self.periodMeanTime = 0.0
else:
if meanCost>self.localBestCost:
self.localBestCost = meanCost
elif meanCost<self.localWorstCost:
self.localWorstCost = meanCost
if meanTime>self.localBestTime:
self.localBestTime = meanTime
elif meanTime<self.localWorstTime:
self.localWorstTime = meanTime
if meanCost>self.bestCost:
self.bestCost = meanCost
if meanCost>0:
extension = ".save" + str(meanCost)
filename = findDataFilename(self.foldername+"Theta/", "theta", extension)
np.savetxt(filename, self.theta)
filename2 = self.foldername + "Best.theta"
np.savetxt(filename2, self.theta)
self.periodMeanCost += meanCost
self.periodMeanTime += meanTime
self.call += 1
self.call = self.call%self.period
if (self.call==0):
self.periodMeanCost = self.periodMeanCost/self.period
self.periodMeanTime = self.periodMeanTime/self.period
self.CMAESCostStore.append((self.localWorstCost,self.periodMeanCost,self.localBestCost))
self.CMAESTimeStore.append((self.localWorstTime,self.periodMeanTime,self.localBestTime))
costfoldername = self.foldername+"Cost/"
checkIfFolderExists(costfoldername)
cost = open(costfoldername+"cmaesCost.log","a")
time = open(costfoldername+"cmaesTime.log","a")
cost.write(str(self.localWorstCost)+" "+str(self.periodMeanCost)+" "+str(self.localBestCost)+"\n")
time.write(str(self.localWorstTime)+" "+str(self.periodMeanTime)+" "+str(self.localBestTime)+"\n")
cost.close()
time.close()
#np.savetxt(costfoldername+"cmaesCost.log",self.CMAESCostStore) #Note: inefficient, should rather add to the file
#np.savetxt(costfoldername+"cmaesTime.log",self.CMAESTimeStore) #Note: inefficient, should rather add to the file
return 10.0*(self.rs.rhoCF-meanCost)/self.rs.rhoCF
