LABORATORIO 3

FACULTAD DE INGENIERA DE SISTEMAS Y ELECTRNICA

ESPECIALIDAD:

INGENIERA MECATRNICA

CURSO:

ROBTICA I

TEMA:

DINAMICA DE UN ROBOT DE 4 GL

PROFESOR:

MACHUCA MINES, JOS

INTEGRANTES:

RAMIREZ MURACAMI, ROBERTO CARLOS 1310457

COLQUI MAXIMILIANO, KATHYANA VICTORIA 1220679

MOSILOT BAZAN, CESAR 1321839

2016
PROCEDIMIENTO EN MATLAB

clc
clear all
close all

syms D1 D2 D3 D4 q1 q2 q3 q4 dq1 dq2 dq3 dq4 R1 R2 R3 R4 M1 M2 M3 M4 g

% Vector articulado de posicin

% q1=29*pi/36*sin(pi/5*t);
% q2=pi/2+7*pi/18*sin(2*pi/5*t);
% q3=pi/2+pi/4*sin(3*pi/5*t);
% q4=0.5-0.3*cos(pi/10*t);

T1 = [cos(q1) 0 sin(q1) 0 ; sin(q1) 0 -cos(q1) 0;0 1 0 (D1); 0 0 0 1];

T2 = [cos(q2) -sin(q2) 0 0; sin(q2) cos(q2) 0 0 ;0 0 1 0 ; 0 0 0 1];
T3 = [cos(q3) 0 sin(q3) 0 ; sin(q3) 0 -cos(q3) 0;0 1 0 0 ; 0 0 0 1];
T4 = [1 0 0 0 ; 0 1 0 0 ;0 0 1 (D4); 0 0 0 1];

%0T3
T = T1*T2*T3*T4;

% Determinar la cinemtica directa de posicin de posicin del punto P

%%%%%%%%%%%%%%%%%%%%%%%%%%% POSICIN %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% Vector P:
Px = T(1,4);
Py = T(2,4);
Pz = T(3,4);
P = [Px ; Py ; Pz]
% Determinar la energa cintica del sistema

%%%%%%%%%%%%%%%%%%%%%%%%%% POSICION P (1) %%%%%%%%%%%%%%%%%%%%%%%%%%%

Px = T1 (1,4);
Py = T1 (2,4);
Pz = T1 (3,4);
P1 = [Px; Py; Pz]

% Calculando la matriz de rotacin de 0R1

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

% Delta de C1
Dc1 = [0; D1/2; 0]

% Hallando C1
C1 = P1+R01*Dc1

%%%%%%%%%%%%%%%%%%%%%%%%%% POSICION P(2) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Realizamos la multiplicacin de las dos primeras matrices: 0T2

T02 = T1*T2

% Entonces la posicin 2 es:

Px2 = T02(1,4);
Py2 = T02(2,4);
Pz2 = T02(3,4);
P2 = [Px2 ; Py2 ; Pz2]

% Calculando la matriz de rotacin de 0R2

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

% Delta de C2
Dc2 = [0;-D2/2;0]

% Hallando C2
C2 = P2+R02*Dc2

%%%%%%%%%%%%%%%%%%%%%%%%%% POSICION P(3) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Realizamos la multiplicacin de las tres matrices: 0T3

T03 = T1*T2*T3;

% Entonces la posicion 3 es:

Px3 = T03(1,4);
Py3 = T03(2,4);
Pz3 = T03(3,4);
P3 = [Px3 ; Py3 ; Pz3]

% Calculando la matriz de rotacion de 0R3

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

% Delta de C3
Dc3 = [0;0;-D3/2]

% Hallando C3
C3 = P3+R03*Dc3
%%%%%%%%%%%%%%%%%%%%%%%%%% POSICION P(4) %%%%%%%%%%%%%%%%%%%%%%%%%%%

% Realizamos la multiplicacin de las tres matrices: 0T4

T04 = T1*T2*T3*T4;

% Entonces la posicion 4 es:

Px4 = T04(1,4);
Py4 = T04(2,4);
Pz4 = T04(3,4);
P4 = [Px4 ; Py4 ; Pz4]

% Calculando la matriz de rotacion de 0R3

R04 = T04(1:3,1:3)

% Delta de C4
Dc4 = [0;0;-D4/2]

% Hallando C4
C4 = P4+R04*Dc4
%%%%%%%%%%%%%%%%%%%%%%% TENSORES DE INERCIA %%%%%%%%%%%%%%%%%%%%%%%%

