Practica 1 Fundamentos computacionales

Vista previa del material en texto

Practica 1 Fundamentos computacionales
Planificación de movimientos de robots
Objetivos:
A partir del modelo cinemático directo e inverso desarrollado previamente para el robot manipulador Motoman K6SB y el GP7, los
alumnos desarrollaran 2 trayectorias específicas que el eslabón final del robot seguirá.
Indicaciones:
 Para la primera trayectoria en el robot K6SB, partiendo de la posición final del manipulador dibujará un triángulo equilátero
en el espacio: el punto de partida será la mitad de la base del triángulo, el triángulo está en el plano ZY con una posición
constante en el eje X, cada lado del triángulo tiene una dimensión de 200 mm, al finalizar regresara al punto de partida
terminando la simulación.
 La segunda trayectoria en el robot GP7, partiendo de la posición final del manipulador dibujará un círculo en el espacio: El
plano de trabajo es XY, con una posición constante en el eje Z, La posición inicial del efector final será el punto de partida
para dibuja la circunferencia, quedando alineado con la mitad del círculo, al terminar de dibujarlo regresara a la posición
inicial, el radio del círculo es 150mm.
 El número de muestras depende del usuario.
 Entregará 1 archivo .m para cada trayectoria, y un archivo .avi para cada trayectoria.
Resumen:
 Cree una trayectoria para un triángulo equilátero de 200 mm por lado con el robot Motoman K6SB en Matlab.
 Cree una trayectoria para un círculo de150 mm de radio con el robot Motoman G7P en Matlab  Entregue el
archivo .m y el video .avi creado en Matlab.
CODIGO EN MATLAB - TRIANGULO:
%P2_fundcompTriangulo
clc
clear
n=100;
d2=100;%mm
d3=300;
d4=400;
r4=200;
 
deltaini=0;
delta=deltaini;
delta2=deltaini;
delta3=deltaini;
delta4=deltaini;
Alumno: Francisco Gerardo Gómez Rivera. 
Número de control: 18130352.
Materia: Planificación de movimientos de robots.
Profesor: Jesus Eduardo Fierro Proa.
28/10/2022
delta5=deltaini;
delta6=deltaini;
delta7=deltaini;
delta8=deltaini;
delta9=deltaini;
delta10=deltaini;
 
%TM
TM=[1 0 0 0
 0 1 0 0
 0 0 1 0
 0 0 0 1];
 
for i=1:n
 
 if i>0 && i<=20
 delta=delta;
 deltay(i)=deltaini+delta;
 delta2=0;
 deltaz(i)=delta2+deltaini;
 X=0;
 y=0;
 z=300;
 end
 
 if i>20 && i<=40
 delta3=delta3;
 deltay(i)=(deltaini+delta3);
 delta4=delta4;
 deltaz(i)=delta4+deltaini;
 x=0;
 y=100;
 z=300; 
 end
 
 if i>40 && i<=60
 delta5=delta5;
 deltay(i)=(deltaini+delta5);
 delta6=delta6;
 deltaz(i)=delta6+deltaini;
 x=0;
 y=0;
 z=473.20; 
 end
 
 if i>60 && i<=80
 delta7=delta7;
 deltay(i)=deltaini+delta7;
 delta8=0;
 deltaz(i)=delta8+deltaini;
 x=0;
 y=-100;
 z=300; 
 end
 
 if i>80 && i<=100
 delta9=delta9;
 deltay(i)=deltaini+delta9;
 delta10=0;
 deltaz(i)=delta10;
 x=0;
 y=0;
 z=300; 
 end
 
 sx=0;
 nx=-1;
 ax=-1;
 px=600;
 
 sy=0;
 ny=-1;
 ay=-1;
 py=y+deltay(i);
 
 sz=1;
 nz=0;
 az=0;
 pz=z+deltaz(i);
 
 U0=[sx nx ax px
 sy ny ay py
 sz nz az pz
 0 0 0 1];
 
%Desarrollo de ecuaciones del modelo inverso:
%Theta 1:
a1 = py;
b1 = -px;
c1 = 0;
eps1 = 1;
sin1 = (b1*c1+eps1*a1*sqrt((a1^2)+(b1^2)-(c1^2)))/((a1^2)+(b1^2));
cos1 = (a1*c1-eps1*b1*sqrt((a1^2)+(b1^2)-(c1^2)))/((a1^2)+(b1^2));
%tan1 = sin1/cos1;
th1 = atan2(sin1,cos1);
t1(i) = atan2(sin1,cos1);
 
