def plotPerfSizeDist(foldername):
rs = ReadSetupFile()
dicoCost = {}
for i in range(len(rs.sizeOfTarget)):
name = rs.CMAESpath + str(
rs.sizeOfTarget[i]) + "/" + foldername + "/Cost/"
costs = getCostData(name)
for k, v in costs.items():
for j in range(len(v)):
distance = round(rs.getDistanceToTarget(v[j][0], v[j][1]), 2)
if not distance in dicoCost.keys():
dicoCost[distance] = {}
if not rs.sizeOfTarget[i] in dicoCost[distance].keys():
dicoCost[distance][rs.sizeOfTarget[i]] = []
dicoCost[distance][rs.sizeOfTarget[i]].append(v[j][2])
plotTab = []
fig = plt.figure(1, figsize=(16, 9))
plt.ylabel("performance")
plt.xlabel("Target size (mm)")
for key in sorted(dicoCost.keys()):
plotTab.append(
plt.plot([i for i in sorted(dicoCost[key].keys())], [
np.mean(dicoCost[key][i]) for i in sorted(dicoCost[key].keys())
],
label=str("Distance: " + str(key))))
plt.legend(loc=0)
plt.savefig("ImageBank/perfdist" + foldername + ".png",
bbox_inches='tight')
plt.show(block=True)
def plotPerfSizeDist(foldername):
rs = ReadSetupFile()
dicoCost = {}
for i in range(len(rs.sizeOfTarget)):
name = rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/Cost/"
costs = getCostData(name)
for k, v in costs.items():
for j in range(len(v)):
distance = round(rs.getDistanceToTarget(v[j][0],v[j][1]),2)
if not distance in dicoCost.keys():
dicoCost[distance] = {}
if not rs.sizeOfTarget[i] in dicoCost[distance].keys():
dicoCost[distance][rs.sizeOfTarget[i]] = []
dicoCost[distance][rs.sizeOfTarget[i]].append(v[j][2])
plotTab = []
fig = plt.figure(1, figsize=(16,9))
plt.ylabel("performance")
plt.xlabel("Target size (mm)")
for key in sorted(dicoCost.keys()):
plotTab.append(plt.plot([i for i in sorted(dicoCost[key].keys())], [np.mean(dicoCost[key][i]) for i in sorted(dicoCost[key].keys())], label = str("Distance: " + str(key))))
plt.legend(loc = 0)
plt.savefig("ImageBank/perfdist"+foldername+".png", bbox_inches='tight')
plt.show(block = True)
def plotXYEstimErrorOfSpeed(what, foldername="None", targetSize="All"):
rs = ReadSetupFile()
plt.figure(1, figsize=(16, 9))
if what == "CMAES" and targetSize == "All":
for i in range(len(rs.sizeOfTarget)):
ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
name = rs.CMAESpath + str(
rs.sizeOfTarget[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.sizeOfTarget[i]))
else:
if what == "CMAES":
name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.RBFNpath + foldername + "/Log/"
plotEstimErrorOfSpeed(name, plt)
#makeInitPlot(rs)
plt.xlabel("Velocity (m/s)")
plt.ylabel("Estimation error (m)")
plt.title("Estimation error function of velocity for " + what)
plt.savefig("ImageBank/" + what + '_estimError' + foldername + '.png',
bbox_inches='tight')
plt.show(block=True)
def plotVelocityProfile(what, foldername="None"):
rs = ReadSetupFile()
arm = Arm()
plt.figure(1, figsize=(16, 9))
if what == "CMAES":
for i in range(4):
ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
name = rs.CMAESpath + str(
rs.sizeOfTarget[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.sizeOfTarget[i])))
else:
if what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.RBFNpath + foldername + "/Log/"
makeVelocityData(rs, arm, name, plt)
plt.xlabel("time (s)")
plt.ylabel("Instantaneous velocity (m/s)")
plt.title("Velocity profiles for " + what)
plt.savefig("ImageBank/" + what + '_velocity_profiles' + foldername +
'.png',
bbox_inches='tight')
plt.show(block=True)
def plotArticularPositions(what, foldername="None", targetSize="0.05"):
rs = ReadSetupFile()
if what == "CMAES":
name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.RBFNpath + foldername + "/Log/"
state = getStateData(name)
plt.figure(1, figsize=(16, 9))
for k, 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)
plt.savefig("ImageBank/" + what + '_articular' + foldername + '.png',
bbox_inches='tight')
plt.show(block=True)
def UnitTestArmModel():
'''
Tests the next state
'''
rs = ReadSetupFile()
arm = Arm()
arm.setDT(rs.dt)
state, estimState, command, noisycommand, nextEstimState, nextState = {}, {}, {}, {}, {}, {}
for el in os.listdir(BrentTrajectoriesFolder):
state[el], estimState[el], command[el], noisycommand[el], nextEstimState[el], nextState[el] = [], [], [], [], [], []
data = np.loadtxt(BrentTrajectoriesFolder + el)
for i in range(data.shape[0]):
estimState[el].append(np.array([data[i][4], data[i][5], data[i][6], data[i][7]]))
state[el].append(np.array([data[i][8], data[i][9], data[i][10], data[i][11]]))
noisycommand[el].append(np.array([data[i][12], data[i][13], data[i][14], data[i][15], data[i][16], data[i][17]]))
command[el].append(np.array([data[i][18], data[i][19], data[i][20], data[i][21], data[i][22], data[i][23]]))
nextEstimState[el].append(np.array([data[i][24], data[i][25], data[i][26], data[i][27]]))
nextState[el].append(np.array([data[i][28], data[i][29], data[i][30], data[i][31]]))
for el in os.listdir(BrentTrajectoriesFolder):
for i in range(len(state[el])):
if rd.random()<0.06:
outNextStateNoisy = arm.computeNextState(noisycommand[el][i],state[el][i])
outNextState = arm.computeNextState(command[el][i],state[el][i])
print("U :", command[el][i])
print("UNoisy :", noisycommand[el][i])
print("---------------------------------------------------------")
print("Real :", nextState[el][i])
print("ArmN :", outNextStateNoisy)
print("Arm :", outNextState)
print("---------------------------------------------------------")
def plotXYPositions(what, foldername = "None", targetSize = "All", plotEstim=False):
rs = ReadSetupFile()
plt.figure(1, figsize=(16,9))
if what == "CMAES" and targetSize == "All":
for i in range(len(rs.sizeOfTarget)):
ax = plt.subplot2grid((2,2), (i/2,i%2))
name = rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/Log/"
plotPos(name, ax, plotEstim)
#makeInitPlot(rs)
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_title("XY Positions for target " + str(rs.sizeOfTarget[i]))
else:
if what == "CMAES":
name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.NNpath + foldername + "/Log/"
plotPos(name, plt, plotEstim)
#makeInitPlot(rs)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("XY Positions for " + what)
plt.savefig("ImageBank/"+what+'_trajectories'+foldername+'.png', bbox_inches='tight')
plt.show(block = True)
def launchCMAESForSpecificTargetSize(sizeOfTarget, thetaFile, save):
'''
Run cmaes for a specific target size
Input: -sizeOfTarget, size of the target, float
'''
print("Starting the CMAES Optimization for target " + str(sizeOfTarget) +
" !")
rs = ReadSetupFile()
foldername = rs.CMAESpath + str(sizeOfTarget) + "/"
thetaname = foldername + thetaFile
if save:
copyRBFNtoCMAES(rs, thetaFile, sizeOfTarget)
#Initializes all the class used to generate trajectory
exp = Experiments(rs, sizeOfTarget, False, foldername, thetaname,
rs.popsizeCmaes, rs.period)
theta = exp.tm.controller.theta
thetaCMA = theta.flatten()
#run the optimization (cmaes)
resCma = cma.fmin(exp.runTrajectoriesCMAES,
thetaCMA,
rs.sigmaCmaes,
options={
'maxiter': rs.maxIterCmaes,
'popsize': rs.popsizeCmaes,
'CMA_diagonal': True,
'verb_log': 50,
'verb_disp': 1,
'termination_callback': term()
})
print("End of optimization for target " + str(sizeOfTarget) + " !")
def UnitTestRBFNController():
'''
Tests the approximated command obtained from training states
'''
rs = ReadSetupFile()
fa = initRBFNController(rs)
fa.train_rbfn()
fa.saveTheta("test")
fa.loadTheta("test")
state, command = {}, {}
for el in os.listdir(BrentTrajectoriesFolder):
state[el], command[el] = [], []
data = np.loadtxt(BrentTrajectoriesFolder + el)
for i in range(data.shape[0]):
state[el].append(
(data[i][8], data[i][9], data[i][10], data[i][11]))
command[el].append((data[i][18], data[i][19], data[i][20],
data[i][21], data[i][22], data[i][23]))
for el in os.listdir(BrentTrajectoriesFolder):
for i in range(len(state[el])):
if rd.random() < 0.06:
outrbfn = fa.computeOutput(np.array(state[el][i]))
print("Real :", command[el][i])
print("Learn :", outrbfn)
def generateRichDataFromCMAES(repeat, thetaFile, saveDir='Data'):
rs = ReadSetupFile()
for el in rs.sizeOfTarget:
thetaName = rs.CMAESpath + str(el) + "/" + thetaFile
saveName = rs.CMAESpath + str(el) + "/" + saveDir + "/"
GenerateRichDataFromTheta(rs, el, saveName, thetaName, repeat, True)
print("CMAES:End of generation")
def UnitTestNNController():
'''
Tests the approximated command obtained from training states
'''
rs = ReadSetupFile()
fa = initNNController(rs)
stateAll, commandAll = stateAndCommandDataFromTrajs(
loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
#stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
#print ("len:", len(commandAll[0]))
state = np.vstack(np.array(stateAll))
command = np.vstack(np.array(commandAll))
fa.getTrainingData(state, command)
fa.train()
fa.saveTheta("test")
fa.loadTheta("test")
for el in os.listdir(BrentTrajectoriesFolder):
for i in range(len(state)):
if rd.random() < 0.06:
outNN = fa.computeOutput(np.array(state[i]))
print("Real :", command[i])
print("Learn :", outNN)
def plotHitDispersion(foldername, sizeT):
rs = ReadSetupFile()
name = rs.CMAESpath + 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)
fig = plt.figure(1, figsize=(16, 9))
plt.plot([-rs.sizeOfTarget[0] / 2, rs.sizeOfTarget[0] / 2],
[rs.YTarget, rs.YTarget],
c='r')
plt.scatter([-rs.sizeOfTarget[0] / 2, rs.sizeOfTarget[0] / 2],
[rs.YTarget, rs.YTarget],
marker=u'|',
s=100)
plt.scatter(tabx, taby, c='b')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.savefig("ImageBank/hit" + str(sizeT) + foldername + ".png",
bbox_inches='tight')
plt.show(block=True)
def runNN(name):
'''
Takes the Brent trajectories as input, shuffles them, and then runs the NN regression algorithm
'''
rs = ReadSetupFile()
#state, command = getStateAndCommandData(BrentTrajectoriesFolder)
#stateAll, commandAll = dicToArray(state), dicToArray(command)
#print ("old:", stateAll[0])
stateAll, commandAll = stateAndCommandDataFromTrajs(
loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
#stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
#print ("len:", len(commandAll[0]))
stateAll = np.vstack(np.array(stateAll))
commandAll = np.vstack(np.array(commandAll))
#print ("len global:", len(commandAll))
#print ("new:", commandAll[0])
#this works because both shuffles generate the same order, due to the seed
np.random.seed(0)
np.random.shuffle(stateAll)
np.random.seed(0)
np.random.shuffle(commandAll)
print("nombre d'echantillons: ", len(stateAll))
fa = NeuralNet(rs.inputDim, rs.outputDim)
fa.getTrainingData(stateAll, commandAll)
fa.train()
fa.saveTheta(rs.NNpath + name + ".theta")
def runRBFN(name,fromStruct):
'''
Takes the Brent trajectories as input, shuffles them, and then runs the RBFN regression algorithm
'''
rs = ReadSetupFile()
#state, command = getStateAndCommandData(BrentTrajectoriesFolder)
#stateAll, commandAll = dicToArray(state), dicToArray(command)
#print ("old:", stateAll[0])
stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(pathDataFolder + "TrajRepository/"))
#stateAll, commandAll = stateAndCommandDataFromTrajs(loadStateCommandPairsByStartCoords(BrentTrajectoriesFolder))
#print ("len:", len(commandAll[0]))
stateAll = np.vstack(np.array(stateAll))
commandAll = np.vstack(np.array(commandAll))
#print ("len global:", len(commandAll))
#print ("new:", commandAll[0])
#this works because both shuffles generate the same order, due to the seed
np.random.seed(0)
np.random.shuffle(stateAll)
np.random.seed(0)
np.random.shuffle(commandAll)
print("nombre d'echantillons: ", len(stateAll))
fa = rbfn(rs.numfeats,rs.inputDim,rs.outputDim)
fa.getTrainingData(stateAll, commandAll)
if fromStruct == True:
initFromExistingStruct(fa,rs.RBFNpath+name+".struct")
else:
initFromData(fa,rs.RBFNpath+name+".struct")
fa.train_reg_rbfn(rs.lamb)
fa.saveTheta(rs.RBFNpath + name+".theta")
def trajectoriesAnimation(what, foldername="None", targetSize="0.05"):
rs = ReadSetupFile()
if what == "CMAES":
name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.RBFNpath + foldername + "/Log/"
ec = getXYElbowData(name)
hc = getXYHandData(name)
posIni = np.loadtxt(pathDataFolder + rs.experimentFilePosIni)
xEl, yEl, xHa, yHa = [], [], [], []
for key, val in ec.items():
for el in val:
xEl.append(el[0])
yEl.append(el[1])
for elhc in hc[key]:
xHa.append(elhc[0])
yHa.append(elhc[1])
fig = plt.figure()
upperArm, = plt.plot([], [])
foreArm, = plt.plot([], [])
plt.xlim(-0.7, 0.7)
plt.ylim(-0.7, 0.7)
plt.plot([-0.7, 0.7], [rs.YTarget, rs.YTarget])
plt.scatter([-rs.sizeOfTarget[3] / 2, rs.sizeOfTarget[3] / 2],
[rs.YTarget, rs.YTarget],
c='g',
marker='o',
s=50)
plt.scatter([el[0] for el in posIni], [el[1] for el in posIni], c='b')
def init():
upperArm.set_data([0], [0])
foreArm.set_data([xEl[0]], [yEl[0]])
return upperArm, foreArm
def animate(i):
xe = (0, xEl[i])
ye = (0, yEl[i])
xh = (xEl[i], xHa[i])
yh = (yEl[i], yHa[i])
upperArm.set_data(xe, ye)
foreArm.set_data(xh, yh)
return upperArm, foreArm
ani = animation.FuncAnimation(fig,
animate,
init_func=init,
frames=len(xEl),
blit=True,
interval=20,
repeat=True)
plt.show(block=True)
def plotInitPos(filename):
'''
Plots the initial position of trajectories present in the Brent directory
'''
plt.figure()
rs = ReadSetupFile()
makeInitPlot(rs, filename)
plt.show(block=True)
def setPosCircu540():
rs=ReadSetupFile()
filename2 = pathDataFolder + "PosCircu540"
data2 = []
for j in range(20):
for i in range(27):
point = [rs.XTarget+ (0.1+j*0.02)*np.cos(-i*np.pi/40-np.pi/5), rs.YTarget+ (0.1+j*0.02)*np.sin(-i*np.pi/40-np.pi/5)]
data2.append(point)
np.savetxt(filename2,data2)
def setPosSquare():
rs = ReadSetupFile()
filename3 = pathDataFolder + "PosSquare"
data3 = []
for j in range(20):
for i in range(20):
point = [rs.XTarget - 0.3 + i * 0.03, rs.YTarget - 0.4 + j * 0.01]
data3.append(point)
np.savetxt(filename3, data3)
def setPosCircu28():
rs=ReadSetupFile()
filename = pathDataFolder + "PosCircu28"
data2 = []
for j in range(4):
for i in range(7):
point = [rs.XTarget+ (0.1+j*0.1)*np.cos(-i*np.pi/12-np.pi/4), rs.YTarget+ (0.1+j*0.1)*np.sin(-i*np.pi/12-np.pi/4)]
data2.append(point)
np.savetxt(filename,data2)
def launchCMAESForAllTargetSizesMulti():
'''
Launch in parallel (on differents processor) the cmaes optimization for each target size
'''
#initializes setup variables
rs = ReadSetupFile()
#initializes a pool of worker, ie multiprocessing
p = Pool()
#run cmaes on each targets size on separate processor
p.map(launchCMAESForSpecificTargetSize, rs.sizeOfTarget, "theta")
def plotMuscularActivations(what, 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
'''
rs = ReadSetupFile()
if what == "CMAES":
name = rs.CMAESpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.RBFNpath + foldername + "/Log/"
U = getNoiselessCommandData(name)
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.figure()
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("ImageBank/" + what + "_muscu" + key + foldername +
".png",
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 plotScattergram(what, foldername):
rs = ReadSetupFile()
data = {}
if what == "CMAES":
for i in range(len(rs.sizeOfTarget)):
name = rs.CMAESpath + str(
rs.sizeOfTarget[i]) + "/" + foldername + "/finalX/"
tmp = getLastXData(name)
tabx = []
for el in tmp.values():
for j in range(len(el)):
tabx.append(el[j])
data[rs.sizeOfTarget[i]] = tabx
plt.figure(1, figsize=(16, 9))
for i in range(len(rs.sizeOfTarget)):
ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
ax.hist(data[rs.sizeOfTarget[i]], 20)
ax.plot([-rs.sizeOfTarget[i] / 2, -rs.sizeOfTarget[i] / 2],
[0, 500],
c='r',
linewidth=3)
ax.plot([rs.sizeOfTarget[i] / 2, rs.sizeOfTarget[i] / 2], [0, 500],
c='r',
linewidth=3)
ax.set_title(
str("Hit Dispersion for Target " + str(rs.sizeOfTarget[i])))
elif what == "RBFN":
name = rs.RBFNpath + 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.sizeOfTarget)):
plt.plot([-rs.sizeOfTarget[i] / 2, -rs.sizeOfTarget[i]] / 2,
[0, 20],
c='r',
linewidth=3)
plt.plot([rs.sizeOfTarget[i] / 2, rs.sizeOfTarget[i]] / 2, [0, 20],
c='r',
linewidth=3)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("Hit Dispersion for RBFN")
plt.savefig("ImageBank/" + what + "_hitdisp" + foldername + ".png",
bbox_inches='tight')
plt.show(block=True)
def plotTrajsInRepo():
rs = ReadSetupFile()
plt.figure(1, figsize=(16, 9))
plotPos(pathDataFolder + "TrajRepository/", plt, False)
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title("XY Positions")
plt.savefig("ImageBank/TrajRepo.png", bbox_inches='tight')
#plt.savefig("ImageBank/"+what+'_trajectories.png')
plt.show(block=True)
def plotFittsLaw(foldername, rbfn=False):
rs = ReadSetupFile()
timeDistWidth = []
for i in range(len(rs.sizeOfTarget)):
name = rs.CMAESpath + str(
rs.sizeOfTarget[i]) + "/" + foldername + "/TrajTime/"
trajTimes = getTrajTimeData(name)
for k, 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.sizeOfTarget[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, p_value, std_err = stats.linregress(DI, MT)
yLR = slope * np.asarray(DI) + intercept
plt.figure()
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)")
plt.savefig("ImageBank/fitts" + foldername + ".png", bbox_inches='tight')
plt.show(block=True)
def plotFittsLaw(foldername, rbfn = False):
rs = ReadSetupFile()
timeDistWidth = []
for i in range(len(rs.sizeOfTarget)):
name = rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/" + foldername + "/TrajTime/"
trajTimes = getTrajTimeData(name)
for k, 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.sizeOfTarget[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, p_value, std_err = stats.linregress(DI,MT)
yLR = slope * np.asarray(DI) + intercept
plt.figure()
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)")
plt.savefig("ImageBank/fitts"+foldername+".png", bbox_inches='tight')
plt.show(block = True)
def setPosCircu15():
rs=ReadSetupFile()
filename = pathDataFolder + "PosCircu15"
data = []
for i in range(3):
point = [rs.XTarget+ 0.0975*np.cos((-i-1)*np.pi/4), rs.YTarget+ 0.0975*np.sin((-i-1)*np.pi/4)]
data.append(point)
for i in range(5):
point = [rs.XTarget+ 0.243*np.cos(-i*np.pi/8-np.pi/4), rs.YTarget+ 0.243*np.sin(-i*np.pi/8-np.pi/4)]
data.append(point)
for i in range(7):
point = [rs.XTarget+ 0.39*np.cos(-i*np.pi/12-np.pi/4), rs.YTarget+ 0.39*np.sin(-i*np.pi/12-np.pi/4)]
data.append(point)
np.savetxt(filename,data)
def plotManipulability2():
rs = ReadSetupFile()
fig = plt.figure(1, figsize=(16, 9))
arm = Arm()
q1 = np.linspace(-0.6, 2.6, 100, True)
q2 = np.linspace(-0.2, 3, 100, True)
target = [rs.XTarget, rs.YTarget]
pos = []
for i in range(len(q1)):
for j in range(len(q2)):
config = np.array([q1[i], q2[j]])
coordHa = arm.mgdEndEffector(config)
pos.append(coordHa)
x, y, cost = [], [], []
for el in pos:
x.append(el[0])
y.append(el[1])
config = arm.mgi(el[0], el[1])
manip = arm.manipulability(config, target)
cost.append(manip)
xi = np.linspace(-0.7, 0.8, 100)
yi = np.linspace(-0.5, 0.8, 100)
zi = griddata(x, y, cost, xi, yi)
#t1 = plt.scatter(x, y, c=cost, marker=u'o', s=5, cmap=cm.get_cmap('RdYlBu'))
#CS = plt.contourf(xi, xi, zi, 15, cmap=cm.get_cmap('RdYlBu'))
t1 = plt.scatter(x, y, c=cost, s=5, cmap=cm.get_cmap('RdYlBu'))
CS = plt.contourf(xi, yi, zi, 15, cmap=cm.get_cmap('RdYlBu'))
fig.colorbar(t1, shrink=0.5, aspect=5)
plt.scatter(rs.XTarget, rs.YTarget, c="g", marker=u'*', s=200)
#plt.plot([-0.3,0.3], [rs.YTarget, rs.YTarget], c = 'g')
plt.xlabel("X (m)")
plt.ylabel("Y (m)")
plt.title(str("Manipulability map"))
plt.savefig("ImageBank/manipulability2.png", bbox_inches='tight')
plt.show(block=True)
def plotExperimentSetup():
rs = ReadSetupFile()
fig = plt.figure(1, figsize=(16, 9))
arm = Arm()
q1 = np.linspace(-0.6, 2.6, 100, True)
q2 = np.linspace(-0.2, 3, 100, True)
posIni = np.loadtxt(pathDataFolder + rs.experimentFilePosIni)
xi, yi = [], []
xb, yb = [0], [0]
t = 0
for el in posIni:
if el[1] == np.min(posIni, axis=0)[1] and t == 0:
t += 1
a, b = arm.mgi(el[0], el[1])
a1, b1 = arm.mgdFull(np.array([[a], [b]]))
xb.append(a1[0])
xb.append(b1[0])
yb.append(a1[1])
yb.append(b1[1])
xi.append(el[0])
yi.append(el[1])
pos = []
for i in range(len(q1)):
for j in range(len(q2)):
coordHa = arm.mgdEndEffector(np.array([[q1[i]], [q2[j]]]))
pos.append(coordHa)
x, y = [], []
for el in pos:
x.append(el[0])
y.append(el[1])
plt.scatter(x, y)
plt.scatter(xi, yi, c='r')
plt.scatter(0, 0.6175, c="r", marker=u'*', s=200)
plt.plot(xb, yb, c='r')
plt.plot([-0.3, 0.3], [0.6175, 0.6175], c='g')
plt.savefig("ImageBank/setup.png", bbox_inches='tight')
plt.show(block=True)
def plotCMAESCostProgress():
rs = ReadSetupFile()
fig = plt.figure(1, figsize=(16, 9))
for i in range(len(rs.sizeOfTarget)):
ax = plt.subplot2grid((2, 2), (i / 2, i % 2))
name = rs.CMAESpath + str(rs.sizeOfTarget[i]) + "/Cost/cmaesCost.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("Cost Target " + str(rs.sizeOfTarget[i])))
plt.savefig("ImageBank/costProgress.png")
plt.show(block=True)
def saveRandomNN():
rs = ReadSetupFile()
fa = NeuralNet(rs.inputDim, rs.outputDim)
fa.saveTheta(rs.NNpath + "Random.theta")
def generateRichDataFromRBFN(repeat, thetaFile, saveDir):
rs = ReadSetupFile()
thetaName = rs.RBFNpath + thetaFile
saveName = rs.RBFNpath + saveDir + "/"
GenerateRichDataFromTheta(rs, 0.05, saveName, thetaName, repeat, False)
print("RBFN:End of generation")
def launchCMAESForAllTargetSizes(thetaname, save):
rs = ReadSetupFile()
for el in rs.sizeOfTarget:
launchCMAESForSpecificTargetSize(el, thetaname, save)
