Euler-Lagrange

TABLA DE PARAMETROS 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
c1=cos(q1)

c1 =

s1=sin(q1)

s1 =

c2=cos(q2)

c2 =

s2=sin(q2)

s2 =

c3=cos(q3)

c3 =

s3=sin(q3)
 s3 =

MATRICES HOMOGENEAS

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

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

A01 =

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 =
 A02=simplify(A01*A12)

A02 =

A03=simplify(A02*A23)

A03 =

MATRICES DE ROTACION DIRECTA

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

R01 =

R02 = A02(1:3,1:3)

R02 =

R03 = A03(1:3,1:3)

R03 =

MATRIZ DE ROTACION INVERSA

R10 = transpose(R01)

R10 =

R20 = transpose(R02)

R20 =
 R30 = transpose(R03)

R30 =

matriz de inercia

para el eslabón 1 articulación rotacional i =1

i11=[(m1*l1*l1)/12 0 0;...
0 (m1*l1*l1)/12 0;...
0 0 0]

i11 =

I1=simplify (R01*i11*R10)

I1 =
para el eslabón 2 articulación prismática i =2

i22=[(m2*q2*q2)/12 0 0;...
0 (m2*q2*q2)/12 0;...
0 0 0]

i22 =

I2=simplify(R02*i22*R20)

I2 =

para el eslabón 3 articulación prismática i =3

i33=[0 0 0;...
 0 (m3*q3*q3)/12 0;...
0 0 (m3*q3*q3)/12]

i33 =

I3=simplify(R03*i33*R30)

I3 =

Vector de posición

pc01=[ 0; 0 ;l1/2]

pc01 =
pc02=[ 0; 0 ;q2/2+l1]

pc02 =

pc03=[ q3/2*c1; -q3/2*s1 ;q2+l1]

pc03 =
pc12=[ 0; 0 ;q2/2]

pc12 =

pc13=[ q3/2*c1; -q3/2*s1 ;q2]

pc13 =
pc23=[ q3/2; 0 ; 0]

pc23 =

pc11=[ 0; 0 ; -l1/2]

pc11 =
 pc22=[ -q2; 0 ; 0]

pc22 =

pc33=[ -q3/2; 0 ; 0]

pc33 =

z0=[0; 0; 1]

z0 = 3×1
0
0
1

z1=[0; 0; 1]

z1 = 3×1
0
0
1

z2=[0; 0; 1]

z2 = 3×1
0
0
1

Para el eslabón 1, i=1, con la articulación rotacional 1, j=1, j<=i

jv11=cross(z0,pc01)

jv11 =
Para el eslabón 1, i=1, con la articulación prismática 2, j=2, j>=i

jv21=[0; 0; 0]

jv21 = 3×1
0
0
0

Para el eslabón 1, i=1, con la articulación prismática 3, j=3, j>=i

jv31=[0; 0; 0]

jv31 = 3×1
0
0
0

Entonces

jv1=[jv11,jv21,jv31]

jv1 =

Para el eslabón 2, i=2, con la articulación rotacional 1, j=1, j<=i

jv12=cross(z0,pc02)

jv12 =

Para el eslabón 2, i=2, con la articulación prismática 2, j=2, j<=i

jv22=cross(z1,pc12)

jv22 =
Para el eslabón 2, i=2, con la articulación prismática 3, j=3, j>=i

jv32=[0; 0; 0]

jv32 = 3×1
0
0
0

Entonces

jv2=[jv12,jv22,jv32]

jv2 =

Para el eslabón 3, i=3, con la articulación rotacional 1, j=1, j<=i

jv13=cross(z0,pc03)

jv13 =

Para el eslabón 3, i=3, con la articulación prismática 2, j=2, j<=i

jv23=z1

jv23 = 3×1
0
0
1

Para el eslabón 3, i=3, con la articulación prismática 3, j=3, j<=i

jv33=z2

jv33 = 3×1
0
0
1

Entonces
 jv3=[jv13,jv23,jv33]

jv3 =

Para el eslabón 1, i=1, con la articulación rotacional 1, j=1, j<=i

jw11=z0

jw11 = 3×1
0
0
1

Para el eslabón 1, i=1, con la articulación prismática 2, j=2, j>=i

jw21=[0; 0; 0]

jw21 = 3×1
0
0
0

Para el eslabón 1, i=1, con la articulación prismática 3, j=3, j>=i

jw31=[0; 0; 0]
 jw31 = 3×1
0
0
0

Entonces

jw1=[jw11,jw21,jw31]

jw1 = 3×3
0 0 0
0 0 0
1 0 0

Para el eslabón 2, i=2, con la articulación rotacional 1, j=1, j<=i

jw12=z0

jw12 = 3×1
0
0
1

Para el eslabón 2, i=2, con la articulación prismática 2, j=2, j<=i

jw22=[0; 0; 0]

jw22 = 3×1
0
0
0

Para el eslabón 2, i=2, con la articulación prismática 3, j=3, j>=i

jw32=[0; 0; 0]

jw32 = 3×1
0
0
0

Entonces

jw2=[jw12,jw22,jw32]

jw2 = 3×3
0 0 0
0 0 0
 1 0 0

Para el eslabón 3, i=3, con la articulación rotacional 1, j=1, j<=i

jw13=z0

jw13 = 3×1
0
0
1

Para el eslabón 3, i=3, con la articulación prismática 2, j=2, j<=i

jw23=[0; 0; 0]

jw23 = 3×1
0
0
0

Para el eslabón 3, i=3, con la articulación prismática 3, j=3, j<=i

jw33=[0; 0; 0]

jw33 = 3×1
0
0
0

Entonces

jw3=[jw13,jw23,jw33]

jw3 = 3×3
0 0 0
0 0 0
1 0 0
M=simplify( transpose(jv1)*m1*jv1 + transpose(jw1)*I1*jw1 + ...
transpose(jv2)*m2*jv2 + transpose(jw2)*I2*jw2 +...
transpose(jv3)*m3*jv3 + transpose(jw3)*I3*jw3 )

M =

m1=M(1,1)
 m1 =

dm11=2*(m2*q2/12)+2*(m3*q3/3)

dm11 =

v1=dm11*dq2*dq3

v1 =

v2=0

v2 = 0

v3=0

v3 = 0

V = [v1;v2;v3]

V =

e) Vector Gravitacional
 g=[0;0;gc]

g =

G1 = -m1*transpose(g)*jv11 +...
-m2*transpose(g)*jv12 +...
-m3*transpose(g)*jv13

G1 =
0

G2 = -m1*transpose(g)*jv21 +...
-m2*transpose(g)*jv22 +...
-m3*transpose(g)*jv23

G2 =

G3 = -m1*transpose(g)*jv31 +...
-m2*transpose(g)*jv32 +...
-m3*transpose(g)*jv33

G3 =

entonces
G=[G1;G2;G3]

G =

d2q = [d2q1;d2q2;d2q3]

d2q =

tau=M*d2q + V + G

tau =

tau1 = tau(1);
tau1 = simplify(tau(1))

tau1 =

tau2 = tau(2);
tau2 = simplify(tau(2))

tau2 =

tau3 = tau(3);
tau3 = simplify(tau(3))

tau3 =