UNIVERSIDAD POLITÉCNICA DE ZACATECAS

INGENIERÍA MECATRONICA

CINEMATICA DE ROBOTS

JOSE ABRAHAM CENTENO BASURTO

PRACTICA 3 - MATLAB

GABRIEL MUÑ OZ HERNANDEZ

8.1M

FRESNILLO ZAC., 10/ABRIL/2019

CINEMATICA DE ROBOTS ROBOT FERSTO

JOSE ABRAHAM CENTENO BASURTO

1
CINEMATICA DE ROBOTS ROBOT FERSTO

JOSE ABRAHAM CENTENO BASURTO

2
CINEMATICA DE ROBOTS ROBOT FERSTO

JOSE ABRAHAM CENTENO BASURTO

3
CINEMATICA DE ROBOTS ROBOT FERSTO

JOSE ABRAHAM CENTENO BASURTO

4
CINEMATICA DE ROBOTS ROBOT FERSTO

Directo FESTO
CODIGO DE PROGRAMACION
%cinemática directa del FESTO
clear,clc
J1=90;
J2=0;
J3=90;
J5=0;
J6=45;
J1=J1*pi()/180;
J2=J2*pi()/180;
J3=J3*pi()/180;
J5=J5*pi()/180;
J6=J6*pi()/180;
d1=0;
L=300-d1;
M01=[cos(J1) -sin(J1) 0 0;sin(J1) cos(J1) 0 0;0 0 1 d1;0 0 0 1];
A=[1 0 0 0;0 1 0 0;0 0 1 L;0 0 0 1];
B=[0 0 -1 0;0 1 0 0;1 0 0 0;0 0 0 1];
%C=[1 0 0 0;0 0 1 0;0 -1 0 0;0 0 0 1];
D=[cos(J2) -sin(J2) 0 0;sin(J2) cos(J2) 0 0;0 0 1 0;0 0 0 1];
M12=A*B*D;
M23=[cos(J3) -sin(J3) 0 250;sin(J3) cos(J3) 0 0;0 0 1 0;0 0 0 1];
M34=[cos(J5) -sin(J5) 0 160;sin(J5) cos(J5) 0 0;0 0 1 0;0 0 0 1];
E=[1 0 0 72;0 1 0 0;0 0 1 0;0 0 0 1];
F=[0 0 1 0;0 1 0 0;-1 0 0 0;0 0 0 1];
%G=[1 0 0 0;0 0 -1 0;0 1 0 0;0 0 0 1];
H=[cos(J6) -sin(J6) 0 0;sin(J6) cos(J6) 0 0;0 0 1 0;0 0 0 1];
M45=E*F*H;
M05=M01*M12*M23*M34*M45

RESULTADOS
M05 =

0.0000 -0.0000 -1.0000 -232.0000

0.7071 -0.7071 0.0000 0.0000

-0.7071 -0.7071 0.0000 550.0000

0 0 0 1.0000

JOSE ABRAHAM CENTENO BASURTO

5
CINEMATICA DE ROBOTS ROBOT FERSTO

MATLAB

JOSE ABRAHAM CENTENO BASURTO

6
CINEMATICA DE ROBOTS ROBOT FERSTO

Directo FESTO

CODIGO DE PROGRAMACION
%cinemática inversa FESTO
%Posición y orientación deseada
%extraer Posición
Px=M05(1,4);
Py=M05(2,4);
Pz=M05(3,4);
%extraer orientación de eje z en efector final
ax=M05(1,3);
ay=M05(2,3);
az=M05(3,3);
ox=M05(1,2);
oy=M05(2,2);
oz=M05(3,2);
nx=M05(1,1);
ny=M05(2,1);
nz=M05(3,1);
%Dimensiones del robot
l1=300
l2=250
l3=160
l4=72
%cálculo de posición de muñeca
Pmx=Px-l4*ax;
Pmy=Py-l4*ay;
Pmz=Pz-l4*az;
%cálculo de articulaciones de posicionamiento
q1=atan(-Pmy/Pmx);
q21=atan(sqrt(Pmy^2+Pmx^2)/(Pmz-l1-l2));
q22=atan(-sqrt(Pmy^2+Pmx^2)/(Pmz-l1-l2));
q31=-acos((cos(q21)*(cos(q1)*ox+sin(q1)*oy)+cos(q21)*oz));
q32=-acos((sin(q22)*(cos(q1)*ox+sin(q1)*oy)+cos(q22)*oz));
%Conversión a grados
q1=q1*180/pi();
q21=q21*180/pi();
q22=q22*180/pi();
q31=q31*180/pi();
q32=q32*180/pi();
%Cálculo de matriz R031
NOA=[nx ny nz;ox oy oz;ax ay az];
A=[cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 0 1];
B=[0 0 -1;0 1 0;1 0 0];
Rii01=A*B;
Rii12=[cos(q21) -sin(q21) 0;sin(q21) cos(q21) 0;0 0 1];
C=[cos(q31) -sin(q31) 0;sin(q31) cos(q31) 0;0 0 1];
D=[0 0 1;0 1 0;-1 0 0];
Rii23=C*D;
R031=Rii01*Rii12*Rii23;
R031=transpose(R031)*NOA;
%Cálculo de articulaciones de orientación
r13=R031(1,3);
r23=R031(2,3);

JOSE ABRAHAM CENTENO BASURTO

7
CINEMATICA DE ROBOTS ROBOT FERSTO

r33=R031(3,3);
r31=R031(3,1);
r32=R031(3,2);
q4=0;
q51=acos(r33);
q61=atan(-r32/r31);
q1=q1
q4=q4
%Resultados1
Opcion=1
q21=q21
q31=q31
q51=q51
q61=q61
%Cálculo de matriz R032
A=[cos(q1) -sin(q1) 0;sin(q1) cos(q1) 0;0 0 1];
B=[0 0 -1;0 1 0;1 0 0];
Rii01=A*B;
Rii12=[cos(q22) -sin(q22) 0;sin(q22) cos(q22) 0;0 0 1];
C=[cos(q32) -sin(q32) 0;sin(q32) cos(q32) 0;0 0 1];
D=[0 0 1;0 1 0;-1 0 0];
Rii23=C*D;
R032=Rii01*Rii12*Rii23;
R032=transpose(R032)*NOA;
%Cálculo de articulaciones de orientación
r13=R032(1,3);
r23=R032(2,3);
r33=R032(3,3);
r31=R032(3,1);
r32=R032(3,2);
q52=acos(r33);
q62=atan(-r32/r31);
%Resultados1
Opcion=2
q22=q22
q32=q32
q52=q52
q62=q62

RESULTADOS
>> InverseFESTO

l1 =

300

l2 =

JOSE ABRAHAM CENTENO BASURTO

8
CINEMATICA DE ROBOTS ROBOT FERSTO

250

l3 =

160

l4 =

72

q1 =

3.5084e-15

q4 =

Opcion =

q21 =

JOSE ABRAHAM CENTENO BASURTO

9
CINEMATICA DE ROBOTS ROBOT FERSTO

90

q31 =

-90

q51 =

1.5708

q61 =

6.1232e-17

Opcion =

q22 =

-90

q32 =

JOSE ABRAHAM CENTENO BASURTO

10
CINEMATICA DE ROBOTS ROBOT FERSTO

-90

q52 =

2.1730

q62 =

0.7580

>> DirectFESTO

InverseFESTO

JOSE ABRAHAM CENTENO BASURTO

11
CINEMATICA DE ROBOTS ROBOT FERSTO

MATLAB

JOSE ABRAHAM CENTENO BASURTO

12