%Theta 2:
a2 = (-2*d3*(px*cos(t1(i)) - d2 + py*sin(t1(i))));
b2 = -2*d3*pz;
c2 = d4^2 + r4^2 - pz^2 - (px*cos(t1(i)) - d2 + py*sin(t1(i)))^2 - d3^2;
eps2 = -1;
sin2 =(b2*c2+eps2*a2*sqrt((a2^2)+(b2^2)-(c2^2)))/((a2^2)+(b2^2));
cos2 =(a2*c2-eps2*b2*sqrt((a2^2)+(b2^2)-(c2^2)))/((a2^2)+(b2^2));
%tan2 = sin2/cos2;
th2 = atan2(sin2, cos2);
t2(i) = atan2(sin2, cos2);
 
%Theta 3:
a3 = -r4;
b3 = d4;
c3 = pz*cos(t2(i)) - sin(t2(i))*(px*cos(t1(i)) + py*sin(t1(i))) + d2*sin(t2(i));
eps3 = -1;
sin3 =(b3*c3+eps3*a3*sqrt((a3^2)+(b3^2)-(c3^2)))/((a3^2)+(b3^2));
cos3 =(a3*c3-eps3*b3*sqrt((a3^2)+(b3^2)-(c3^2)))/((a3^2)+(b3^2));
%tan3 = sin3/cos3;
th3 = atan2(sin3, cos3);
t3(i) = atan2(sin3, cos3);
 
%Theta 4:
a4 = px*sin(t1(i)) - py*cos(t1(i));
b4 = d4 + d3*cos(t3(i)) - cos(t3(i))*(pz*sin(t2(i)) - d2*cos(t2(i)) + cos(t2(i))*(px*cos(t1(i)) + 
py*sin(t1(i)))) - sin(t3(i))*(pz*cos(t2(i)) - sin(t2(i))*(px*cos(t1(i)) + py*sin(t1(i))) + d2*sin(t2(i)));
c4 = 0;
eps4 = 1;
sin4 =(b4*c4+eps4*a4*sqrt((a4^2)+(b4^2)-(c4^2)))/((a4^2)+(b4^2));
cos4 =(a4*c4-eps4*b4*sqrt((a4^2)+(b4^2)-(c4^2)))/((a4^2)+(b4^2));
%tan4 = sin4/cos4; 
th4 = atan2(sin4, cos4);
t4(i) = atan2(sin4, cos4);
 
%Theta 5:
a5= -cos(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - sz*cos(t2(i))) - sin(t3(i))*(sz*sin(t2(i)) + 
cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))));
b5= sin(t4(i))*(sy*cos(t1(i)) - sx*sin(t1(i))) - cos(t4(i))*(cos(t3(i))*(sz*sin(t2(i)) + 
cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i)))) - sin(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - 
sz*cos(t2(i))));
c5= 0;
eps5 = 1;
sin5 =(b5*c5+eps5*a5*sqrt((a5^2)+(b5^2)-(c5^2)))/((a5^2)+(b5^2));
cos5 =(a5*c5-eps5*b5*sqrt((a5^2)+(b5^2)-(c5^2)))/((a5^2)+(b5^2));
%tan5 = sin5/cos5; 
th5 = atan2d(sin5, cos5);
t5(i) = atan2d(sin5, cos5);
 
%Theta 6:
sin6 = - sin(t4(i))*(cos(t3(i))*(sz*sin(t2(i)) + cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i)))) - 
sin(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - sz*cos(t2(i)))) - cos(t4(i))*(sy*cos(t1(i)) - 
sx*sin(t1(i)));
cos6 = - sin(t4(i))*(cos(t3(i))*(nz*sin(t2(i)) + cos(t2(i))*(nx*cos(t1(i)) + ny*sin(t1(i)))) - 
sin(t3(i))*(sin(t2(i))*(nx*cos(t1(i)) + ny*sin(t1(i))) - nz*cos(t2(i)))) - cos(t4(i))*(ny*cos(t1(i)) - 
nx*sin(t1(i)));
eps6 = 1;
%tan6 = simplify(sin6/cos6);
th6 = atan2(sin6, cos6);
t6(i) = atan2(sin6, cos6);
 
