#a = modC.estimateAng_cyl([0,0], phalMid, sMid, jointMid, b_cyl[-1])
#print "time needed: ", time.time()-startT
#print a.x
#print angles[-1]
estAngm = np.zeros((len(angles),len(sensList)*2))
estAng_Ind = np.zeros((len(angles),2))
#estAng_Mid = np.zeros((len(angles),2))
#estAng_Rin = np.zeros((len(angles),2))
#estAng_Pin = np.zeros((len(angles),2))
##tolerance = 1.e-06
#cnt = 0
startT = time.time()
for i in range(len(b_ind[1:])):
print "estimating step ",i ," of ", len(angles)
temp = modC.estimateAng_cyl(estAngm[i], fingList, sensList, jointList, b_multB[i+1])
estAngm[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Ind[i], np.array(phalInd), sInd, jointInd, b_ind[i+1])
# estAng_Ind[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Mid[i], np.array(phalMid), sMid, jointMid, b_mid[i+1])
# estAng_Mid[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Rin[i], np.array(phalRin), sRin, jointRin, b_rin[i+1])
# estAng_Rin[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Pin[i], np.array(phalPin), sPin, jointPin, b_pin[i+1])
# estAng_Pin[i+1] = temp.x
cnt += 1
print "time needed estimation: ", time.time()-startT
plt.close('all')
#plo.plotter2d((b_mid,),("index",))
#plo.plotter2d((b_mult,b_ind,b_mid,b_rin,b_pin),("multi","index","middle","ring","pinky"))
''' estimation '''
#startT = time.time()
#a = modC.estimateAng_cyl([0,0], phalMid, sMid, jointMid, b_cyl[-1])
#print "time needed: ", time.time()-startT
#print a.x
#print angles[-1]
estAng = np.zeros((len(angles),2))
stat = np.zeros((len(angles),2))
tolerance = 1.e-06
cnt = 0
startT = time.time()
for i in b_cyl[1:]:
startEst = time.time()
temp = modC.estimateAng_cyl(estAng[cnt], phalMid, sMid, jointMid, i)
stat[cnt] = [time.time()-startEst, temp.fun]
estAng[cnt+1] = temp.x
cnt += 1
print "time needed: ", time.time()-startT
d = datAc.textAcquisition('150814_pinkyOff')
plt.close('all')
plo.plotter2d((b_cyl,estAng,angles),("model","estimated Angles","perfectAngles"),shareAxis=False)
#plo.plotter2d((b_cyl,angles),("model","angles"),shareAxis=False)
#plt.figure()
#plo.plotter2d((d,),("meas",))
plt.show()
#print "time needed: ", time.time()-startT
#print a.x
#print angles[-1]
estAng_Ind = np.zeros((len(angles),2))
estAng_Mid = np.zeros((len(angles),2))
estAng_Rin = np.zeros((len(angles),2))
estAng_Pin = np.zeros((len(angles),2))
#tolerance = 1.e-06
cnt = 0
startT = time.time()
for i in range(len(b_ind[1:])):
print "estimating step ",i ," of ", len(angles)
# temp = modC.estimateAng_cyl(estAng_Ind[i], np.array(phalInd), sInd, jointInd, b_ind[i+1])
# estAng_Ind[i+1] = temp.x
temp = modC.estimateAng_cyl(estAng_Mid[i], np.array(phalMid), sMid, jointMid, b_mid[i+1])
estAng_Mid[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Rin[i], np.array(phalRin), sRin, jointRin, b_rin[i+1])
# estAng_Rin[i+1] = temp.x
# temp = modC.estimateAng_cyl(estAng_Pin[i], np.array(phalPin), sPin, jointPin, b_pin[i+1])
# estAng_Pin[i+1] = temp.x
cnt += 1
print "time needed estimation: ", time.time()-startT
plt.close('all')
#plo.plotter2d((b_mid,),("index",))
plo.plotter2d((b_ind,b_mid,b_rin,b_pin),("index","middle","ring","pinky"))
#plt.figure()
plo.plotter2d((estAng_Ind,estAng_Mid,estAng_Rin,estAng_Pin),("angle Ind","angle Mid","angle Rin","angle Pin"))
''' estimation... '''
bnds = ((0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2),
(0.,np.pi/2))
print "estimation running..."
estAng = np.zeros((len(fingB3D),2*len(fingList)))
cnt = 0
startT = time.time()
for i in fingB3D[1:]:
tmp = modC.estimateAng_cyl(estAng[cnt], fingList, sensList, jointList, i, bnds)
print tmp.fun
estAng[cnt+1] = tmp.x
cnt += 1
print "time needed: ", time.time()-startT
plt.close('all')
plo.plotter2d((fingB3D[:,:3],fingB3D[:,3:6],fingB3D[:,6:9],fingB3D[:,9:]),("ind","mid","ring","pinky"))
plt.figure()
plo.plotter2d((estAng[:,:2],estAng[:,2:4],estAng[:,4:6],estAng[:,6:]),("ANGLEind","mid","ring","pinky"))
#print "time needed: ", time.time()-startT
#print a
#print "estimated angle: ",a.x
#print "real angle: ",angles[-1]
bnds = ((0.,np.pi/2),(0,np.pi/2))
estAng = np.zeros((len(angles),2))
stat = np.zeros((len(angles),2))
tolerance = 1.e-06
cnt = 0
startT = time.time()
for i in b_cyl[1:]:
# print "estimating...",cnt
# startEst = time.time()
temp = modC.estimateAng_cyl(estAng[cnt], np.array(phalMid), np.array(sMid), np.array(jointMid), i)
# stat[cnt] = [time.time()-startEst, temp.fun]
estAng[cnt+1] = temp.x
# estAng[cnt+1] = temp[0]
cnt += 1
print "time needed: ", time.time()-startT
#d = datAc.textAcquisition('150814_pinkyOff')
plt.close('all')
#plo.plotter2d((b_cyl,angles),("B_mid","perfect angles"),shareAxis=False)
plo.plotter2d((b_cyl,estAng,angles),("model","estimated Angles","perfectAngles"),shareAxis=False)
#plo.plotter2d((b_cyl_py,b_cyl_cy,angles),("PY","CY","angles"),shareAxis=False)
#plt.figure()
#plo.plotter2d((d,),("meas",))