% 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*R3.^2 0 0; 0 M3*((6*R3.^2+ D3.^2)/12) 0; 0 0 M3*((6*R3.^2+
D3.^2)/12)]
% Tensor de Inercia 4: I4
I4 = [0 0 0; 0 M4*D4.^2/12 0; 0 0 M4*D4.^2/12]

% Tensores de inercia respecto del referencial base trasladados a sus

centros de masa

%%%%%%%%%%%%%%%%%%%%%%% TENSORES Dr1(q) (raya)%%%%%%%%%%%%%%%%%%%%%%

R10 = R01.';
D1rq = R01*I1*R10

D1rq =

[ M1*(D1^2/12 + R1^2/4)*cos(q1)^2 + M1*(D1^2/12 + R1^2/4)*sin(q1)^2, 0, 0 ]

[ 0, M1*(D1^2/12 + R1^2/4)*cos(q1)^2 + M1*(D1^2/12 + R1^2/4)*sin(q1)^2, 0]
[ 0, 0, (M1*R1^2)/2]

%%%%%%%%%%%%%%%%%%%%%%%%%% TENSORES Dr2(q) (raya)%%%%%%%%%%%%%%%%%%%%%%%%%

R20 = R02.';
D2rq = R02*I2*R20

D2rq =

[ M2*sin(q1)^2*(D2^2/12 + R2^2/4) + M2*cos(q1)^2*cos(q2)^2*(D2^2/12 + R2^2/4) +

(M2*R2^2*cos(q1)^2*sin(q2)^2)/2, M2*cos(q1)*cos(q2)^2*sin(q1)*(D2^2/12 + R2^2/4) -
M2*cos(q1)*sin(q1)*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q1)*sin(q1)*sin(q2)^2)/2,
M2*cos(q1)*cos(q2)*sin(q2)*(D2^2/12 + R2^2/4) - (M2*R2^2*cos(q1)*cos(q2)*sin(q2))/2]

[ M2*cos(q1)*cos(q2)^2*sin(q1)*(D2^2/12 + R2^2/4) - M2*cos(q1)*sin(q1)*(D2^2/12 +

R2^2/4) + (M2*R2^2*cos(q1)*sin(q1)*sin(q2)^2)/2, M2*cos(q1)^2*(D2^2/12 + R2^2/4) +
M2*cos(q2)^2*sin(q1)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*sin(q1)^2*sin(q2)^2)/2,
M2*cos(q2)*sin(q1)*sin(q2)*(D2^2/12 + R2^2/4) - (M2*R2^2*cos(q2)*sin(q1)*sin(q2))/2]

[ M2*cos(q1)*cos(q2)*sin(q2)*(D2^2/12 + R2^2/4) - (M2*R2^2*cos(q1)*cos(q2)*sin(q2))/2,

M2*cos(q2)*sin(q1)*sin(q2)*(D2^2/12 + R2^2/4) - (M2*R2^2*cos(q2)*sin(q1)*sin(q2))/2,
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2]
%%%%%%%%%%%%%%%%%%%%%%%%%% TENSORES Dr3(q) (raya)%%%%%%%%%%%%%%%%%%%%%%%%%
R30 = R03.';
D3rq = R03*I3*R30

D3rq =

[M3*sin(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q1)*cos(q2)*sin(q3) +

cos(q1)*cos(q3)*sin(q2))^2*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2, M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2) - M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) - M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)),
M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))]

[ M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) - M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)), M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2 +
M3*cos(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2*(D3^2/12 + R3^2/2), - M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) -
M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))]

[M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)), -
M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) - M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)),
M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + M3*R3^2*(cos(q2)*sin(q3)
+ cos(q3)*sin(q2))^2]

%%%%%%%%%%%%%%%%%%%%%%%%%% TENSORES Dr4(q) (raya)%%%%%%%%%%%%%%%%%%%%%%%%%

R40 = R04.';
D4rq = R04*I4*R40

D4rq =

