print "delta y estPos[0]-Index", max(estPos[0][:,1])-min(estPos[0][:,1])
print "delta y estPos[1]-Middle", max(estPos[1][:,1])-min(estPos[1][:,1])
print "delta y estPos[2]-Ring", max(estPos[2][:,1])-min(estPos[2][:,1])
print "delta y estPos[3]-Pinky", max(estPos[3][:,1])-min(estPos[3][:,1])
# remove the y-axis motion...
#estPos[0][:,1] = jointInd[1]
#estPos[1][:,1] = jointMid[1]
#estPos[2][:,1] = jointRin[1]
#estPos[3][:,1] = jointPin[1]
""" plotting stuff """
#plo.plotter3d((pos[0],pos[1],pos[2], pos[3]),("Ind real","mid Real", "ring Real", "pin Real"))
#plo.plotter2d((summedInd,summedMid,summedRin,summedPin),("index","Mid","Rin","Pin"))
plo.multiPlotter(estPos[0],"Index")
plo.multiPlotter(estPos[1],"Middle")
plo.multiPlotter(estPos[2],"Ring")
plo.multiPlotter(estPos[3],"Pinky")
#plt.figure("callTime Position")
#plt.hist(lapPos[:,0],100)
#plt.figure("callHist Position")
#plt.hist(lapPos[:,1],100)
#plt.figure("callTime Angle")
#plt.hist(lapAng[:,0],100)
#plt.figure("callHist Angle")
#plt.hist(lapAng[:,1],100)
#plt.figure("angles")
#plt.plot(estAng[0][:,0])
#plt.plot(estAng[1][:,0])
angInd = [0.02957, 0.09138, 0.01087]
rInd = 0.08
for i in t:
# position of the index finger
pos1 = np.append(pos1, [[angInd[0],
angInd[1]+rInd*np.cos(i),
angInd[2]+rInd*np.sin(i)]],
axis=0)
pos1 = pos1[1:]
calcB = [[0.,0.,0.]]
s1 = [0.02957, 0.06755, 0.]
for i in range(pos1.shape[0]):
calcB = np.append(calcB,
modE.evalfuncMagDot(pos1[i],s1), axis=0)
calcB = calcB[1:]
#P0 = [0.02957, 0.17138, 0.01087]
#S = [0.02957, 0.06755, 0.]
#B = [0. , -182.50857868, -19.10688867]
estPos = np.zeros((pos1.shape[0],3))
estPos[0] = pos1[0]
''' the estimation loop '''
for i in range(pos1.shape[0]-1):
# print "ESTPOS.SHAPE: ", estPos[i].shape
estPos[i+1] = estimate(estPos[i],s1,calcB[i+1]).T
plo.multiPlotter(estPos,"Index",pos1)
#
