res = np.zeros((len(data),))
cnt = 0
for i in data:
res[cnt] = (np.sqrt(i[0]**2+i[1]**2+i[2]**2) - ref) / ref
cnt += 1
res *= 100
return res
''' data acquisition '''
#d = datAc.pipeAcquisition("gatttool -t random -b E3:C0:07:76:53:70 --char-write-req --handle=0x000f --value=0300 --listen",
# d = datAc.pipeAcquisition("gatttool -t random -b E7:00:30:16:CD:18 --char-write-req --handle=0x000f --value=0300 --listen",
# 4,measNr=500, fileName="160127_noRot")
d = datAc.textAcquisition("160208_calDat")
# d = datAc.textAcquisition("160212_calDat")
''' calculate and the hard/soft bias values '''
#(hard_old, soft_old) = calcHardSoft(d[0])
#print "hard_old: ", hard_old
#print "soft_old: ", soft_old
''' Freescale '''
#free0 = calcFreescale(d0[0])
#free1 = calcFreescale(d1[0])
#free2 = calcFreescale(d2[0])
#
#off0 = [0.5*free0[0], 0.5*free0[1], 0.5*free0[2]]
#mag0 = np.sqrt(free0[3]+off0[0]**2+off0[1]**2+off0[2]**2)
''' for x-movement '''
t = np.arange(0.06,0.11,(0.05/62))
b_cyl = np.zeros((len(t),3))
b_dip = np.zeros((len(t),3))
cnt = 0
for i in t:
b_cyl[cnt] = modC.calcB_cyl(np.array([i, 0.,0.]),np.array([0., 0.]))*1e+3
b_dip[cnt] = modD.calcB(np.array([i,0.,0.]),np.array([1, 0, 0]))*1e+3
cnt += 1
d = datAc.textAcquisition("151216_xMove")[0]*1e-4
meas = d[9:71]
''' meas fitting '''
range_m = max(meas[:,0]) - min(meas[:,0])
range_s = max(b_cyl[:,0]) - min(b_cyl[:,0])
scaleMeas = range_s/range_m
meas = meas[:,0]*scaleMeas
offsetMeas = b_cyl[0][0] - meas[0]
meas = meas + offsetMeas
meas *= 1
''' re-estimation '''
estCyl = np.zeros((len(b_cyl[1:]),1))
estCyl[0] = t[0]
import numpy as np
import modelDip as modD
import modelCyl as modC
import plotting as plo
import matplotlib.pyplot as plt
import dataAcquisitionMulti as datAc
dP = datAc.textAcquisition("160215_clipPos")[0]*1e-4
dN = datAc.textAcquisition("160215_clipNeg")[0]*1e-4
#dP -= np.mean(dP[:,0][:20])
#dN -= np.mean(dN[:,0][:15])
dP = dP[20:] *[1.,0.,0.]
dN = dN[15:] *[1.,0.,0.]
start = 7.1
stopP = 2.0625
rPos = np.arange(stopP,start,((start-stopP)/len(dP)))[::-1]
stopN = 1.4528
rNeg = np.arange(stopN,start,((start-stopN)/len(dN)))[::-1]
tP = rPos
tN = rNeg
#tP = np.arange(0.03,0.07,(0.03/95))[::-1]
#tN = np.arange(0.04,0.07,(0.03/70))[::-1]
bP = np.zeros((len(tP),3))
bN = np.zeros((len(tN),3))
#s4 = [-0.03154, 0.06755, 0.] # sensor beneath pinky
s4 = [0.04755, -0.03012, 0.]
# length of fingers
rInd = 0.08 # length of index finger (from jointle)
rMid = 0.08829 # length of middle finger (from jointle)
rRin = 0.07979 # length of ring finger (from jointle)
rPin = 0.07215 # length of pinky finger (from jointle)
# lengths of phalanges
phalInd = [0.03038, 0.02728, 0.02234]
phalMid = [0.03640, 0.03075, 0.02114]
phalRin = [0.03344, 0.02782, 0.01853]
phalPin = [0.02896, 0.02541, 0.01778]
b = datAcM.textAcquisition("perfectB")
""" fitting the data to the model """
""" estimating the position from the measurments """
estPos = np.zeros((4,len(b[0]),3))
# initial positions
estPos[0][0] = [jointInd[0]+rInd, jointInd[1], jointInd[2]]
estPos[1][0] = [jointMid[0]+rMid, jointMid[1], jointMid[2]]
estPos[2][0] = [jointRin[0]+rRin, jointRin[1], jointRin[2]]
estPos[3][0] = [jointPin[0]+rPin, jointPin[1], jointPin[2]]
# fixed bnds for position
bndsPos = ((jointInd[0],jointInd[0]+rInd), # index finger
# (jointInd[1]-0.003,jointInd[1]+0.003),
(jointInd[1]-0.0001,jointInd[1]+0.0001),
(jointInd[2]-rInd,jointInd[2]),
''' 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()
# modE.angToB(i,phalPin,jointPin,s3),axis=0)
#
# calcBPin = np.append(calcBPin,modE.angToB(i,phalInd,jointInd,s4)+
# modE.angToB(i,phalMid,jointMid,s4)+
# modE.angToB(i,phalRin,jointRin,s4)+
# modE.angToB(i,phalPin,jointPin,s4),axis=0)
## calcB_old = np.append(calcB_old,modE.evalfuncMagDotH(calcPos[cnt],calcOrien[cnt],s1),axis=0)
# cnt += 1
#calcBInd = calcBInd[1:]
#calcBMid = calcBMid[1:]
#calcBRin = calcBRin[1:]
#calcBPin = calcBPin[1:]
#calcB_old = calcB_old[1:]
# simply get the data from the textfile...
calcBdata = datAc.textAcquisition("151103_perfectB_straight")
#calcBdata = datAc.textAcquisition("151030_perfectB_H")
''' estimating the angles '''
#estAngIndPy = np.zeros((len(t),3))
#estAngMidPy = np.zeros((len(t),3))
#estAngRinPy = np.zeros((len(t),3))
#estAngPinPy = np.zeros((len(t),3))
#estAngIndCy = np.zeros((len(t),3))
#estAngMidCy = np.zeros((len(t),3))
#estAngRinCy = np.zeros((len(t),3))
#estAngPinCy = np.zeros((len(t),3))
estAngInd = np.zeros((len(t),3))
estAngMid = np.zeros((len(t),3))
estAngRin = np.zeros((len(t),3))
estAngPin = np.zeros((len(t),3))
''' script for experimenting with the measured and the modeled B-field '''
import dataAcquisitionMulti as datAc
import numpy as np
import subprocess,time, os
import modelEqMultiCython as modE
import plotting as plo
import matplotlib.pyplot as plt
import perfectBangles as b
cmd = "gatttool -t random -b E3:C0:07:76:53:70 --char-write-req --handle=0x000f --value=0300 --listen"
calcB = datAc.textAcquisition("151102_perfectB")
#(d_index,d_middle,d_ring,d_pinky) = datAc.collectForTime(cmd, 10, 0.01, avgFil=False, avgN=10, fileName="151101_rawMeas_move2")
#(d_index,d_middle,d_ring,d_pinky) = datAc.collectForTime(cmd, 20, 0.01, fileName="151102_rawMeas_ovMove")
data = datAc.textAcquisition("151102_rawMeas_move")
(d_index,d_middle,d_ring,d_pinky) = (data[0],data[1],data[2],data[3])
# absolute positions of wooden-joints
#jointInd = [0.09138, 0.02957, -0.01087] # to wooden-joint(index)
#jointMid = [0.09138, 0.00920, -0.01087] # to wooden-joint(middle)
#jointRin = [0.09138, -0.01117, -0.01087] # to wooden-joint(ring)
#jointPin = [0.09138, -0.03154, -0.01087] # to wooden-joint(pinky)
jointInd = b.jointInd
jointMid = b.jointMid
jointRin = b.jointRin
jointPin = b.jointPin
## position of sensor
#s1 = [0.06755, 0.02957, 0.] # sensor beneath index
#s2 = [0.04755, 0.00920, 0.] # sensor beneath middle
''' script for estimation with multiple sensors per magnet '''
import dataAcquisitionMulti as datAc
import matplotlib.pyplot as plt
import modelEqMultiCython as modE
import numpy as np
from scipy.optimize import *
import plotting as plo
import time,random
''' acquiring the data '''
cmd = "gatttool -t random -b E3:C0:07:76:53:70 --char-write-req --handle=0x000f --value=0300 --listen"
#d = datAc.pipeAcquisition(cmd,4,measNr=199,fileName="151209_four")
d = datAc.textAcquisition("151209_index2")
#d = datAc.collectForTime(cmd,5,wait=0)
#d_ind = datAc.moving_average3d(d[0],10)
#d_mid = datAc.moving_average3d(d[1],10)
#d_rin = datAc.moving_average3d(d[2],10)
#d_pin = datAc.moving_average3d(d[3],10)
index = datAc.moving_average3d(d[0],10)
middle = datAc.moving_average3d(d[1],10)
ring = datAc.moving_average3d(d[2],10)
pinky = datAc.moving_average3d(d[3],10)
#index = np.delete(index,np.s_[1],1)
#middle = np.delete(middle,np.s_[1],1)
#ring = np.delete(ring,np.s_[1],1)
#pinky = np.delete(pinky,np.s_[1],1)
sInd = [-0.03, -0.0, 0.024] # rack1
tmp[cnt] = (i-(s0+zOff))
# b_dip0[cnt] = modD.calcB(i, h0)
# b_cyl0[cnt] = modC.calcB_cyl(i, 0.)
cnt += 1
#plo.plotter2d((b_dip0, b_dip1, b_dip2, b_dip3), ("DIP 0", "1", "2", "3"), shareAxis=False)
#plo.plotter2d((b_cyl0, b_cyl1, b_cyl2, b_cyl3), ("CYL 0", "1", "2", "3"))
#plo.plotter2d((tmp,),("a",))
''' fitting of measured data '''
#d = datAc.textAcquisition("160208_flatFit1")
d = datAc.textAcquisition("160212_flatSens7")
#d = datAc.textAcquisition("160212_flatSens5")
#d[0] -= [ 92.87515152, 286.92060606, -318.85131313] # for 160201_s1_x5-10
#d[1] -= [ 46.86343434, 276.33575758, -423.56191919]
#d[2] -= [ 47.29494949, 264.67717172, -422.53242424]
#d[3] -= [ 74.65848485, 233.96656566, -403.30909091]
#d[0] -= [ 95.95686869, 288.99565657, -316.52959596] # for 160201_y4-0_x5 - 160201_y4-0_x7_2
#d[1] -= [ 49.74555556, 276.99181818, -424.61929293]
#d[2] -= [ 50.31323232, 265.79050505, -421.99272727]
#d[3] -= [ 77.2389899 , 235.17080808, -403.23666667]
#d[0] -= [ -41.76743719, 252.39633166, -391.40733668] # for 160208
#d[1] -= [ -55.70944724 , 230.11417085, -402.72698492]
#d[2] -= [ -60.59075377 , 213.51316583, -400.75899497]
i, 0., # angle rin
0., 0]) # angle pin
cnt += 1
bMid = np.zeros((len(t), 3*len(sensList_dip)))
bRin = np.zeros((len(t), 3*len(sensList_dip)))
cnt = 0
for i in range(len(t)):
# b_cyl[i] = modC.cy.angToB_cyl(angles_cyl[i],fingList_cyl,sensList_cyl,jointList_cyl) # simulating cylindrical model
bMid[i] = modD.cy.angToBm_cy(angles1[i],fingList_dip,sensList_dip,jointList_dip) # simulating dipole model
bRin[i] = modD.cy.angToBm_cy(angles2[i],fingList_dip,sensList_dip,jointList_dip) # simulating dipole model
b_meas = datAc.textAcquisition("160131_allMid2")
b_meas *= 0.01
print "-----------middle----------"
print "INDEX"
(scale1, off1) = datAc.getScaleOff(bMid[:,0:3],b_meas[0])
b_ind = b_meas[0]*scale1+off1
print "MIDDLE"
#(scale2, off2) = datAc.getScaleOff(bMid[:,3:6],b_meas[1])
#b_mid = b_meas[0]*scale2+off2
print "RING"
(scale3, off3) = datAc.getScaleOff(bMid[:,6:9],b_meas[2])
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
# 0, i, # angle mid
# i, 0.4, # angle rin
# i, i*0.3]) # angle pin
cnt += 1
b = np.zeros((len(t), 3*len(sensList)))
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)
''' script for estimation with multiple sensors per magnet '''
import dataAcquisitionMulti as datAc
import matplotlib.pyplot as plt
import modelEqMultiCython as modE
import numpy as np
from scipy.optimize import *
import plotting as plo
import time
''' acquiring the data '''
cmd = "gatttool -t random -b E3:C0:07:76:53:70 --char-write-req --handle=0x000f --value=0300 --listen"
#d = datAc.pipeAcquisition(cmd,4,measNr=199,fileName="151201_onefour1")
d = datAc.textAcquisition("151201_onefour1")
d_ind = datAc.moving_average3d(d[0],10)
d_mid = datAc.moving_average3d(d[1],10)
d_rin = datAc.moving_average3d(d[2],10)
d_pin = datAc.moving_average3d(d[3],10)
sInd = [-0.03, -0.0, 0.024]
sMid = [-0.03, -0.022, 0.024]
sRin = [-0.03, -0.044, 0.024]
sPin = [-0.03, -0.066, 0.024]
sInd2 = [-0.07, 0.0, 0.024]
sMid2 = [-0.07, -0.022, 0.024]
sRin2 = [-0.07, -0.044, 0.024]
sPin2 = [-0.07, -0.066, 0.024]
## print time.time()-startT
## startT = time.time()
# m = np.append(m,datAc.readMagPacket(proc),0)
## t = np.append(t,time.time()-startT)
# cnt += 1
#
#except KeyboardInterrupt:
# pass
#
#proc.kill()
#datAc.saveToFile(m,"160217_MPU_mag")
#d = datAc.sortData(m)
''' take the datapoints for 160217_MPU2 from [300:-1] (for sensor 0 or 1) and display them reversed '''
d = datAc.textAcquisition("160217_MPU2")
oneC = d[0][300:][::-1]
twoC = d[1][300:][::-1]
oneR = d[2][300:][::-1]
twoR = d[3][300:][::-1]
''' plot everything... '''
plt.close('all')
#plt.figure()
#plt.plot(oneC[:,0],'r-',label='cancel1')
#plt.plot(oneC[:,1],'r--')
#plt.plot(oneC[:,2],'r:')
#plt.plot(oneR[:,0],'b-',label='raw1')
#plt.plot(oneR[:,1],'b--')
#plt.plot(oneR[:,2],'b:')
