#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)
(0.0,np.pi/2), # ring
(0.0,np.pi/2),
(0.0,np.pi/2),
(0.0,np.pi/2), # pinky
(0.0,np.pi/2),
(0.0,np.pi/2))
cnt = 0
hurray = 0
startTime = time.time()
for i in range(len(calcBdata[0][1:])):
# estimating 4 magnets with 4 sensors
res = modE.estimate_BtoAng(np.concatenate((estAngInd[i], estAngMid[i], estAngRin[i], estAngPin[i])),
[phalInd,phalMid,phalRin,phalPin],
[jointInd,jointMid,jointRin,jointPin],
[s1,s2,s3,s4],
np.concatenate((calcBdata[0][i+1],calcBdata[1][i+1],calcBdata[2][i+1],calcBdata[3][i+1])),
bnds)
if res.success:
hurray += 1
else:
print "error, iteration: ",cnt
estAngInd[i+1] = res.x[0:3]
estAngMid[i+1] = res.x[3:6]
estAngRin[i+1] = res.x[6:9]
estAngPin[i+1] = res.x[9:12]
cnt += 1
# estAngIndPy[i+1] = resPy.x[0:3]
# estAngMidPy[i+1] = resPy.x[3:6]
(0.0,np.pi/2),
(0.0,np.pi/2), # MCP
(0.0,np.pi/(180/110)), # PIP
(0.0,np.pi/2),
(0.0,np.pi/2), # MCP
(0.0,np.pi/(180/110)), # PIP
(0.0,np.pi/2))
### perfect values
func = np.zeros((len(angles),))
estAng = np.zeros((len(angles),len(fingerList)*2))
cnt = 0
errCnt = 0
startTime = time.time()
for i in range(len(calcBInd_m[1:])):
res = modE.estimate_BtoAng(estAng[cnt],fingerList,yOffList,sensList,calcBInd_m[cnt+1],bnds[:2])
if not res.success:
print "error, iter: ",cnt
errCnt += 1
estAng[cnt+1] = res.x
func[cnt] = res.fun
print "estimating ",cnt
cnt += 1
print "time needed for estimation: ", time.time()-startTime
## fitted values
#errCnt2 = 0
#estAngMeas = np.zeros((len(measB),2*len(fingerList)))
##estAngMeas = np.zeros((len(measB),3))
#startTime = time.time()
# func[cnt] = res[1]
# print "estimating ",cnt
# cnt += 1
#print "time needed for estimation: ", time.time()-startTime
#print "errCnt: ", errCnt
#print "max func: ", max(func)
## fitted values
errCnt2 = 0
estAngMeas = np.zeros((len(measB),2*len(fingerList)))
funcMeas = np.zeros((len(measB),))
startTime = time.time()
for i in range(len(measB[1:])):
print "estimating fitted",i
# 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[:len(fingerList)*2],method='cy')
# if not res.success:
if res[2]['warnflag']:
print "error, iter: ",i
print res
errCnt2 += 1
# estAngMeas[i+1] = estAngMeas[i]
# estAngMeas[i+1] = res.x
estAngMeas[i+1] = res[0]
funcMeas[i] = res[1]
print "time needed for estimation: ", time.time()-startTime
print "nr of errors: ", errCnt2
print "maxFunc: ", max(funcMeas)
errCnt = 0
#f = plt.figure()
#graphMid = f.add_subplot(131)
#graphMid.set_title('angle Mid')
#graphRin = f.add_subplot(132,sharey=graphMid)
#graphRin.set_title('angle Rin')
#graphPin = f.add_subplot(133,sharey=graphMid)
#graphPin.set_title('angle Pin')
#cnt = 0
print "estimating calculated"
for i in range(len(calcBMid[1:])):
# for one magnet and one sensor...
res = modE.estimate_BtoAng(np.concatenate((estAngCalcInd[i],estAngCalcMid[i],estAngCalcRin[i],estAngCalcPin[i])),
[phalInd,phalMid,phalRin,phalPin],
[yInd,yMid,yRin,yPin],
[sInd,sMid,sRin,sPin],
np.concatenate((calcBInd[i+1],calcBMid[i+1],calcBRin[i+1],calcBPin[i+1])),
bnds)
if not res.success:
errCnt += 1
print "error!", cnt
estAngCalcInd[i+1] = res.x[:3]
estAngCalcMid[i+1] = res.x[3:6]
estAngCalcRin[i+1] = res.x[6:9]
estAngCalcPin[i+1] = res.x[9:]
print "estimated nr: ",cnt
# xValues = np.arange(0,i,1)
# graphMid.clear()
# graphRin.clear()
# graphPin.clear()
# graphMid.plot(xValues,estAngCalcMid[:,0][:i],'r',xValues,estAngCalcMid[:,1][:i],'g',xValues,estAngCalcMid[:,2][:i],'b',)
