#t = np.arange(0.6,1.0,0.001)
angles = np.zeros((len(t),2))
cnt = 0
for i in t:
angles[cnt] = np.array([i, 0])
cnt += 1
b_cyl = np.zeros((len(angles),2))
b_dip = np.zeros((len(angles),3))
fingerPos = np.zeros((len(angles),2))
ang = np.zeros((len(angles),))
cnt = 0
startT = time.time()
for i in angles:
b_cyl[cnt] = modC.angToB_cyl(i,phalMid,sMid,jointMid)
cnt += 1
print "time needed: ", time.time()-startT
''' 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()
angles = np.zeros((len(t),2))
cnt = 0
for i in t:
angles[cnt] = np.array([i, 0])
cnt += 1
b_ind = np.zeros((len(angles),2))
b_mid = np.zeros((len(angles),2))
b_rin = np.zeros((len(angles),2))
b_pin = np.zeros((len(angles),2))
cnt = 0
startT = time.time()
for i in angles:
# b_ind[cnt] = modC.angToB_cyl(i,np.array(phalInd),sInd,jointInd)
b_mid[cnt] = modC.angToB_cyl(i+[0,0.5],np.array(phalMid),sMid,jointMid)
# b_rin[cnt] = modC.angToB_cyl(i*0.2,np.array(phalRin),sRin,jointRin)
# b_pin[cnt] = modC.angToB_cyl(i+[0,i[0]*0.3],np.array(phalPin),sPin,jointPin)
cnt += 1
print "time needed simulation: ", time.time()-startT
''' 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_Ind = np.zeros((len(angles),2))
estAng_Mid = np.zeros((len(angles),2))
estAng_Rin = np.zeros((len(angles),2))
angles = np.zeros((len(t),2*len(sensList)))
cnt = 0
for i in t:
angles[cnt] = np.array([i, 0.,
i, 0.,
i, 0.,
i, 0.])
cnt += 1
b_dip = np.zeros((len(t),3*len(sensList)))
b_cyl = np.zeros((len(t),3*len(sensList)))
cnt = 0
for i in angles:
b_dip[cnt] = modD.cy.angToBm_cy(i,fingerList,sensList,jointList)
b_cyl[cnt] = modC.angToB_cyl(i,fingerList,sensList,jointList)
cnt += 1
# caliPos = calcBInd_m[0]
(scale, off) = datAc.getScaleOff(b_dip, meas)
meas_fit = meas * scale + off
plo.plotter2d((b_dip[:,:3], b_dip[:,3:6], b_dip[:,6:9], b_dip[:,9:]),
("dipole index","dipole middle","dipole ring","dipole pinky"))
plo.plotter2d((meas_fit[:,:3], meas_fit[:,3:6], meas_fit[:,6:9], meas_fit[:,9:]),
("meas_fit index","meas_fit middle","meas_fit ring","meas_fit pinky"))
plo.plotter2d((meas[:,:3], meas[:,3:6], meas[:,6:9], meas[:,9:]),
("meas_raw index","meas_raw middle","meas_raw ring","meas_raw pinky"))
for i in t:
angles[cnt] = [i, 0]
cnt += 1
#angles = angles[::-1]
b_cyl = np.zeros((len(angles),2))
#b_cyl_cy = np.zeros((len(angles),2))
b_dip = np.zeros((len(angles),3))
pos_py = np.zeros((len(angles),2))
pos_cy = np.zeros((len(angles),2))
ang = np.zeros((len(angles),))
cnt = 0
startT = time.time()
for i in angles:
b_cyl[cnt] = modC.angToB_cyl(i+[0,0.5],np.array(phalMid),np.array(sMid),np.array(jointMid))
# b_cyl_py[cnt] = modC.angToB_cyl(i,phalMid,sMid,jointMid)
# b_cyl_py[cnt] = modC.angToB_cyl(i,np.array(phalMid),np.array(sMid),np.array(jointMid),'py')
# b_cyl_cy[cnt] = modC.angToB_cyl(i,np.array(phalMid),np.array(sMid),np.array(jointMid),'cy')
cnt += 1
print "time needed PY: ", time.time()-startT
#d = b_cyl_py-b_cyl_cy
#p = modC.cel_bul(1.,1.,1.,1.)
#c = modC.cy.cel_bul_cy(1.,1.,1.,1.)
#print "python: ", p
#print "cython: ", c
''' estimation '''
#startT = time.time()
