Calculo de modelo dinamico manipulador 3gdl

Tabla de parametro DH
% Eslabon Theta(i) di ai alpha(i)
% 1 q1 l1 0 0
% 2 0 q2 0 90
% 3 0 q3 0 0

syms q1 q2 q3 dq1 dq2 dq3 c1 s1 c2 c2 l1

syms d2q1 d2q2 d2q3 gc m1 m2 m3

Mtrices homogeneas

A01 = [c1 -s1 0 0;...

s1 c1 0 0;...
0 0 1 l1;...
0 0 0 1]

A01 =

1
 A12 = [1 0 0 0;...
0 0 -1 0;...
0 1 0 q2;...
0 0 0 1]

A12 =

A23 = [1 0 0 0;...
0 1 0 0;...
0 0 1 q3;...
0 0 0 1]

A23 =

Matrices de rotacion directa

R01 = A01(1:3,1:3)

R01 =

R12 = A12(1:3,1:3)

R12 =

R23 = A23(1:3,1:3)

R23 =

2
Matriz de rotacion inversa

R10 =transpose(R01)

R10 =

R21 =transpose(R12)

R21 =

R32 =transpose(R23)

R32 =

condiciones iniciales

v00=[0;0;0]

v00 = 3×1
0
0
0

dv00=[0;0;0]

dv00 = 3×1
0
0
0

w00=[0;0;0]

w00 = 3×1
0
0
0

dw00=[0;0;0]

dw00 = 3×1

3
 0
0
0

z00=[0;0;1]

z00 = 3×1
0
0
1

z11=[0;0;1]

z11 = 3×1
0
0
1

z22=[0;0;1]

z22 = 3×1
0
0
1

a) propagacion de la velocidad angular

para el eslabon 1 articulacion rotacional i =1

w11=R10*(w00+z00*dq1)

w11 =

para el eslabon 2 articulacion prismatica i =2

w22=R21*w11

w22 =

4
para el eslabon 3 articulacion prismatica i =3

w33=R32*w22

w33 =

b)propagacion de aceleracion angular

para el eslabon 1 articulacion rotacional i =1

dw11=R10*(dw00+z00*d2q1 + cross(w00,(z00*dq1)))

dw11 =

para el eslabon 2 articulacion prismatica i =2

dw22=R21*dw11

dw22 =

para el eslabon 3 articulacion prismatica i =3

dw33=R32*dw22

dw33 =

c) propagacion de velocidad lineal

5
 r11=[0;0;l1]

r11 =

r22=[0;q2;0]

r22 =

r33=[0;0;q3]

r33 =

para el eslabon 1 articulacion rotacional i =1

v11=R01*v00 + cross(w11,(r11))

v11 =

para el eslabon 2 articulacion prismatica i =2

v22= R21*(v11+z11*dq2)+cross(w22,(r22))

6
 v22 =

para el eslabon 3 articulacion prismatica i =3

v33= R32*(v22+z22*dq2)+cross(w33,(r33))

v33 =