# modE.angToB(i,phalMid,jointMid[j],s1)+
# modE.angToB(i,phalRin,jointRin[j],s1)+
# modE.angToB(i,phalPin,jointPin[j],s1))
# calcBMid[j][cnt] = (modE.angToB(i,phalInd,jointInd[j],s2)+
# modE.angToB(i,phalMid,jointMid[j],s2)+
# modE.angToB(i,phalRin,jointRin[j],s2)+
# modE.angToB(i,phalPin,jointPin[j],s2))
# calcBRin[j][cnt] = (modE.angToB(i,phalInd,jointInd[j],s3)+
# modE.angToB(i,phalMid,jointMid[j],s3)+
# modE.angToB(i,phalRin,jointRin[j],s3)+
# modE.angToB(i,phalPin,jointPin[j],s3))
# calcBPin[j][cnt] = (modE.angToB(i,phalInd,jointInd[j],s4)+
# modE.angToB(i,phalMid,jointMid[j],s4)+
# modE.angToB(i,phalRin,jointRin[j],s4)+
# modE.angToB(i,phalPin,jointPin[j],s4))
calcBMid[j][cnt] = (modE.angToB(i,phalMid,jointMid[j],s2)+
modE.angToB(i,phalRin,jointRin[j],s2))
calcBRin[j][cnt] = (modE.angToB(i,phalMid,jointMid[j],s3)+
modE.angToB(i,phalRin,jointRin[j],s3))
cnt += 1
''' plotting '''
#plo.plotter2d((measDat[0],measDat[1],measDat[2],measDat[3]),("measInd","measMid","measRin","measPin"))
ax = 2
#plt.figure()
#cnt = 0
#for i in calcBInd:
# plt.plot(i[:,ax],label=str(cnt))
angles = np.zeros((len(t)*2,3))
cnt = 0
for i in t:
angles[cnt] = np.array([i, 0., 0.])
cnt += 1
for i in t[::-1]:
angles[cnt] = np.array([i, 0., 0.])
cnt += 1
calcBMid = np.array([[0.,0.,0.]])
#pos = np.array([[0.,0.,0.]])
#orien = np.array([[0.,0.,0.]])
for i in angles:
calcBMid = np.append(calcBMid, modE.angToB(i,phalMid,sMid,yMid),axis=0)
# pos = np.append(pos, [modE.angToP(i,phalMid,jointMid)],axis=0)
# orien = np.append(orien, [modE.angToH(i)],axis=0)
calcBMid = calcBMid[1:]
#pos = pos[1:]
#orien = orien[1:]
''' casadi version '''
estAngCalcCS = [[0.,0.,0.]]
for i in range(len(calcBMid[1:])):
estAngCalcCS = np.append(estAngCalcCS,np.array(estimate_BtoAng(estAngCalcCS[i],
phalMid,yMid,sMid,calcBMid[i+1])).T,axis=0)
#b = angToB(angles[0],phalMid,yMid,sMid)
#est = estimate_BtoAng([0.,0.,0.],phalMid,yMid,sMid,calcBMid[1])
angles[cnt] = np.array([i, 0., 0.])
cnt += 1
#for i in t[::-1]:
# angles[cnt] = np.array([i, 0, 0.])
# cnt += 1
''' calculating the B-field '''
calcBInd = np.zeros((len(angles),3))
calcBMid = np.zeros((len(angles),3))
calcBRin = np.zeros((len(angles),3))
calcBPin = np.zeros((len(angles),3))
# for moving all 4 fingers the same way...
cnt = 0
for i in angles:
calcBInd[cnt] = (modE.angToB(i,phalMid,sInd,yMid)+
modE.angToB(i,phalRin,sInd,yRin)+
modE.angToB(i,phalPin,sInd,yPin)+
modE.angToB(i,phalInd,sInd,yInd))
calcBMid[cnt] = (modE.angToB(i,phalMid,sMid,yMid)+
modE.angToB(i,phalRin,sMid,yRin)+
modE.angToB(i,phalPin,sMid,yPin)+
modE.angToB(i,phalInd,sMid,yInd))
calcBRin[cnt] = (modE.angToB(i,phalMid,sRin,yMid)+
modE.angToB(i,phalRin,sRin,yRin)+
modE.angToB(i,phalPin,sRin,yPin)+
modE.angToB(i,phalInd,sRin,yInd))
calcBPin[cnt] = (modE.angToB(i,phalMid,sPin,yMid)+
modE.angToB(i,phalRin,sPin,yRin)+
modE.angToB(i,phalPin,sPin,yPin)+
modE.angToB(i,phalInd,sPin,yInd))
