def plotPos(name, media, plotEstim, rs):
states = getXYHandData(name, 100)
print(len(states.items()))
for _,v in states.items():
posX, posY = [], []
for j in range(len(v)):
posX.append(v[j][0])
posY.append(v[j][1])
dist=rs.getDistanceToTarget(posX[0],posY[0])
if dist<=0.15:
media.plot(posX,posY, c ='b')
elif dist<=0.28:
media.plot(posX,posY, c ='green')
else:
media.plot(posX,posY, c ='red')
if plotEstim==True:
estimStates = getEstimatedXYHandData(name, 100)
for _,v in estimStates.items():
eX, eY = [], []
for j in range(len(v)):
eX.append(v[j][0])
eY.append(v[j][1])
media.plot(eX,eY, c ='black')
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 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.NNpath + foldername + "/Log/"
ec = getXYElbowData(name)
wc = getXYWristData(name)
hc = getXYHandData(name)
posIni = np.loadtxt(pathDataFolder + rs.experimentFilePosIni)
xEl, yEl, xW, yW, xHa, yHa = [], [], [], [], [], []
for key, val in ec.items():
for el in val:
xEl.append(el[0])
yEl.append(el[1])
for ew in wc[key]:
xW.append(ew[0])
yW.append(ew[1])
for elhc in hc[key]:
xHa.append(elhc[0])
yHa.append(elhc[1])
fig = plt.figure()
upperArm, = plt.plot([],[])
foreArm, = plt.plot([],[])
hand, = 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]])
hand.set_data([xW[0]], [yW[0]])
return upperArm, foreArm, hand
def animate(i):
xe = (0, xEl[i])
ye = (0, yEl[i])
xww = (xEl[i], xW[i])
yww = (yEl[i], yW[i])
xh = (xW[i], xHa[i])
yh = (yW[i], yHa[i])
upperArm.set_data(xe, ye)
foreArm.set_data(xww,yww)
hand.set_data(xh, yh)
return upperArm, foreArm, hand
ani = animation.FuncAnimation(fig, animate, init_func=init, frames=len(xEl), blit=True, interval=20, repeat=True)
plt.show(block = True)
def trajectoriesAnimation(what, rs,foldername = "None", targetSize = "0.05"):
if what == "OPTI":
name = rs.OPTIpath + targetSize + "/" + foldername + "/Log/"
elif what == "Brent":
name = BrentTrajectoriesFolder
else:
name = rs.path + 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(1, figsize=(16,9))
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.target_size[3]/2, rs.target_size[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
animation.FuncAnimation(fig, animate, init_func=init, frames=len(xEl), blit=True, interval=20, repeat=True)
plt.show(block = True)
def plotPos(name, media, plotEstim):
states = getXYHandData(name)
factor = min(1, 100./len(states.items()))
for k,v in states.items():
if rd.random()<factor:
posX, posY = [], []
for j in range(len(v)):
posX.append(v[j][0])
posY.append(v[j][1])
media.plot(posX,posY, c ='b')
if plotEstim==True:
estimStates = getEstimatedXYHandData(name)
for k,v in estimStates.items():
if rd.random()<factor:
eX, eY = [], []
for j in range(len(v)):
eX.append(v[j][0])
eY.append(v[j][1])
media.plot(eX,eY, c ='r')
def plotPos(name, media, plotEstim):
states = getXYHandData(name)
factor = min(1, 100. / len(states.items()))
for k, v in states.items():
if rd.random() < factor:
posX, posY = [], []
for j in range(len(v)):
posX.append(v[j][0])
posY.append(v[j][1])
media.plot(posX, posY, c='b')
if plotEstim == True:
estimStates = getEstimatedXYHandData(name)
for k, v in estimStates.items():
if rd.random() < factor:
eX, eY = [], []
for j in range(len(v)):
eX.append(v[j][0])
eY.append(v[j][1])
media.plot(eX, eY, c='r')
