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"))
Esempio n. 4
0
#                                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.),
Esempio n. 5
0
# 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()
Esempio n. 7
0
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"))
Esempio n. 8
0
#    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)
Esempio n. 10
0
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)    
Esempio n. 11
0
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()
Esempio n. 12
0
#                               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)):
Esempio n. 13
0
##    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)))
Esempio n. 14
0


''' 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"))


Esempio n. 15
0
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()
Esempio n. 17
0
#    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()
Esempio n. 21
0
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"))                
Esempio n. 22
0
    
#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))
Esempio n. 24
0
#        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()
Esempio n. 25
0
#    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()