Laboratorio de Matl

ab

Dinmica de Robot de 4 Grados de Libertad

syms C1 C2 C3 C4 S1 S2 S3 d1 d2 d3 d4 M1 M2 M3 M4 R1 R2 R3 R4
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
syms q1 q2 q3 q4
% se activara cuando
C1=cos(q1);C2=cos(q2);C3=cos(q3);S1=sin(q1);S2= sin(q2);S3=
sin(q3);S4=sin(q4);C4=cos(q4);d2=q2 % hallemos el jacoviano
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
T1=[C1 -C1 0 0 ;S1 C1 0 0;0 0 1 d1;0 0 0 1]
T2=[1 0 0 0;0 0 -1 0;0 1 0 d2;0 0 0 1]
T3=[C3 0 S3 0;S3 0 -C3 0;0 1 0 d3;0 0 0 1]
T4=[C4 -C4 0 0; S4 C4 0 0;0 0 1 0;0 0 0 1]
%0T4 Matriz total de transformacin
T=T1*T2*T3*T4;
%SOLUCION "A" DETERMINAR LA CINEMATICA DIRECTA DE POSICION DEL PUNTO P
%_ _ _ _ _ _ _ _ _ _ _ POSICION_ _ _ _ _ _ _ _ _ _ _
%Vector P:
Px=T(1,4);
Py=T(2,4);
Pz=T(3,4);
P=[Px ; Py ; Pz];

% SOLUCION "B" DETERMINAR LA ENERGIA CINETICA DEL SISTEMA

%------------------Hallando Delta de C-----------------------
% Delta de C1
Dc1=[0;0;-d1/4];
% Delta de C2
Dc2=[0;d1+d2;0];
% Delta de C3
Dc3=[0;0;(d3)/4];
% Delta de C4
Dc4=[0;0;(-d4)/4];

%------------------Hallando Tensores de Inercia /FALTA ANALISIS----------

------------
%Tensor de Inercia 1 (I1)
I1= [M1*((3*R1.^2+ d1.^2)/12) 0
0
0 (M1*R1.^2)/2
0
0 0
M1*((3*R1.^2+ d1.^2)/12)];
%Tensor de Inercia 2 (I2)
I2= [M2*((3*R2.^2+ d2.^2)/12) 0
0
0 (M2*R2.^2)/2
0
0 0
M2*((3*R2.^2+ d2.^2)/12)];
%Tensor de Inercia 3 (I3)
I3= [ M3*((6*R3.^2+ d3.^2)/12) 0
0;
 0 M3*((6*R3.^2+ d3.^2)/12)
0;
0 0
M3*R3.^2 ];
%Tensor de Inercia 4 (I4)
I4= [ M4*d4.^2/12 0
0;
0 M4*d4.^2/12
0;
0 0
0];
%Calculando la matriz de rotacin de 0R1
R01= T1(1:3,1:3);
%Calculando la matriz de rotacin de 0R2
R02= T2(1:3,1:3);
%Calculando la matriz de rotacin de 0R3
R03= T3 (1:3,1:3);
%Calculando la matriz de rotacin de 0R4
R04= T4(1:3,1:3);

%Tensores de inercia respecto del referencial base trasladados a sus

centrosde masa
%------------------------------------------------------------------------
----------------------------------
%--------------------Tensor Dr1(q)
R10=R01.';
D1rq=R01*I1*R10;
%--------------------Tensor Dr2(q)
R20=R02.';
D2rq=R02*I2*R20;
%--------------------Tensor Dr3(q)
R30=R03.';
D3rq=R03*I3*R30;
%--------------------Tensor Dr4(q)
R40=R04.';
D4rq=R04*I4*R40;

%Los vectores desde el origen del referencial base hasta el centro de

masa
%------------------------------------------------------------------------
---------------------
%--------------Calculando la Posicin 1 (P1) ------------------------
Px=T1(1,4);
Py=T1(2,4);
Pz=T1(3,4);
P1=[Px ; Py ; Pz];

%--------------Calculando la Posicin 2 (P2) ------------------------

