def openClaw(m1,m2,m3,m4,m5,slp,ser):
# Apertura suave de la herramienta
puntos=[120,80]
m6=calcTrayectoria(puntos,0.5,slp)
length=int(0.5/slp)
m6p=m6.tolist()
# Tomo último valor del resto de los servos para evitar movimientos
m1p=[fixM1(m1)]*length
m2p=[fixM2(m2)]*length
m3p=[fixM3(m3)]*length
m4p=[fixM4(m4)]*length
m5p=[fixM5(m5)]*length
transmSerie(m1p,m2p,m3p,m4p,m5p,m6p,slp,ser)
m6p=[m6p[-1]]*len(m1p)
return[m6p]
def retCero(L1,L2,L3,L4,timeStep,slp,mOld,cero,m6,ser):
[m1,m2,m3,m4,m5,mNew,TH_0]=volverCero(L1,L2,L3,L4,timeStep,slp,mOld,cero)
# Cargo la posicion Cero en Old, para usar como posicion inicial
# en proximas trayectorias
m1Old=cero[0]
m2Old=cero[1]
m3Old=cero[2]
m4Old=cero[3]
m5Old=cero[4]
[m1New,m2New,m3New,m4New,m5New,m1p,m2p,m3p,m4p,m5p]=ajustesVariables(m1,m2,m3,m4,m5)
transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(timeStep/slp),slp,ser)
m1Old=m1New
m2Old=m2New
m3Old=m3New
m4Old=m4New
m5Old=m5New
mOld=[m1Old,m2Old,m3Old,m4Old,m5Old]
return[m1Old,m2Old,m3Old,m4Old,m5Old,mOld,m1p,m2p,m3p,m4p,m5p,m1,m2,m3,m4,m5]
def transmitir(m1p,m2p,m3p,m4p,m5p,m6,slp,ser,timeStep,m1,m2,m3,m4,m5,m1New,m2New,m3New,m4New,m5New):
while(1):
print """Desea grabar el movimiento?
y/n
"""
print "Please Select:"
selection=raw_input("")
if selection=='y':
dirString='/Users/Dan/Documents/Python Scripts/Videos/'
ts=str(datetime.fromtimestamp(time.time()).strftime('%Y-%m-%d_%H-%M-%S'))
newName='CaptureVideo_TEST_'+ts
[t,s,t_extra,s_extra,st,di]=transmSerieVid(m1p,m2p,m3p,m4p,m5p,[m6]*int(timeStep/slp),slp,ser,dirString,newName)
saveLog(t,s,t_extra,s_extra,st,di,m1,m2,m3,m4,m5,m6)
# Tras transmitir, paso la coordenada nueva a la vieja
m1Old=m1New
m2Old=m2New
m3Old=m3New
m4Old=m4New
m5Old=m5New
mOld=[m1Old,m2Old,m3Old,m4Old,m5Old]
return mOld
elif selection=='n':
t=transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(timeStep/slp),slp,ser)
td=[]
td.append(0)
f=open('/Users/Dan/Documents/Python Scripts/Videos/TiempoTrans_'+str(datetime.fromtimestamp(time.time()).strftime('%Y-%m-%d_%H-%M-%S'))+'.txt','w')
for i in range(len(t)-1):
td.append(t[i+1]-t[i])
f.write(str(round(td[i],3))+' segundos. \n'+ str(round(t[i],3)) +'\nPosición: M1: %s; M2:%s; M3:%s; M4:%s; M5:%s; M6:%s.\n\n'%(m1[i],m2[i],m3[i],m4[i],m5[i],m6))
f.write('Tiempo máximo de transmisión: '+str(round(max(td),3))+' segundos.\n')
f.write('Tiempo total: '+str(sum(td))+' segundos.')
f.close()
# Tras transmitir, paso la coordenada nueva a la vieja
m1Old=m1New
m2Old=m2New
m3Old=m3New
m4Old=m4New
m5Old=m5New
mOld=[m1Old,m2Old,m3Old,m4Old,m5Old]
return mOld
def movimiento(L1,L2,L3,L4,mOld,mNew,timeStep,slp,ser,m6):
# Angulos pre-ajuste a servo
[m1,m2,m3,m4,m5]=preTrayectoria(L1,L2,L3,L4,mOld,mNew,timeStep,slp)
[m1New,m2New,m3New,m4New,m5New,m1p,m2p,m3p,m4p,m5p]=ajustesVariables(m1,m2,m3,m4,m5)
t=transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(timeStep/slp),slp,ser)
#time.sleep(1)
m1Old=m1New
m2Old=m2New
m3Old=m3New
m4Old=m4New
m5Old=m5New
mOld=[m1Old,m2Old,m3Old,m4Old,m5Old]
return [mOld,m1p,m2p,m3p,m4p,m5p,m1,m2,m3,m4,m5]
def autoCalib(L1, L2, L3, L4, mOld1, slp, ser, step, m6):
dire = '/Users/Dan/Documents/Python Scripts/Camera Calibration/'
mtx = np.load(dire + 'matrix1.npy')
dist = np.load(dire + 'dist1.npy')
newcameramtx = np.load(dire + 'newcameramtx1.npy')
m1 = fixM1(0)
m2 = 10
m3 = 0
m4 = 20
m5 = fixM2(0)
m6 = 80
m2v = []
m3v = []
m4v = []
timeStep = 1
step = 2
newName = '0' + '.avi'
directory = '/Users/Dan/Documents/Python Scripts/VideosCalibracion/'
fvid = directory + newName
#Posiciones de calibracion
m2p = [50, 65, 80]
m2p4 = [15]
m3p = [55, 40, 10]
m3p4 = [100]
m4p = [100, 75, 50]
m4p1 = [100]
i = 0
transmSerie([m1], [m2], [m3p[i]], [m4p[i]], [m5], [m6], slp, ser)
time.sleep(5)
camera = cv2.VideoCapture(0)
video = cv2.VideoWriter(fvid, 1, 10.0, (640, 480))
while i < 3:
# Voy a posición del proximo
transmSerie([m1], [m2], [m3p[i]], [m4p[i]], [m5], [m6], slp, ser)
time.sleep(5)
k = 0
while k < 2:
while m2 <= 110:
m2v.append(m2)
transmSerieA([m1], [m2], [m3p[i]], [m4p[i]], [m5], [m6], slp,
ser)
m2 = m2 + 5
time.sleep(2)
f, img = camera.read() #flush el frame viejo
f, img = camera.read() #adquiero frame actual
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
video.write(dst)
time.sleep(2)
mOld1 = [m1, m2, m3p[i], m4p[i], m5]
m2 = 10
k = k + 1
mNew = [m1, m2, m3p[i], m4p[i], m5]
[m1t, m2t, m3t, m4t, m5t] = preTrayectoria(L1, L2, L3, L4, mOld1,
mNew, step, slp)
transmSerie(m1t, m2t, m3t, m4t, m5t, [m6] * len(m1t), slp, ser)
time.sleep(5)
i = i + 1
video.release()
camera.release()
#transmSerieA([m1],[m2p[0]],15,[m4p[0]],[m5],[m6],slp,ser) #para evitar el juego del brazo incialmente
newName = '1' + '.avi'
fvid = directory + newName
camera = cv2.VideoCapture(0)
video = cv2.VideoWriter(fvid, 1, 10.0, (640, 480))
i = 0
while i < 3:
# Voy a posición del proximo
transmSerieA([m1], [m2p[i]], [m3], [m4p[i]], [m5], [m6], slp, ser)
time.sleep(2)
k = 0
while k < 2:
while m3 <= 110:
m3v.append(m3)
transmSerieA([m1], [m2p[i]], [m3], [m4p[i]], [m5], [m6], slp,
ser)
m3 = m3 + 5
time.sleep(2)
f, img = camera.read() #flush el frame viejo
f, img = camera.read() #adquiero frame actual
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
video.write(dst)
time.sleep(2)
mOld1 = [m1, m2p[i], m3, m4p[i], m5]
m3 = 0
k = k + 1
mNew = [m1, m2p[i], m3, m4p[i], m5]
[m1t, m2t, m3t, m4t, m5t] = preTrayectoria(L1, L2, L3, L4, mOld1,
mNew, step, slp)
transmSerie(m1t, m2t, m3t, m4t, m5t, [m6] * len(m1t), slp, ser)
time.sleep(2)
i = i + 1
video.release()
camera.release()
newName = '2' + '.avi'
fvid = directory + newName
camera = cv2.VideoCapture(0)
video = cv2.VideoWriter(fvid, 1, 10.0, (640, 480))
i = 0
#while i<3:
# Voy a posición del proximo
transmSerieA([m1], [m2p4[i]], [m3p4[i]], [m4], [m5], [m6], slp, ser)
time.sleep(2)
k = 0
while k < 6:
while m4 <= 100:
m4v.append(m4)
transmSerieA([m1], [m2p4[i]], [m3p4[i]], [m4], [m5], [m6], slp,
ser)
m4 = m4 + 5
time.sleep(2)
f, img = camera.read() #flush el frame viejo
f, img = camera.read() #adquiero frame actual
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
video.write(dst)
time.sleep(2)
mOld1 = [m1, m2p4[i], m3p4[i], m4, m5]
m4 = 10
k = k + 1
mNew = [m1, m2p4[i], m3p4[i], m4, m5]
[m1t, m2t, m3t, m4t, m5t] = preTrayectoria(L1, L2, L3, L4, mOld1, mNew,
step, slp)
transmSerie(m1t, m2t, m3t, m4t, m5t, [m6] * len(m1t), slp, ser)
time.sleep(2)
#i=i+1
video.release()
camera.release()
np.save(directory + 'm2_motor', m2v)
np.save(directory + 'm3_motor', m3v)
np.save(directory + 'm4_motor', m4v)
return [m2v, m3v, m4v]
def jacob(L1,L2,L3,L4,mOld,timeStep,slp,ser,m6):
#Primero calculo la trayectoria del punto actual a donde comienza el circulo
#que desarrolla el jacobiano
qpitch=0;qroll=0;x=15;y=0;z=24;
[m1New,m2New,m3New,m4New,m5New,status]=cinInv(qpitch,qroll,x,y,z,L1,L2,L3,L4)
if status==0:
m1New=m1New/math.pi*180
m2New=m2New/math.pi*180
m3New=m3New/math.pi*180
m4New=m4New/math.pi*180
m5New=m5New/math.pi*180
mNew=[m1New,m2New,m3New,m4New,m5New]
[m1,m2,m3,m4,m5]=preTrayectoria(L1,L2,L3,L4,mOld,mNew,timeStep,slp)
[m1New,m2New,m3New,m4New,m5New,m1p,m2p,m3p,m4p,m5p]=ajustesVariables(m1,m2,m3,m4,m5)
m1=m1.tolist()
m2=m2.tolist()
m3=m3.tolist()
m4=m4.tolist()
m5=m5.tolist()
t=transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(len(m1p)),slp,ser)
[m1j,m2j,m3j,m4j,m5j]=Jacobiano(L1,L2,L3,L4)
m1j=(m1j/math.pi*180)
m2j=(m2j/math.pi*180)
m3j=(m3j/math.pi*180)
m4j=(m4j/math.pi*180)
m5j=(m5j/math.pi*180)
[m1New,m2New,m3New,m4New,m5New,m1jp,m2jp,m3jp,m4jp,m5jp]=ajustesVariables(m1j,m2j,m3j,m4j,m5j)
t=transmSerie(m1jp,m2jp,m3jp,m4jp,m5jp,[m6]*int(len(m1p)),slp,ser)
#Concateno vectores
#Vector Para Graficar
m1.extend(m1j)
m2.extend(m2j)
m3.extend(m3j)
m4.extend(m4j)
m5.extend(m5j)
# Vectores a Transmitir
m1p.extend(m1jp)
m2p.extend(m2jp)
m3p.extend(m3jp)
m4p.extend(m4jp)
m5p.extend(m5jp)
# Grafico
plt.plot(m1)
plt.plot(m2)
plt.plot(m3)
plt.plot(m4)
plt.plot(m5)
plt.legend(['m1', 'm2', 'm3', 'm4','m5'], loc='best')
plt.show()
return [m1p,m2p,m3p,m4p,m5p,m1,m2,m3,m4,m5]
else:
return [0,0,0,0,0]
def loopPosiciones(L1, L2, L3, L4, mOld1, slp, ser, step, m6):
m1 = 0
#m2=[140,106,73,40]
m2 = [130, 100, 70, 40]
m3 = [-90, -60, -30, 0]
#m4=[-90,-43,4,50]
m4 = [-100, -60, -35, 0]
m5 = 0
#m6=80
timeStep = 1
step = 2
#Posicion de Arranque
mNew = [m1, m2[0], m3[0], m4[0], m5]
[m1t, m2t, m3t, m4t, m5t] = preTrayectoria(L1, L2, L3, L4, mOld1, mNew,
step, slp)
[m1New, m2New, m3New, m4New, m5New, m1p, m2p, m3p, m4p,
m5p] = ajustesVariables(m1t, m2t, m3t, m4t, m5t)
#Transmito
#transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(step/slp),slp,ser)
time.sleep(1)
mOld = [m1, m2[0], m3[0], m4[0], m5]
m2v = []
m3v = []
m4v = []
m2s = []
m3s = []
m4s = []
while timeStep <= 1:
for k in range(0, 3):
for l in range(0, 3):
for m in range(0, 3):
mNew = [m1, m2[k + 1], m3[l + 1], m4[m + 1], m5]
[m1t, m2t, m3t, m4t,
m5t] = preTrayectoria(L1, L2, L3, L4, mOld, mNew,
timeStep, slp)
[
m1Newp, m2Newp, m3Newp, m4Newp, m5Newp, m1p, m2p, m3p,
m4p, m5p
] = ajustesVariables(m1t, m2t, m3t, m4t, m5t)
m2v.extend(m2t)
m3v.extend(m3t)
m4v.extend(m4t)
# dirStringVar='/Users/Dan/Documents/Python Scripts/VideosLoopVariables/'
#
# np.save(dirStringVar+str(timeStep)+'_Step_'+str(k)+str(l)+str(m)+'_m2',m2t)
# np.save(dirStringVar+str(timeStep)+'_Step_'+str(k)+str(l)+str(m)+'_m3',m3t)
# np.save(dirStringVar+str(timeStep)+'_Step_'+str(k)+str(l)+str(m)+'_m4',m4t)
m2s.extend(m2p)
m3s.extend(m3p)
m4s.extend(m4p)
# Transmito y Grabo
#t=transmSerie(m1p,m2p,m3p,m4p,m5p,[m6]*int(timeStep/slp),slp,ser)
dirString = '/Users/Dan/Documents/Python Scripts/VideosLoopDay/'
newName = str(timeStep) + '_Step_' + str(k) + str(l) + str(
m)
[t, s, t_extra, s_extra, st,
di] = transmSerieVid(m1p, m2p, m3p, m4p, m5p,
[m6] * int(timeStep / slp), slp, ser,
dirString, newName)
# saveLog(t,s,t_extra,s_extra,st,di,[m1]*int(timeStep/slp),m2t,m3t,m4t,[m5]*int(timeStep/slp),m6)
saveLog2(m2t, m3t, m4t, t, t_extra, timeStep, k, l, m, s,
s_extra)
saveLog(t, s, t_extra, s_extra, st, dirString, m1t, m2t,
m3t, m4t, m5t, m6)
#np.save(dirStringVar+str(timeStep)+'_Step_'+str(k)+str(l)+str(m)+'_time',s)
time.sleep(1)
mOld = [m1, m2[k + 1], m3[l + 1], m4[m + 1], m5]
# Defino posicion a volver para proxima iteración de grabación
mNew[0] = m1
mNew[4] = m5
#Condiciones de M4
if m == 2:
mNew[3] = m4[0]
elif m < 2:
mNew[3] = m4[m + 1]
#Condiciones de M3
if l == 2 & m == 2:
mNew[2] = m3[0]
elif m == 2:
mNew[2] = m3[l + 1]
else:
mNew[2] = m3[l]
#Condiciones de M2
if k == 2 & l == 2 & m == 2:
mNew[1] = m2[0]
elif l == 2 & m == 2:
mNew[1] = m2[k + 1]
else:
mNew[1] = m2[k]
[m1_0, m2_0, m3_0, m4_0,
m5_0] = preTrayectoria(L1, L2, L3, L4, mOld, mNew,
timeStep, slp)
[
m1Newp, m2Newp, m3Newp, m4Newp, m5Newp, m1p0, m2p0,
m3p0, m4p0, m5p0
] = ajustesVariables(m1_0, m2_0, m3_0, m4_0, m5_0)
# Transmito SIN Grabar
transmSerie(m1p0, m2p0, m3p0, m4p0, m5p0,
[m6] * int(timeStep / slp), slp, ser)
time.sleep(1)
mOld[0] = m1
mOld[1] = mNew[1]
mOld[2] = mNew[2]
mOld[3] = mNew[3]
mOld[4] = m5
m2v.extend(m2_0)
m3v.extend(m3_0)
m4v.extend(m4_0)
m2s.extend(m2p0)
m3s.extend(m3p0)
m4s.extend(m4p0)
timeStep = timeStep * 2
#Vuelvo a Posicion Inicial antes de volver al programa principal
[m1t, m2t, m3t, m4t, m5t] = preTrayectoria(L1, L2, L3, L4, mOld1, mNew,
step, slp)
[m1New, m2New, m3New, m4New, m5New, m1p, m2p, m3p, m4p,
m5p] = ajustesVariables(m1t, m2t, m3t, m4t, m5t)
#Transmito
transmSerie(m1p, m2p, m3p, m4p, m5p, [m6] * int(timeStep / slp), slp, ser)
#Grafico trayectorias reales y en valores de servo
figure(1)
plt.plot(m2v)
#figure(2)
plt.plot(m3v)
#figure(3)
plt.plot(m4v)
plt.legend(['m2', 'm3', 'm4'],
'upper center',
bbox_to_anchor=(0.5, -0.05),
fancybox=True,
shadow=True,
ncol=5)
plt.show()
figure(2)
plt.plot(m2s)
#figure(2)
plt.plot(m3s)
#figure(3)
plt.plot(m4s)
plt.legend(['m2', 'm3', 'm4'],
'upper center',
bbox_to_anchor=(0.5, -0.05),
fancybox=True,
shadow=True,
ncol=5)
plt.show()
return [m2v, m3v, m4v]