def calibrateData(procCmd):
# calData = datAc.pipeAcquisition(procCmd,4,measNr=150)
calData = datAc.collectForTime(procCmd,5)
# convert to Tesla
# print "type calData", type(calData)
# print "type calData[0]",type(calData[0])
nInd = calData[0]*1e-7
nMid = calData[1]*1e-7
nRin = calData[2]*1e-7
nPin = calData[3]*1e-7
# reassign the tuple...
calData = None
calData = (nInd, nMid, nRin, nPin)
# calculating the B-field
fingerList = [h.phalInd]
jointList = [h.jointInd]
sensList = [h.sInd,h.sMid,h.sRin,h.sPin]
t = np.arange(0,1/2.*np.pi,0.01)
angles = np.zeros((len(t),3))
cnt = 0
for i in t:
angles[cnt] = np.array([i, 0., 0.])
cnt += 1
# calcB_dip = np.zeros((len(t),3*len(sensList)))
calcB_cyl = np.zeros((len(t),3*len(sensList)))
cnt = 0
for i in angles:
# calcB_dip[cnt] = modD.cy.angToBm_cy(i,fingerList,sensList,jointList)
calcB_cyl[cnt] = modC.cy.angToB_cyl(i,fingerList,sensList,jointList)
cnt += 1
# fitting to dipole model
(scaleInd,offInd) = datAc.getScaleOff(calcB_cyl[:,0:3] , calData[0])
(scaleMid,offMid) = datAc.getScaleOff(calcB_cyl[:,3:6] , calData[1])
(scaleRin,offRin) = datAc.getScaleOff(calcB_cyl[:,6:9] , calData[2])
(scalePin,offPin) = datAc.getScaleOff(calcB_cyl[:,9:12], calData[3])
scaleArr = np.array([scaleInd, scaleMid, scaleRin, scalePin])
offArr = np.array([offInd, offMid, offRin, offPin])
cInd = calData[0]*scaleInd+offInd
cMid = calData[1]*scaleMid+offMid
cRin = calData[2]*scaleRin+offRin
cPin = calData[3]*scalePin+offPin
plo.plotter2d((cInd,cMid,cRin,cPin),("calIndex","calMid","calRin","calPin"))
plt.figure()
plo.plotter2d((calcB_cyl[:,0:3],calcB_cyl[:,3:6],calcB_cyl[:,6:9],calcB_cyl[:,9:]),
("simIndex","simMid","simRin","simPin"))
return (scaleArr, offArr)
print "maxFunc: ", max(funcMeas)
''' recalculate the B-field with the estimated angles '''
estCalcB = np.zeros((len(estAngMeas),len(sensList)*3))
cnt = 0
for i in estAngMeas:
for j in range(len(fingerList)):
estCalcB[cnt] += modE.angToB_m2(np.array([i[j*2],i[j*2+1]]),fingerList[j],sensList,yOffList[j])
cnt += 1
dif = measB-estCalcB
plt.close('all')
#plo.plotter2d((calcBInd_m[:,:3],calcBInd_m[:,3:6],calcBInd_m[:,6:9],calcBInd_m[:,9:]),("sInd","sMid","sRin","sPin"))
#plo.plotter2d((d_ind,d_mid,d_rin,d_pin),("measInd","measMid","measRin","measPin"))
plo.plotter2d((calcBInd_m[:,:3],measB[:,:3],estCalcB[:,:3]),("ind","measInd","estInd"),shareAxis=True)
plo.plotter2d((calcBInd_m[:,3:6],measB[:,3:6],estCalcB[:,3:6]),("mid","measMid","estMid"),shareAxis=True)
plo.plotter2d((calcBInd_m[:,6:9],measB[:,6:9],estCalcB[:,6:9]),("rin","measRin","estRin"),shareAxis=True)
plo.plotter2d((calcBInd_m[:,9:],measB[:,9:],estCalcB[:,9:]),("pin","measPin","estPin"),shareAxis=True)
plo.plotter2d((dif[:,:3],dif[:,3:6],dif[:,6:9],dif[:,9:]),("difInd","difMid","difRin","difPin"))
#plt.figure()
#plt.plot(estAng[:,0],'r')
#plt.plot(estAng[:,1],'g')
#plt.plot(estAng[:,2],'b')
# plt.title('perfectB')
plt.figure()
plt.plot(estAngMeas[:,0],'r')
plt.plot(estAngMeas[:,1],'g')
plt.plot(estAngMeas[:,1]*(2./3.),'b')
plt.title('index')
# estAngMid[i+1] = res.x[3:6]
# estAngRin[i+1] = res.x[6:9]
# estAngPin[i+1] = res.x[9:12]
# pipeStr = ''
# for i in res.x:
# pipeStr = pipeStr + " {0:.4f}".format(abs(i))
# # put the angles on the pipe...
# try: #thumb #index #middle #ring #pinky
# pipeout.write("0.0000 0.0000 0.0000" + pipeStr)
# pipeout.flush()
# except OSError,e:
# print "error! listener disconnected"
# os.unlink(mPath)
# break
#plt.close('all')
#plo.plotter2d((d_index,calcB[0],d_middle,calcB[1],d_ring,calcB[2],d_pinky,calcB[3]),
# ("ind", "calcInd", "mid", "calcMid","rin", "calcRin", "pin", "calcPin"))
plo.plotter2d((calcB[0],calcB[1],calcB[2],calcB[3]),("calc index","calc middle","calc ring","calc pinky"))
plo.plotter2d((d_index,d_middle,d_ring,d_pinky),("meas index","meas middle","meas ring","meas pinky"))
plo.plotter2d((avgIndex,avgMiddle,avgRing,avgPinky),("avg index","avg middle","avg ring","avg pinky"))
#plo.plotter2d((fitIndex,calcB[0]),("measIndex","calcIndex"),shareAxis=True)
#plo.plotter2d((fitMiddle,calcB[1]),("measMiddle","calcMiddle"),shareAxis=True)
#plo.plotter2d((fitRing,calcB[2]),("measRing","calcRing"),shareAxis=True)
#plo.plotter2d((fitPinky,calcB[3]),("measPinky","calcPinky"),shareAxis=True)
#plo.plotter2d((d_index,avgIndex,calcB[0]),("measIndex","avg","calcIndex"),shareAxis=False)
#plo.plotter2d((d_middle,avgMiddle,calcB[1]),("measMiddle","avg","calcMiddle"),shareAxis=False)
#plo.plotter2d((d_ring,avgRing,calcB[2]),("measRing","avg","calcRing"),shareAxis=False)
#plo.plotter2d((d_pinky,avgPinky,calcB[3]),("measPinky","avg","calcPinky"),shareAxis=False)
#plo.plotter2d((estAngInd,estAngMid,estAngRin,estAngPin),("ind", "mid", "rin", "pin"))
# i, 0., # angle rin
# i, 0]) # angle pin
cnt += 1
b_cyl = np.zeros((len(t), 3*len(sensList)))
b_dip = np.zeros((len(t), 3*len(sensList)))
cnt = 0
for i in range(len(t)):
b_cyl[i] = modC.cy.angToB_cyl(angles_cyl[i],fingList,sensList,jointList) # simulating cylindrical model
b_dip[i] = modD.cy.angToBm_cy(angles_cyl[i],fingList,sensList,jointList) # simulating dipole model
#plt.close('all')
#plo.plotter2d((b_cyl,b_dip),("cyl","dip"),shareAxis=False)
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((b_cyl[:,:3], b_cyl[:,3:6], b_cyl[:,6:9], b_cyl[:,9:]),
("cyl index","cyl middle","cyl ring","cyl pinky"))
#plo.plotter2d((b_cyl,),("mid",))
''' estimation '''
## cylindrical model
estAng_cyl = np.zeros((len(b_cyl), 2*len(fingList)))
fun_cyl = np.zeros((len(b_cyl),))
bnds_cyl = ((0.0,np.pi/2.),
(0.0,np.pi/2.),
(0.0,np.pi/2.),
(0.0,np.pi/2.),
(0.0,np.pi/2.),
(0.0,np.pi/2.),
# REMEMBER: only add the fields, that you realy need!
summedInd=np.zeros(shape=(len(calcBInd[0]),3))
summedInd+=(calcBInd[0]+calcBInd[1]+calcBInd[2]+calcBInd[3])
#summedInd+=(calcBInd[0])
summedMid=np.zeros(shape=(len(calcBMid[0]),3))
summedMid+=(calcBMid[0]+calcBMid[1]+calcBMid[2]+calcBMid[3])
summedRin=np.zeros(shape=(len(calcBRin[0]),3))
summedRin+=(calcBRin[0]+calcBRin[1]+calcBRin[2]+calcBRin[3])
summedPin=np.zeros(shape=(len(calcBPin[0]),3))
summedPin+=(calcBPin[0]+calcBPin[1]+calcBPin[2]+calcBPin[3])
''' save it to a file in the desired format '''
#fi = open("151030_perfectB_H",'w')
#for i in range(len(summedInd)):
# fi.write(str(0) + "\t" + str(summedInd[i][0]) + "\t" +
# str(summedInd[i][1]) + "\t" +
# str(summedInd[i][2]) + "\n")
# fi.write(str(1) + "\t" + str(summedMid[i][0]) + "\t" +
# str(summedMid[i][1]) + "\t" +
# str(summedMid[i][2]) + "\n")
# fi.write(str(2) + "\t" + str(summedRin[i][0]) + "\t" +
# str(summedRin[i][1]) + "\t" +
# str(summedRin[i][2]) + "\n")
# fi.write(str(3) + "\t" + str(summedPin[i][0]) + "\t" +
# str(summedPin[i][1]) + "\t" +
# str(summedPin[i][2]) + "\n")
#fi.close()
plo.plotter2d((summedInd, summedMid, summedRin, summedPin),("ind","mid","rin","pin"))
cnt = 1
for i in b_sim_noise:
# for i in b_sim:
# print "---------step ",cnt,"--------"
(x_p, P_p) = kalman.EKF_predict_dip(x_EKF[cnt - 1], P, Q)
# print "x_p\n", x_p
# print "P_p\n", P_p
(x_EKF[cnt], P) = kalman.EKF_update_dip(kalman.jacoCalcB, i, x_p, P_p, R)
cnt += 1
x_est = np.zeros((len(t) + 1, 3))
x_est[0] = r[0]
cnt = 0
for i in b_sim_noise:
print "---------normal estimation step ", cnt, "--------"
tmp = modD.estimatePos(x_est[cnt], h, i)
x_est[cnt + 1] = tmp.x
cnt += 1
# plt.figure()
plo.plotter2d((x_EKF[:-2], x_est), ("EKF states", "estimated states"))
plt.show()
b_rin = b_meas[2]*scale3+off3
print "PINKY"
(scale4, off4) = datAc.getScaleOff(bMid[:,9:],b_meas[3])
b_pin = b_meas[3]*scale4+off4
#plo.plotter2d((bMid[:,0:3],b_ind,b_meas[0]),("sim index","index fit","raw"))
#plo.plotter2d((bMid[:,3:6],b_mid,b_meas[1]),("sim middle","middle fit","raw"))
#plo.plotter2d((bMid[:,6:9],b_rin,b_meas[2]),("sim ring","ring fit","raw"))
#plo.plotter2d((bMid[:,9:], b_pin,b_meas[3]),("sim pinky","pinky fit","raw"))
b_meas = datAc.textAcquisition("160131_allRin2")
b_meas *= 0.01
print "-----------ring----------"
print "INDEX"
#(scale1, off1) = datAc.getScaleOff(bRin[:,0:3],b_meas[0])
b_ind = b_meas[0]*scale1+off1
print "MIDDLE"
(scale2, off2) = datAc.getScaleOff(bRin[:,3:6],b_meas[1])
b_mid = b_meas[0]*scale2+off2
print "RING"
#(scale3, off3) = datAc.getScaleOff(bRin[:,6:9],b_meas[2])
b_rin = b_meas[2]*scale3+off3
print "PINKY"
#(scale4, off4) = datAc.getScaleOff(bRin[:,9:],b_meas[3])
b_pin = b_meas[3]*scale4+off4
plo.plotter2d((bRin[:,0:3],b_ind,b_meas[0]),("sim index","index fit","raw"))
plo.plotter2d((bRin[:,3:6],b_mid,b_meas[1]),("sim middle","middle fit","raw"))
plo.plotter2d((bRin[:,6:9],b_rin,b_meas[2]),("sim ring","ring fit","raw"))
plo.plotter2d((bRin[:,9:], b_pin,b_meas[3]),("sim pinky","pinky fit","raw"))
# estAngPinPy[i+1] = resPy.x[9:12]
#
# estAngIndCy[i+1] = resCy.x[0:3]
# estAngMidCy[i+1] = resCy.x[3:6]
# estAngRinCy[i+1] = resCy.x[6:9]
# estAngPinCy[i+1] = resCy.x[9:12]
print "time needed: ",time.time()-startTime
'''----------PLOTTING----------'''
#plt.close('all')
plo.plotter2d((calcBdata[0],calcBdata[1],calcBdata[2],calcBdata[3]),("oldind","oldmid","oldrin","oldpin"))
#plo.plotter2d((calcBInd,calcBMid,calcBRin,calcBPin),("ind","mid","rin","pin"))
plo.plotter2d((estAngInd,estAngMid,estAngRin,estAngPin),("angleInd","angleMid","angleRin","anglePin"))
''' code for comparing python and cython results with the perfect angles '''
#pyDev0 = estAngIndPy-angles
#pyDev1 = estAngMidPy-angles
#pyDev2 = estAngRinPy-angles
#pyDev3 = estAngPinPy-angles
#
#cyDev0 = estAngIndCy-angles
#cyDev1 = estAngMidCy-angles
#cyDev2 = estAngRinCy-angles
#cyDev3 = estAngPinCy-angles
#plo.plotter2d((estAngIndPy,estAngMidPy,estAngRinPy,estAngPinPy),("angleInd","angleMid","angleRin","anglePin"))
#plo.plotter2d((estAngIndCy,estAngMidCy,estAngRinCy,estAngPinCy),("angleIndCy","angleMidCy","angleRinCy","anglePinCy"))
s1 = s1[:300]
s2 = s2[:300]
s3 = s3[:300]
s4 = s4[:300]
#print "fitting for fist"
#(scale, off) = datAc.getScaleOff(b_A[:,:3],s1[157:315])
#s1_fit = s1*scale+off
##plo.plotter2d((b_A[:,:3], s1_fit[157:315], s1[157:315]),("sim", "fitted", "meas"))
print "fitting for 90"
start = 42
end = 85
(scale, off) = datAc.getScaleOff(b_A[:,:3],s1[start:end])
s1_fit = s1*scale+off
plo.plotter2d((b_A[:,:3], s1_fit[start:end], s1[start:end]),("sim1", "fitted", "meas"))
(scale, off) = datAc.getScaleOff(b_A[:,3:6],s2[start:end])
s2_fit = s2*scale+off
#plo.plotter2d((b_A[:,3:6], s2_fit[start:end], s2[start:end]),("sim2", "fitted", "meas"))
(scale, off) = datAc.getScaleOff(b_A[:,6:9],s3[start:end])
s3_fit = s3*scale+off
plo.plotter2d((b_A[:,6:9], s3_fit[start:end], s3[start:end]),("sim3", "fitted", "meas"))
(scale, off) = datAc.getScaleOff(b_A[:,9:],s4[start:end])
s4_fit = s4*scale+off
#plo.plotter2d((b_A[:,9:], s4_fit[start:end], s4[start:end]),("sim4", "fitted", "meas"))
# combining everything again...
s1_fit = np.append(s1_fit,np.append(s2_fit,np.append(s3_fit,s4_fit,1),1),1)
cnt = 0
for i in range(len(t)):
b[i] = modD.cy.angToBm_cy(angles[i],fingList,sensList,jointList) # simulating dipole model
''' fitting '''
d = datAc.textAcquisition("160129_mid3")[1]
d *= 0.01
(scale, off) = datAc.getScaleOff(b,d)
d_fit = d*scale+off
plo.plotter2d((b,d_fit,d),("sim","fitted","raw"),shareAxis=False)
''' estimation '''
P = np.eye(2)
# process noise covariance matrix (2x2)
Q = np.diag([1e+2, 1e-2])
# measurement noise covariance matrix (3x3)
R = np.diag([1e+1, 1e+1, 1e+1])
estAngK = np.zeros((len(d_fit)+1,2))
cnt = 1
for i in d_fit:
(x_p, P_p) = k.EKF_predict_fing(estAngK[cnt-1], P, Q)
b = np.zeros((len(t), 3*len(sensList)))
offset = 3*np.random.rand(12,1).reshape(12,)
scale = np.random.rand(1,12).reshape(12,)+0.3
b_noise = np.zeros((len(t), 3*len(sensList)))
for i in range(len(t)):
b[i] = modD.cy.angToBm_cy(angles[i],fingList,sensList,jointList) # simulating dipole model
b_noise[i] = modD.cy.angToBm_cy(angles[i],fingList,sensList,jointList) #*scale + offset
#plt.close('all')
#plo.plotter2d((b_cyl,b_dip),("cyl","dip"),shareAxis=False)
#plo.plotter2d((b[:,:3], b[:,3:6], b[:,6:9], b[:,9:]),
# ("dipole index","dipole middle","dipole ring","dipole pinky"))
plo.plotter2d((b_noise[:,:3], b_noise[:,3:6], b_noise[:,6:9], b_noise[:,9:]),
("dipole index","dipole middle","dipole ring","dipole pinky"))
''' fitting measured data '''
#d = datAc.textAcquisition("160129_all1")
#
#(sInd, oInd) = datAc.getScaleOff(b[0][0:3],d[0])
#d_fit = d[0]*sInd+oInd
''' normal estimation '''
#x_est = np.zeros((len(t)+1, 2*len(fingList)))
#x_est[0] = angles[0]
#
#cnt = 0
#estStart = time.time()
# i*0.5, i,0.])
cnt += 1
for i in t[::-1]:
# simValues_A[cnt] = np.array([i*0.5, i ,0.])
simValues_A[cnt] = np.array([i*0.5, i, -i*0.1, # moving only MCP
i, i, 0.])#,
# i*0.5, i,0.,
# i*0.5, i,0.])
cnt += 1
#plo.plotter2d((simValues_A[:,:3],simValues_A[:,3:6],simValues_A[:,6:9],simValues_A[:,9:]),
# ("perfect angles index","middle","ring","pinky"))
plo.plotter2d((simValues_A,),("perfect angles",))
plt.savefig(folderStr+"perfectAngles.png")
plt.figure()
simValues = np.delete(simValues_A, np.s_[2],1)
saveSimStates(simValues_A,folderStr+"simStates.txt")
#b_cyl = np.zeros((len(simValues_A),3*len(sensList)))
#b_dip = np.zeros((len(simValues_A),3*len(sensList)))
b_cyl_A = np.zeros((len(simValues_A),3*len(sensList)))
#b_dip_A = np.zeros((len(simValues_A),3*len(sensList)))
#noise = 0.5*np.random.randn(1,12)
for i in range(len(simValues_A)):
## res = modE.estimate_BtoAng(estAngMeas[i],fingerList,yOffList,sensList,measB[i+1][3:6],bnds[:3])
# res = modE.estimate_BtoAng(estAngMeas[i],fingerList,yOffList,sensList,measB[i+1],bnds[:2])
#
# if not res.success:
# print "error, iter: ",i
# print res
# errCnt2 += 1
# estAngMeas[i+1] = np.array([res.x[0], res.x[1]])
#print "time needed for estimation: ", time.time()-startTime
#sens_I = calcBInd[:,0:3]
plt.close('all')
#plt.figure()
#plo.plotter2d((calcBInd_m,),("sMid_m",))
plo.plotter2d((calcBInd_m[:,:3],calcBInd_m[:,3:6],calcBInd_m[:,6:9],calcBInd_m[:,9:]),("sInd","sMid","sRin","sPin"))
#plo.plotter2d((d_ind,d_mid,d_rin,d_pin),("measInd","measMid","measRin","measPin"))
#plo.plotter2d((calcBInd_m[:,:3],index),("ind","measInd"),shareAxis=True)
#plo.plotter2d((calcBInd_m[:,3:6],middle),("mid","measMid"),shareAxis=True)
#plo.plotter2d((calcBInd_m[:,6:9],ring),("rin","measRin"),shareAxis=True)
#plo.plotter2d((calcBInd_m[:,9:],pinky),("pin","measPin"),shareAxis=True)
plt.figure()
plt.plot(estAng[:,0])
plt.plot(estAng[:,1])
plt.plot(estAng[:,1]*(2/3))
#plo.plotter2d((estAngMeas,angles),("meas estAngles","perfect estAng"))
#plo.plotter2d((estAngMeas[:,:3],estAngMeas[:,3:6],estAngMeas[:,6:9],estAngMeas[:,9:]),("estInd","estMid","estRin","estPin"))
#plo.plotter2d((estAng[:,:3],estAng[:,3:]),("mid","rin"))
#plt.figure()
#plo.plotter2d((estAngMeas[:,:3],),("estAnglesMid",))
#plt.plot(np.arange(0,np.pi/2,np.pi/2/len(estAngMeas)))
''' 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"))
calc_cross=datAcM.sortData(calc_cross)
calc_dot=np.zeros(shape=[len(calcB_dot),1])
calc_dot=np.append(calc_dot,calcB_dot,axis=1)
calc_dot=datAcM.sortData(calc_dot)
# fitting the data to the model
#offset = [30.,0.,-33.37]
#scale = [2.4505,0.,3.7334]
#bla = middleOff[0][110:370]
#bla = (bla * scale) + offset
#bla=modE.fitMeasurements(calc[0],middleOff[0],(0,110)) # GREAT! the fitting things work here!
#data=np.zeros(shape=[len(bla),1])
#data=np.append(data,bla,axis=1)
#data=datAcM.sortData(data)
# estimating the position from the measurments
#estPos=[[(angle[0]+s0[0]+r), 0.01150, 0.]]
#cnt = 0
#for i in data[0]:
# estPos = np.append(estPos, [modE.estimatePos(estPos[cnt], s0, i)], axis=0)
# cnt+=1
## round everything to 4 decimals
#estPos = np.around(estPos,4)
plo.plotter2d((calc_cross,calc_dot,middleOff),("calc_cross", "calc_dot", "raw meas"), True)
#plo.plotter3d((pos,estPos),("model","calc"))
#plt.clf()
#plt.plot(estPos[:,0], estPos[:,2], color='b')
#plt.plot(pos[:,0], pos[:,2], color='r')
#plt.show()
''' 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()
# estPos[1][cnt+1] = tmp[3:]
# cnt+=1
#estimated = modE.estimatePos(estPos,s0,calcB[1],bnds)
#print "result estimation: ", estimated.reshape(2,1,3)
#print "real position: ", pos[0][1],pos[1][1]
## estimating the measured data
##estPos=[[0.,0.,0.]]
#cnt = 0
## bounds for index finger
#bnds=((angMid[0]+s0[0]-0.003,angMid[0]+s0[0]+0.003),
# (angMid[1]+s0[1],angMid[1]+s0[1]+rMid),
# (angMid[2]+s0[2],angMid[2]+s0[2]+rMid))
## bounds for middle finger
#
#for i in dataS[0]:
## estPos = np.append(estPos, [modE.estimatePos(estPos[cnt], s0,i) * [1.,0.,1.]+[0.,angle[1]+s0[1], 0.]], axis=0)
# estPos = np.append(estPos, [modE.estimatePos(estPos[cnt], s0, i,bnds)], axis=0)
# cnt+=1
## round everything to 4 decimals
#estPos = np.around(estPos,4)
plo.plotter2d((calc,calc2,delta),("calcMiddle","calcPinky","raw"), True)
#plo.plotter3d((pos[0],pos[1], estPos), ("middle","pinky", "estPos"))
#print "delta x: ", max(estPos[:,0])-min(estPos[:,0])
cnt += 1
except KeyboardInterrupt:
print "ended, cnt: ", cnt
print "errorCnt: ", errCnt
procBle.kill()
plt.figure()
# plot angles...
plo.plotter2d((estAng,),("angles",))
# plot B
plo.plotter2d((sensI, sensM, sensR, sensP),
("B ind", "B mid", "B rin", "B pin"))
#plt.show()
# print scaleInd,offInd
# print scaleMid,offMid
# print scaleRin,offRin
# print scalePin,offPin
#if __name__ == "__main__":
# main()
#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"))
plt.show()
#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"))
#plo.plotter2d((b_multA[:,0:2],b_multB[:,0:2],b_multA[:,2:4],b_multB[:,2:4]),("multiA","cyA","multB","cyB"))
#plo.plotter2d((b_mult[:,0:2],b_conA,b_mult[:,2:4],b_conB),("multiA","cyA","multB","cyB"))
plo.plotter2d((estAngm[:,0:2],estAngm[:,2:4],estAngm[:,4:6],estAngm[:,6:]),("ind","mid","rin","pin"))
#plt.figure()
#plo.plotter2d((estAng_Ind,estAng_Mid,estAng_Rin,estAng_Pin),("angle Ind","angle Mid","angle Rin","angle Pin"))
plt.show()
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"))
#b = angToB(angles[0],phalMid,yMid,sMid)
#est = estimate_BtoAng([0.,0.,0.],phalMid,yMid,sMid,calcBMid[1])
''' python version '''
bnds = ((0.0,np.pi/2), # MCP
(0.0,np.pi/(180/110)), # PIP
(0.0,np.pi/2))
estAngCalcMid = np.zeros((len(calcBMid),3))
#error = np.zeros((len(calcBMid),))
cnt = 0
errCnt = 0
for i in range(len(calcBMid[1:])):
# for one magnet and one sensor...
res = modE.estimate_BtoAng(estAngCalcMid[i],
[phalMid],
[yMid],
[sMid],
calcBMid[i+1],bnds)
if not res.success:
errCnt += 1
print "error!", cnt
estAngCalcMid[i+1] = res.x[:3]
cnt += 1
# error[i+1] = res.fun
plt.close('all')
plo.plotter2d((calcBMid,estAngCalcMid,estAngCalcCS),("b","anglePy","angleCS"),shareAxis=False)
cnt = 0
for i in a:
b_cylA[cnt] = modC.calcB_cyl(np.array([r[0], 0]), i)
cnt += 1
#a = modC.calcB_cyl(np.array([r[0], 0]), np.pi/2)
#plt.close('all')
#plt.figure()
#plt.plot(r,b_bar,'r')
#plt.title('bar Magnet')
#plt.figure()
#plt.plot(a,b_cyl[:,0],'r',a,b_cyl[:,1],'g')
#plt.scatter(r,b_cyl1[:,0])
plo.plotter2d((b_cyl1,b_cylA,b_cyl2),("B one","B angle","B two"), shareAxis=True)
#plt.figure()
#plo.plotter2d((pos1,pos2),("POS 1", "POS 2"))
#plt.plot(a,b_cyl[:,0],'r',a,b_cyl[:,1],'g')
#plt.title('cyl formula')
#plt.figure()
#plt.plot(r,b_dip[:,0],'r',r,b_dip[:,1],'g',r,b_dip[:,2],'b')
#plt.title('dip formula')
''' estimation things '''
#a = modC.estimatePos_cyl(np.array([0.04, 0]), 0, b_cyl[1]) # perfect pos for b_cyl[1]: [r[1],0]
#a = modC.estimatePos_cyl(np.array([0, 0]), 0, b_cyl1[1])
#print "RESULT:\n", a
estPos = np.zeros((len(r),2))
# normal estimation
# res = modD.estimate_BtoAng(estAngMeas[-1], fingerList, jointList, sensList, measB)
# estAngMeas = np.append(estAngMeas,[res.x],axis=0)
# fval = np.append(fval,res.fun)
# EKF estimation
(x_p, P_p) = k.EKF_predict_hand(x_EKF[cnt-1], P, Q)
(x_EKF_new, P) = k.EKF_update_hand(j.jacMulti, measB, x_p, P_p, R)
print x_EKF_new
x_EKF = np.append(x_EKF,[x_EKF_new],axis=0)
except KeyboardInterrupt:
print "ended, cnt: ", cnt
print "errorCnt: ", errCnt
procBle.kill()
plo.plotter2d((sensI,sensM,sensR,sensP),("index","middle","ring","pinky"))
# print scaleInd,offInd
# print scaleMid,offMid
# print scaleRin,offRin
# print scalePin,offPin
#if __name__ == "__main__":
# main()
# plt.pause(0.0001)
cnt += 1
''' estimating angles with the fitted values '''
#print "estimating fitted"
##startTime = time.time()
#estAngMeasMid = np.zeros((len(fitMid),3))
##estAngMeasRin = np.zeros((len(fitRin),3))
#for i in range(len(fitMid[1:])):
# res = modE.estimate_BtoAng(estAngMeasMid[i],
# [phalMid],
# [yMid],
# [sMid],
# fitMid[i+1],
# bnds)
#
# if not res.success:
# errCnt += 1
# print "error!", i
# estAngMeasMid[i+1] = res.x[:3]
## estAngMeasRin[i+1] = res.x[3:]
#plt.close('all')
plo.plotter2d((calcBInd,calcBMid,calcBRin,calcBPin),("B-ind","B-mid","B-rin","B-pin"))
plo.plotter2d((estAngCalcInd,estAngCalcMid,estAngCalcRin,estAngCalcPin),("angles Ind","angles Mid","angles Rin","angles Pin"))
plt.show()