T02=T1*T2; %Realizamos la multiplicacin de las
dos primeras matrices ->0T2
% Entonces la posicin 2 es:
Px2=T02(1,4);
Py2=T02(2,4);
Pz2=T02(3,4);
P2=[Px2 ; Py2 ; Pz2];
%--------------Calculando la Posicin 3 (P3) ------------------------
T03=T1*T2*T3; %Realizamos la multiplicacin de las tres
matrices ->0T3
% Entonces la posicin 3 es:
Px3=T03(1,4);
Py3=T03(2,4);
Pz3=T03(3,4);
P3=[Px3; Py3; Pz3];

%--------------Calculando la Posicin 4 (P4) ------------------------

T04=T1*T2*T3*T4; %Realizamos la multiplicacin de las tres
matrices ->0T4
% Entonces la posicin 4 es:
Px4=T04(1,4);
Py4=T04(2,4);
Pz4=T04(3,4);
P4=[Px4 ; Py4 ; Pz4];

%Hallando c1
c1=P1+R01*Dc1;
%Hallando c2
c2=P2+R02*Dc2;
%Hallando c3
c3=P3+R03*Dc3;
%Hallando c4
c4=P4+R04*Dc4;

%Vectores velocidad lineal cartesiana del centro de masa/Falta Arreglarlo

.
%----------------------------------------------------------------------
syms q1 q2 q3 q4
q=[q1 q2 q3 q4];
%------------------Velocidad Jv1(q)-----------------------------------
Jv1q=jacobian(c1,q)
%------------------Velocidad Jv2(q)-----------------------------------
Jv2q=jacobian(c2,q)
%------------------Velocidad Jv3(q)-----------------------------------
Jv3q=jacobian(c3,q)
%------------------Velocidad Jv4(q)-----------------------------------
Jv4q=jacobian(c4,q)

%Velocidades angulares relativas al centro de masa con respecto al

%referencial base con el vector velocidad articular REVISAR
Jw1q=[0 0 0 0
0 0 0 0
1 0 0 0];

Jw2q=[0 C1 0 0
0 S1 0 0
1 0 0 0];

Jw3q=[0 C1 S1*C2 0
0 S1 -C1*C2 0
1 0 0 0];

Jw4q=[0 C1 S1*C2 0
 0 S1 -C1*C2 0
1 0 0 0];

% Tensores de inercia reflejados a los ejes de giro de cada articulacin

% respecto al referencial base:

%------------------------D1(q)---------------------------------------
D1=Jv1q.'*M1*Jv1q+Jw1q.'*D1rq*Jw1q;
%------------------------D2(q)---------------------------------------
D2=Jv2q.'*M2*Jv2q+Jw2q.'*D2rq*Jw2q;
%------------------------D3(q)---------------------------------------
D3=Jv3q.'*M3*Jv3q+Jw3q.'*D3rq*Jw3q;
%------------------------D4(q)---------------------------------------
D4=Jv4q.'*M4*Jv4q+Jw4q.'*D4rq*Jw4q;

%-----------------------ENERGIA CINETICA-----------------------------
%------------------------------------------------------------------------
------
%--------------------Matriz D(q)-----------------------------------------
--
Dq=D1+D2+D3+D4;
Dq=Dq.*eye(4);

%---------Elementos de la diagonal del tensor de inercia total ----------

-----
d11q=Dq(1,1);
d22q=Dq(2,2);
d33q=Dq(3,3);
d44q=Dq(4,4);
%----------------Energa cintica E(q,diff(q))-----------------------
syms dq1 dq2 dq3 dq4 g
Ec=(d11q*dq1.^2+d22q*dq2.^2+d33q*dq3.^2+d44q*dq4.^2)/2;

%-----------------------ENERGIA POTENCIAL----------------------------
%-----------------------vector gravedad------------------------------
C1z=c1(3,1);
C2z=c2(3,1);
C3z=c3(3,1);
C4z=c4(3,1);
%---------------Energia potencial Ep(q)------------------------------
Ep=g*(M1*C1z+M2*C2z+M3*C3z+M4*C4z);

%--------------LAGRANGEANO L(q,diff(q))------------------------------
Lq=Ec-Ep;
%diary('Lagrangeano.txt')
Ec;
Lq;
diary off

%-----------------fuerza de la friccin------------------------------
syms bs1 bv1 bs2 bv2 bs3 bv3 bs4 bv4
b1=sign(dq1)*bs1+bv1*dq1;
b2=sign(dq2)*bs2+bv1*dq2;
b3=sign(dq3)*bs3+bv1*dq3;
b4=sign(dq4)*bs4+bv1*dq4;