%Retomando modelo cinemático directo:
T01=[cos(t1(i)) -sin(t1(i)) 0 0
 sin(t1(i)) cos(t1(i)) 0 0
 0 0 1 0
 0 0 0 1];
 
T12=[cos(t2(i)) -sin(t2(i)) 0 d2
 0 0 -1 0
 sin(t2(i)) cos(t2(i)) 0 0
 0 0 0 1];
 
T23=[cos(t3(i)) -sin(t3(i)) 0 d3
 sin(t3(i)) cos(t3(i)) 0 0
 0 0 1 0
 0 0 0 1];
 
T34=[cos(t4(i)) -sin(t4(i)) 0 d4
 0 0 -1 -r4
 sin(t4(i)) cos(t4(i)) 0 0
 0 0 0 1];
 
T45=[cos(t5(i)) -sin(t5(i)) 0 0
 0 0 1 0
 -sin(t5(i)) -cos(t5(i)) 0 0
 0 0 0 1];
 
T56=[cos(t6(i)) -sin(t6(i)) 0 0
 0 0 -1 0
 sin(t6(i)) cos(t6(i)) 0 0
 0 0 0 1];
 
T06=TM*T01*T12*T23*T34*T45*T56;
T05=TM*T01*T12*T23*T34*T45;
T04=TM*T01*T12*T23*T34;
T03=TM*T01*T12*T23;
T02=TM*T01*T12;
T01=TM*T01;
 
%de x0 a x1, y0 a y1, z0 a z1
r01x=[TM(1,4) T01(1,4)];
r01y=[TM(2,4) T01(2,4)];
r01z=[TM(3,4) T01(3,4)];
 
r12x=[T01(1,4) T02(1,4)];
r12y=[T01(2,4) T02(2,4)];
r12z=[T01(3,4) T02(3,4)];
 
r23x=[T02(1,4) T03(1,4)];
r23y=[T02(2,4) T03(2,4)];
r23z=[T02(3,4) T03(3,4)];
 
r34x=[T03(1,4) T04(1,4)];
r34y=[T03(2,4) T04(2,4)];
r34z=[T03(3,4) T04(3,4)];
 
r45x=[T04(1,4) T05(1,4)];
r45y=[T04(2,4) T05(2,4)];
r45z=[T04(3,4) T05(3,4)];
 
r56x=[T05(1,4) T06(1,4)];
r56y=[T05(2,4) T06(2,4)];
r56z=[T05(3,4) T06(3,4)];
 
%Trazas:
trazax(i)=T06(1,4);
trazay(i)=T06(2,4);
trazaz(i)=T06(3,4);
 
