%Declaramos nuestros eslabones
L1= Link('revolute', 'd',39,'a',0,'alpha',pi/2);
L2= Link('revolute', 'd',0,'a',52,'alpha',0,'offset',pi/2);
L3= Link('revolute', 'd',0,'a',0,'alpha',pi/2);
L4= Link('prismatic', 'theta',0,'a',0,'alpha',-pi/2);
L5= Link('revolute', 'd',0,'a',0,'alpha',pi/2);
L6= Link('revolute', 'd',13,'a',0,'alpha',0);
%Agregamos los límites de nuestro de la articulación prismática L4.qlim=[22 40];
%Creamos nuestra base
Base=eye(4);
%Cambiamos la posicion de la base a 90° del eje y para que se posiciones el
%robot en la pared
Base=trotx(180);
%Trasladamos la posición de la base del robot a las coordenadas (124,0,170)
Base(1,4)=124;
Base(3,4)=170;
%Creamos una matriz identidad para la herramienta
Tooll= eye(4);
%Le damos una logitud de 75 mm a la herramienta
Tooll(3,4)=18;
%Unimos todas las parted ya declaradas del robot
Robot_arm=SerialLink([L1 L2 L3 L4 L5 L6])
%Añadimos la base en lq que se desea que esté el robot
Robot_arm.base=Base
%Añadimos la herramietna al robot
Robot_arm.tool=Tooll
%Mostramos al robot
Robot_arm.plot([0 0 0 0 0 0],'workspace',[0 300 -150 150 0 300],'jaxes','joints','jointdiam',1.2,'base')
%Robot_arm.teach()
hold on
%Gráfica del primer campo de colisión Col1=transl(160,-150,40)*troty(90)
Col11=transl(160,150,40)*trotx(180)
trplot(Col1,'color','r','frame','Colision1','length',350,'thick',3)
trplot(Col11,'color','r','frame','Colision1','length',350,'thick',3) %Gráfica del segundo campo de colisión
Col2=transl(170,-150,120)
Col22=transl(170,150,120)*trotx(180)*troty(90)
trplot(Col2,'color','r','frame','Colision2','length',350,'thick',3)
trplot(Col22,'color','r','frame','Colision2','length',350,'thick',3)
%Posición del objeto Inicio y Fin ObIn=transl(124,0,30)*troty(180)
ObFi=transl(178,0,95)*troty(90)
%Gráfica de los puntos de inicio y fin trplot(ObIn,'color','b','frame','Inicio','length',20,'thick',2)
trplot(ObFi,'color','g','frame','Fin','length',20,'thick',2)