%---------------derivada parcial d11q---------------------------------

dp11=diff(d11q,q1);
dp12=diff(d11q,q2);
dp13=diff(d11q,q3);
dp14=diff(d11q,q4);
%---------------derivada parcial d22q---------------------------------
dp21=diff(d22q,q1);
dp22=diff(d22q,q2);
dp23=diff(d22q,q3);
dp24=diff(d22q,q4);
%---------------derivada parcial d33q---------------------------------
dp31=diff(d33q,q1);
dp32=diff(d33q,q2);
dp33=diff(d33q,q3);
dp34=diff(d33q,q4);
%---------------derivada parcial d44q---------------------------------
dp41=diff(d44q,q1);
dp42=diff(d44q,q2);
dp43=diff(d44q,q3);
dp44=diff(d44q,q4);
%-------------Derivada parcial de la energia potencial---------------
dEp1=diff(Ep,q1);
dEp2=diff(Ep,q2);
dEp3=diff(Ep,q3);
dEp4=diff(Ep,q4);
%------------------------------------------------------------------------
---------------------------------------
%------------------------------------------------------------------------
---------------------------------------
%-----------------------para i=1-----------------------------------------
-------------------------------------
%Derivada con respecto al tiempo de la derivada parcial de L
syms d2q1 d2q2 d2q3 d2q4 d2q
DDL1=(dp11*dq1+dp12*dq2+dp13*dq3+dp14*dq4)*dq1+d11q*d2q1;
%Derivada parcial respecto a q1 de L
DL1=(dp11*dq1.^2+dp21*dq2.^2+dp31*dq3.^2+dp41*dq4.^2)/2-dEp1;
%------------------------torsion1----------------------------------------
-------------------------------------
Tq1=DDL1-DL1+b1;

%------------------------------------------------------------------------
---------------------------------------
%------------------------------------------------------------------------
---------------------------------------

%-----------------------para i=2-------------------------------------
%--Derivada con respecto al tiempo de la derivada parcial de L-------
DDL2=(dp21*dq1+dp22*dq2+dp23*dq3+dp24*dq4)*dq2+d22q*d2q2;
%------------Derivada parcial respecto a q2 de L---------------------
DL2=(dp12*dq1.^2+dp22*dq2.^2+dp32*dq3.^2+dp42*dq4.^2)/2-dEp2;
%-----------------------------torsin 2-------------------------------
Tq2=DDL2-DL2+b2;
%------------------------------------------------------------------------
---------------------------------------
%------------------------------------------------------------------------
---------------------------------------

%-----------------------para i=3-------------------------------------
%--Derivada con respecto al tiempo de la derivada parcial de L-------
DDL3=(dp31*dq1+dp32*dq2+dp33*dq3+dp34*dq4)*dq3+d33q*d2q3;
%------------Derivada parcial respecto a q3 de L---------------------
DL3=(dp13*dq1.^2+dp23*dq2.^2+dp33*dq3.^2+dp43*dq4.^2)/2-dEp3;
%-----------------------------torsion 3-------------------------------
Tq3=DDL3-DL3+b3;

%------------------------------------------------------------------------
---------------------------------------
%------------------------------------------------------------------------
---------------------------------------

%-----------------------para i=4-------------------------------------
%--Derivada con respecto al tiempo de la derivada parcial de L-------
DDL4=(dp41*dq1+dp42*dq2+dp43*dq3+dp44*dq4)*dq4+d44q*d2q4;
%------------Derivada parcial respecto a q3 de L---------------------
DL4=(dp14*dq1.^2+dp24*dq2.^2+dp34*dq3.^2+dp44*dq4.^2)/2-dEp4;
%-----------------------------torsin 1------------------------------
Tq4=DDL4-DL4+b4;

%------------------------------------------------------------------------
---------------------------------------
%------------------------------------------------------------------------
---------------------------------------

%------------------------Dinmica del ROBOT----------------------------

Cq=[DDL1;DDL2;DDL3;DDL4];
hq=[DL1;DL2;DL3;DL4];
bq=[b1;b2;b3;b4];
d2q=[d2q1;d2q2;d2q3;d2q4];
Tcq=Dq*d2q+Cq+hq+bq;