figure(3)
clf
hold on
grid on
plot3(r01x,r01y,r01z
,'y',r12x,r12y,r12z,'y',r23x,r23y,r23z,'y',r34x,r34y,r34z,'y',r45x,r45y,r45z,'y',r56x,r56y,r56z,'y','Linewidth
',2)
plot3(trazax,trazay,trazaz,'k', 'LineWidth',2);
 
xlabel('x');
ylabel('y');
zlabel('z');
axis([-1000 1000 -1000 1000 -1000 1000])
%Az=-160;El=20;
Az=-90;El=0; %lateral
%Az=130;El=30;%isometrica
%Az=-0;El=0; %frente
%Az=0;El=90; %superior
view(Az,El)
drawnow;
F(i)=getframe(figure(3));
end
v=VideoWriter('P2TRIANGULO.avi');
v.FrameRate=15;
open(v);
writeVideo(v,F)
close(v);
Capturas de evidencia:
CODIGO EN MATLAB – CIRCULO:
%P2_fundcompCirculoclc 
clear
n=73;
c=0;
d2=100;%mm
d3=300;
d4=400;
r4=200;
 
%TM
TM=[1 0 0 0
 0 1 0 0
 0 0 1 0
 0 0 0 1];
 
for i=1:n
 sx=1;
 nx=0;
 ax=0;
 px=-75*cosd(c);
 
 sy=1;
 ny=0;
 ay=0;
 py=75*sind(c);
 
 sz=1;
 nz=0;
 az=0;
 pz=700;
 
 c=c+5;
 
 U0=[sx nx ax px
 sy ny ay py
 sz nz az pz
 0 0 0 1];
 
%Desarrollo de ecuaciones del modelo inverso:
%Theta 1:
a1 = py;
b1 = -px;
c1 = 0;
eps1 = 1;
sin1 = (b1*c1+eps1*a1*sqrt((a1^2)+(b1^2)-(c1^2)))/((a1^2)+(b1^2));
cos1 = (a1*c1-eps1*b1*sqrt((a1^2)+(b1^2)-(c1^2)))/((a1^2)+(b1^2));
%tan1 = sin1/cos1;
th1 = atan2(sin1,cos1);
t1(i) = atan2(sin1,cos1);
 
%Theta 2:
a2 = (-2*d3*(px*cos(t1(i)) - d2 + py*sin(t1(i))));
b2 = -2*d3*pz;
c2 = d4^2 + r4^2 - pz^2 - (px*cos(t1(i)) - d2 + py*sin(t1(i)))^2 - d3^2;
eps2 = -1;
sin2 =(b2*c2+eps2*a2*sqrt((a2^2)+(b2^2)-(c2^2)))/((a2^2)+(b2^2));
cos2 =(a2*c2-eps2*b2*sqrt((a2^2)+(b2^2)-(c2^2)))/((a2^2)+(b2^2));
%tan2 = sin2/cos2;
th2 = atan2(sin2, cos2);
t2(i) = atan2(sin2, cos2);
 
%Theta 3:
a3 = -r4;
b3 = d4;
c3 = pz*cos(t2(i)) - sin(t2(i))*(px*cos(t1(i)) + py*sin(t1(i))) + d2*sin(t2(i));
eps3 = -1;
sin3 =(b3*c3+eps3*a3*sqrt((a3^2)+(b3^2)-(c3^2)))/((a3^2)+(b3^2));
cos3 =(a3*c3-eps3*b3*sqrt((a3^2)+(b3^2)-(c3^2)))/((a3^2)+(b3^2));
%tan3 = sin3/cos3;
th3 = atan2(sin3, cos3);
t3(i) = atan2(sin3, cos3);
 
%Theta 4:
a4 = px*sin(t1(i)) - py*cos(t1(i));
b4 = d4 + d3*cos(t3(i)) - cos(t3(i))*(pz*sin(t2(i)) - d2*cos(t2(i)) + cos(t2(i))*(px*cos(t1(i)) + 
py*sin(t1(i)))) - sin(t3(i))*(pz*cos(t2(i)) - sin(t2(i))*(px*cos(t1(i)) + py*sin(t1(i))) + d2*sin(t2(i)));
c4 = 0;
eps4 = 1;
sin4 =(b4*c4+eps4*a4*sqrt((a4^2)+(b4^2)-(c4^2)))/((a4^2)+(b4^2));
cos4 =(a4*c4-eps4*b4*sqrt((a4^2)+(b4^2)-(c4^2)))/((a4^2)+(b4^2));
%tan4 = sin4/cos4; 
th4 = atan2(sin4, cos4);
t4(i) = atan2(sin4, cos4);
 
%Theta 5:
a5= -cos(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - sz*cos(t2(i))) - sin(t3(i))*(sz*sin(t2(i)) + 
cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))));
b5= sin(t4(i))*(sy*cos(t1(i)) - sx*sin(t1(i))) - cos(t4(i))*(cos(t3(i))*(sz*sin(t2(i)) + 
cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i)))) - sin(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - 
sz*cos(t2(i))));
c5= 0;
eps5 = 1;
sin5 =(b5*c5+eps5*a5*sqrt((a5^2)+(b5^2)-(c5^2)))/((a5^2)+(b5^2));
cos5 =(a5*c5-eps5*b5*sqrt((a5^2)+(b5^2)-(c5^2)))/((a5^2)+(b5^2));
%tan5 = sin5/cos5; 
th5 = atan2d(sin5, cos5);
t5(i) = atan2d(sin5, cos5);
 
%Theta 6:
sin6 = - sin(t4(i))*(cos(t3(i))*(sz*sin(t2(i)) + cos(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i)))) - 
sin(t3(i))*(sin(t2(i))*(sx*cos(t1(i)) + sy*sin(t1(i))) - sz*cos(t2(i)))) - cos(t4(i))*(sy*cos(t1(i)) - 
sx*sin(t1(i)));
cos6 = - sin(t4(i))*(cos(t3(i))*(nz*sin(t2(i)) + cos(t2(i))*(nx*cos(t1(i)) + ny*sin(t1(i)))) - 
sin(t3(i))*(sin(t2(i))*(nx*cos(t1(i)) + ny*sin(t1(i))) - nz*cos(t2(i)))) - cos(t4(i))*(ny*cos(t1(i)) - 
nx*sin(t1(i)));
eps6 = 1;
%tan6 = simplify(sin6/cos6);
th6 = atan2(sin6, cos6);
t6(i) = atan2(sin6, cos6);
 