[ (D4^2*M4*sin(q1)^2)/12 + (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2)/12, (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/12 -
(D4^2*M4*cos(q1)*sin(q1))/12, -(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12]
[ (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/12 - (D4^2*M4*cos(q1)*sin(q1))/12,
(D4^2*M4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/12 +
(D4^2*M4*cos(q1)^2)/12, -(D4^2*M4*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12]

[ -(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12, -(D4^2*M4*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12,
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12]

% Vectores velocidad lineal cartesiana del centro de masa.

%%%%%%%%%%%%%%%%%%%%%%%%%%%% VELOCIDAD Jv1(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%

Jv1q = [0 0 0 0;
0 0 0 0;
0 0 0 0]

%%%%%%%%%%%%%%%%%%%%%%%%%%%% VELOCIDAD Jv2(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%

dC2q1 = diff(C2,q1)
dC2q2 = diff(C2,q2)
dC2q3 = diff(C2,q3)
dC2q4 = diff(C2,q4)

Jv2q = [dC2q1 dC2q2 dC2q3 dC2q4]

%%%%%%%%%%%%%%%%%%%%%%%%%%%% VELOCIDAD Jv3(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%

dC3q1 = diff(C3,q1)
dC3q2 = diff(C3,q2)
dC3q3 = diff(C3,q3)
dC3q4 = diff(C3,q4)

Jv3q = [dC3q1 dC3q2 dC3q3 dC3q4]

Jv3q =

[ (D3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/2, -(D3*(cos(q1)*cos(q2)*cos(q3)

- cos(q1)*sin(q2)*sin(q3)))/2, -(D3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))/2, 0]

[ -(D3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))/2, (D3*(sin(q1)*sin(q2)*sin(q3)

- cos(q2)*cos(q3)*sin(q1)))/2, (D3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/2, 0]
[ 0, -(D3*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/2, -(D3*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/2,
0]

%%%%%%%%%%%%%%%%%%%%%%%%%%%% VELOCIDAD Jv4(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%

dC4q1 = diff(C4,q1)
dC4q2 = diff(C4,q2)
dC4q3 = diff(C4,q3)
dC4q4 = diff(C4,q4)

Jv4q = [dC4q1 dC4q2 dC4q3 dC4q4]

Jv4q =

[ -(D4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/2, (D4*(cos(q1)*cos(q2)*cos(q3)

- cos(q1)*sin(q2)*sin(q3)))/2, (D4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3)))/2, 0]

[ (D4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2)))/2, -(D4*(sin(q1)*sin(q2)*sin(q3)

- cos(q2)*cos(q3)*sin(q1)))/2, -(D4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/2, 0]

[ 0, (D4*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/2, (D4*(cos(q2)*sin(q3) + cos(q3)*sin(q2)))/2, 0]

% Velocidades angulares relativas al centro de masa

% con referencial a la base con el vector velocidad articular:

Jw1q = [0 0 0 0;
0 0 0 0;
1 1 0 0];

Jw2q = [0 0 0 0;
0 0 0 0;
1 1 0 0];

Jw3q = [0 0 sin(q3) 0;
0 0 -cos(q3) 0;
1 1 0 0];

Jw4q = [0 0 sin(q3) 0;
0 0 -cos(q3) 0;
1 1 0 0];

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

% respecto al referencial base:

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% D1(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Dq1 = Jv1q.'*M1*Jv1q+Jw1q.'*D1rq*Jw1q
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% D2(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Dq2 = Jv2q.'*M2*Jv2q+Jw2q.'*D2rq*Jw2q

Dq2 =

[ M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 +

(D2^2*M2*cos(q1)^2*sin(q2)^2)/4 + (D2^2*M2*sin(q1)^2*sin(q2)^2)/4,
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2, 0, 0]

[M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2, M2*sin(q2)^2*(D2^2/12 +

R2^2/4) + (M2*R2^2*cos(q2)^2)/2 + (D2^2*M2*sin(q2)^2)/4 +
(D2^2*M2*cos(q1)^2*cos(q2)^2)/4 + (D2^2*M2*cos(q2)^2*sin(q1)^2)/4, 0, 0]

[0,0, 0, 0]

[0, 0, 0, 0]

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% D3(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Dq3 = Jv3q.'*M3*Jv3q+Jw3q.'*D3rq*Jw3q

Dq3 =

[ (D3^2*M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 + M3*(D3^2/12 +

R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + M3*R3^2*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2 + (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/4,
M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + M3*R3^2*(cos(q2)*sin(q3)
+ cos(q3)*sin(q2))^2 - (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4, sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) -
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4, 0]

[M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + M3*R3^2*(cos(q2)*sin(q3)

+ cos(q3)*sin(q2))^2 - (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4, (D3^2*M3*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) -
sin(q2)*sin(q3))^2 + (D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
M3*R3^2*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4, sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4, 0]

[ sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) + cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))) - (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4, sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4, sin(q3)*(cos(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
sin(q3)*(M3*sin(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)) + cos(q3)*(sin(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
cos(q3)*(M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2 +
M3*cos(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2*(D3^2/12 + R3^2/2))) + (D3^2*M3*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4, 0]

[0, 0, 0, 0]
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% D4(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Dq4 = Jv4q.'*M4*Jv4q+Jw4q.'*D4rq*Jw4q

Dq4 =

[ (D4^2*M4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + (D4^2*M4*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))^2)/4, (D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4,
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12, 0]

[ (D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3)

+ cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4, (D4^2*M4*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3)
- cos(q1)*sin(q2)*sin(q3))^2)/4, (D4^2*M4*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4 +
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12, 0]

[ (D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12,
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12 -
(D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12, cos(q3)*(cos(q3)*((D4^2*M4*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2)/12 + (D4^2*M4*cos(q1)^2)/12) +
sin(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/12)) +
sin(q3)*(sin(q3)*((D4^2*M4*sin(q1)^2)/12 + (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2)/12) + cos(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/12)) + (D4^2*M4*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4, 0]

[0, 0, 0, 0]

%%%%%%%%%%%%%%%%%%%%%%%%%%%% ENERGA CINTICA %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Matriz D(q) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

Dq = Dq1+Dq2+Dq3+Dq4

Dq =

[(M1*R1^2)/2 + (D3^2*M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 +

(D4^2*M4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 + M3*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D4^2*M4*(cos(q2)*cos(q3) -
sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2 +
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 +
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/4 +
(D2^2*M2*cos(q1)^2*sin(q2)^2)/4 + (D2^2*M2*sin(q1)^2*sin(q2)^2)/4,
(M1*R1^2)/2 + M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 +
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2 + M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 -
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4,
sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) + cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))) - (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4 + (D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12 -
(D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12, 0]

[ (M1*R1^2)/2 + M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 +

(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2 + M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 -
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4,
(M1*R1^2)/2 + (D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 + M3*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D3^2*M3*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2 + M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 +
(D2^2*M2*sin(q2)^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D2^2*M2*cos(q1)^2*cos(q2)^2)/4 +
(D2^2*M2*cos(q2)^2*sin(q1)^2)/4, sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4
+ (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4 +
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12, 0]

[ sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) + cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) +
M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))) - (D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/4 + (D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12 -
(D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12, sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4
+ (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4 +
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12,
cos(q3)*(cos(q3)*((D4^2*M4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/12 +
(D4^2*M4*cos(q1)^2)/12) + sin(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/12)) + sin(q3)*(sin(q3)*((D4^2*M4*sin(q1)^2)/12 +
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/12) +
cos(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/12)) +
sin(q3)*(cos(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) - M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
sin(q3)*(M3*sin(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)) + cos(q3)*(sin(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
cos(q3)*(M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2 +
M3*cos(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2*(D3^2/12 + R3^2/2))) + (D3^2*M3*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D4^2*M4*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4, 0]

[ 0, 0, 0, 0]

%%%%%%%%%%%%%%%%%%%%%%%%%% Vector Diferencia (q) %%%%%%%%%%%%%%%%%%%%%%%%%%

difq = [dq1; dq2; dq3; dq4];

%%%%%%%%%%%%%%%%%%%%%% Energa Cintica E(q,diff(q) %%%%%%%%%%%%%%%%%%%%%%%

Ec = (difq.'*Dq*difq)/2;

%%%%%%%%%%%%%%%%%%%%%%%%%%% ENERGA POTENCIAL %%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Vector gravedad %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
G = [0;0;-g];

%%%%%%%%%%%%%%%%%%%%%%%% Energa Potencial Ep(q) %%%%%%%%%%%%%%%%%%%%%%%%%%

Ep = -G.'*(M1*C1+M2*C2+M3*C3+M4*C4)

Ep =

g*(M3*(D1 + (D3*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/2) + M4*(D1 - (D4*(cos(q2)*cos(q3) -

sin(q2)*sin(q3)))/2) + M2*(D1 - (D2*cos(q2))/2) + (3*D1*M1)/2)

%%%%%%%%%%%%%%%%%%%%%%% Lagrangeano L(q,diff(q)) %%%%%%%%%%%%%%%%%%%%%%%%%%

Lq = Ec-Ep

Lq =

(dq2*(dq3*(sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4
+ (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4 +
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12) + dq2*((M1*R1^2)/2 +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 + M3*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D3^2*M3*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2 + M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 +
(D2^2*M2*sin(q2)^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D2^2*M2*cos(q1)^2*cos(q2)^2)/4 +
(D2^2*M2*cos(q2)^2*sin(q1)^2)/4) + dq1*((M1*R1^2)/2 + M3*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D4^2*M4*(cos(q2)*cos(q3) -
sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2 +
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 -
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4)))/2 +
(dq3*(dq2*(sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) + cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3)
+ cos(q1)*cos(q3)*sin(q2))*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) +
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) +
(D3^2*M3*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D4^2*M4*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2)/4 +
(D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D4^2*M4*(cos(q2)*sin(q3) +
cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4
+ (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))^2)/4 +
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 - (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12) -
dq1*((D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) -
sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 +
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 + (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12) +
dq3*(cos(q3)*(cos(q3)*((D4^2*M4*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2)/12 + (D4^2*M4*cos(q1)^2)/12) +
sin(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 - (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/12)) +
sin(q3)*(sin(q3)*((D4^2*M4*sin(q1)^2)/12 + (D4^2*M4*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2)/12) + cos(q3)*((D4^2*M4*cos(q1)*sin(q1))/12 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2)))/12)) + sin(q3)*(cos(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
sin(q3)*(M3*sin(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))^2*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)) + cos(q3)*(sin(q3)*(M3*cos(q1)*sin(q1)*(D3^2/12 + R3^2/2) -
M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))*(D3^2/12 + R3^2/2) + M3*R3^2*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))) +
cos(q3)*(M3*R3^2*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1))^2 +
M3*cos(q1)^2*(D3^2/12 + R3^2/2) + M3*(cos(q2)*sin(q1)*sin(q3) +
cos(q3)*sin(q1)*sin(q2))^2*(D3^2/12 + R3^2/2))) + (D3^2*M3*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D4^2*M4*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))^2)/4 + (D3^2*M3*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2)/4 + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))^2)/4)))/2 + (dq1*(dq1*((M1*R1^2)/2 +
(D3^2*M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))^2)/4 + M3*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D4^2*M4*(cos(q2)*cos(q3) -
sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2 +
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 +
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/4 +
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))^2)/4 +
(D2^2*M2*cos(q1)^2*sin(q2)^2)/4 + (D2^2*M2*sin(q1)^2*sin(q2)^2)/4) -
dq3*((D3^2*M3*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) - cos(q2)*cos(q3)*sin(q1)))/4 -
cos(q3)*(M3*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3)) + M3*R3^2*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1))*(cos(q2)*sin(q3) + cos(q3)*sin(q2))) -
sin(q3)*(M3*R3^2*(cos(q1)*cos(q2)*cos(q3) - cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q3) +
cos(q3)*sin(q2)) - M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(D3^2/12 +
R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))) + (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 +
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 + (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*cos(q3)*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2))*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/12 + (D4^2*M4*sin(q3)*(cos(q1)*cos(q2)*sin(q3) +
cos(q1)*cos(q3)*sin(q2))*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/12) + dq2*((M1*R1^2)/2 +
M3*(D3^2/12 + R3^2/2)*(cos(q2)*cos(q3) - sin(q2)*sin(q3))^2 + (D4^2*M4*(cos(q2)*cos(q3)
- sin(q2)*sin(q3))^2)/12 + M3*R3^2*(cos(q2)*sin(q3) + cos(q3)*sin(q2))^2 +
M2*sin(q2)^2*(D2^2/12 + R2^2/4) + (M2*R2^2*cos(q2)^2)/2 -
(D3^2*M3*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D3^2*M3*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4 -
(D4^2*M4*(cos(q1)*cos(q2)*sin(q3) + cos(q1)*cos(q3)*sin(q2))*(sin(q1)*sin(q2)*sin(q3) -
cos(q2)*cos(q3)*sin(q1)))/4 - (D4^2*M4*(cos(q1)*cos(q2)*cos(q3) -
cos(q1)*sin(q2)*sin(q3))*(cos(q2)*sin(q1)*sin(q3) + cos(q3)*sin(q1)*sin(q2)))/4)))/2 -
g*(M3*(D1 + (D3*(cos(q2)*cos(q3) - sin(q2)*sin(q3)))/2) + M4*(D1 - (D4*(cos(q2)*cos(q3) -
sin(q2)*sin(q3)))/2) + M2*(D1 - (D2*cos(q2))/2) + (3*D1*M1)/2)