%Retomando modelo cinemático directo:
T01=[cos(t1(i)) -sin(t1(i)) 0 0
 sin(t1(i)) cos(t1(i)) 0 0
 0 0 1 0
 0 0 0 1];
 
T12=[cos(t2(i)) -sin(t2(i)) 0 d2
 0 0 -1 0
 sin(t2(i)) cos(t2(i)) 0 0
 0 0 0 1];
 
T23=[cos(t3(i)) -sin(t3(i)) 0 d3
 sin(t3(i)) cos(t3(i)) 0 0
 0 0 1 0
 0 0 0 1];
 
T34=[cos(t4(i)) -sin(t4(i)) 0 d4
 0 0 -1 -r4
 sin(t4(i)) cos(t4(i)) 0 0
 0 0 0 1];
 
T45=[cos(t5(i)) -sin(t5(i)) 0 0
 0 0 1 0
 -sin(t5(i)) -cos(t5(i)) 0 0
 0 0 0 1];
 
T56=[cos(t6(i)) -sin(t6(i)) 0 0
 0 0 -1 0
 sin(t6(i)) cos(t6(i)) 0 0
 0 0 0 1];
 
T06=TM*T01*T12*T23*T34*T45*T56;
T05=TM*T01*T12*T23*T34*T45;
T04=TM*T01*T12*T23*T34;
T03=TM*T01*T12*T23;
T02=TM*T01*T12;
T01=TM*T01;
 
%de x0 a x1, y0 a y1, z0 a z1
r01x=[TM(1,4) T01(1,4)];
r01y=[TM(2,4) T01(2,4)];
r01z=[TM(3,4) T01(3,4)];
 
r12x=[T01(1,4) T02(1,4)];
r12y=[T01(2,4) T02(2,4)];
r12z=[T01(3,4) T02(3,4)];
 
r23x=[T02(1,4) T03(1,4)];
r23y=[T02(2,4) T03(2,4)];
r23z=[T02(3,4) T03(3,4)];
 
r34x=[T03(1,4) T04(1,4)];
r34y=[T03(2,4) T04(2,4)];
r34z=[T03(3,4) T04(3,4)];
 
r45x=[T04(1,4) T05(1,4)];
r45y=[T04(2,4) T05(2,4)];
r45z=[T04(3,4) T05(3,4)];
 
r56x=[T05(1,4) T06(1,4)];
r56y=[T05(2,4) T06(2,4)];
r56z=[T05(3,4) T06(3,4)];
 
%Trazas:
trazax(i)=T06(1,4);
trazay(i)=T06(2,4);
trazaz(i)=T06(3,4);
 
figure(3)
clf
hold on
grid on
plot3(r01x,r01y,r01z
,'b',r12x,r12y,r12z,'b',r23x,r23y,r23z,'b',r34x,r34y,r34z,'b',r45x,r45y,r45z,'b',r56x,r56y,r56z,'b','Linewidth
',2)
plot3(trazax,trazay,trazaz,'k', 'LineWidth',2);
 
xlabel('x');
ylabel('y');
zlabel('z');
axis([-1000 1000 -1000 1000 -1000 1000])
%Az=-160;El=20;
%Az=-90;El=0; %lateral
%Az=130;El=30;%isometrica
%Az=-0;El=0; %frente
Az=-90;El=90; %superior
view(Az,El)
drawnow;
F(i)=getframe(figure(3));
end
 
v=VideoWriter('P2_circulo.avi');
v.FrameRate=15;
open(v);
writeVideo(v,F)
close(v);
Capturas de evidencia:
CONCLUSIÓN:
El desarrollo de la presente practica fue un tanto extenso, pero de un procedimiento simple, no se presentó alguna complicación
grave para su desarrollo y conclusión de la misma, fue interesante el poder visualizar y programar mediante este lenguaje en el
programa de MATLAB, así pudiendo comprender mas de forma practica el como es que se puede hacer manipular un robot y que
haga diversos diseños, en este caso un triangulo y un circulo con unas medidas específicas.