% Universidad Nacional de Trujillo - Ingeniería Mecatrónica

% Laboratorio 7
% Robótica
% Tema: Dinámico del Robot - Método Lagrange Euler.
% Autor: Juarez Mercedes, Luis Alexander
% Fecha: 06/25/2019
%-------------------------------------------------------------------------
clc, clear, close all

disp(' ');
disp('Ejercicio 2. Robot RP.');
disp(' ');

syms q1 q2 dq1 dq2 ddq1 ddq2 L m1 m2 g pi

syms Ixx1 Iyy1 Izz1 Ixy1 Ixz1 Iyz1
syms Ixx2 Iyy2 Izz2 Ixy2 Ixz2 Iyz2
g0 = [0;-g;0];
m = [m1;m2];
q = [q1;q2];
dq = [dq1;dq2];
ddq = [ddq1;ddq2];
%-------------------------------------------------------------------------
% Parámetros de Denavit Hartenberg
theta = [q1+pi/2;0];
d = [0;q2];
a = [0;0];
alpha = [pi/2;0];
MDH = [theta d a alpha] % Tabla de parámetros de DH.
%-------------------------------------------------------------------------
% Matriz de Transformación Homogénea entre sistemas de coordenadas
T01 = simplify(denavit(theta(1),d(1),a(1),alpha(1)));
T12 = simplify(denavit(theta(2),d(2),a(2),alpha(2)));
T02 = simplify(simplify(T01*T12));
% Posicion
P2 = T02(1:3,4);
% Matrices de Rotación
R00 = eye(3)
R01 = T01(1:3,1:3)
R02 = T02(1:3,1:3)
% MTH del centro de masa de la barra 1 respecto a base
Tcm01 = simplify(denavit(q1,0,L,0));
Pcm1 = Tcm01(1:3,4)
% MTH del centro de masa de la barra 2 respecto a base
Pcm2 = T02(1:3,4)
%-------------------------------------------------------------------------
% Jacobianos de Velocidad lineal
Jv1 = [diff(Pcm1(1),q1) 0; % Jacobiano eslabon 1
diff(Pcm1(2),q1) 0;
diff(Pcm1(3),q1) 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2); % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2);
diff(Pcm2(3),q1) diff(Pcm2(3),q2)]
% Columnas para matriz Jacobiana de velocidad angular
z00 = zeros(3,1); % Vector Cero.
z0 = R00(1:3,3); % Vector Columna de matriz R00
z1 = R01(1:3,3); % Vector Columna de matriz R01
% Jacobianos de Velocidad angular
Jw1 = [z0 z00] % Jacobiano eslabon 1
Jw2 = [z0 z00] % Jacobiano eslabon 2
% Tensores
I1 = [Ixx1 0 0;
0 Iyy1 0;
0 0 Izz1];
I2 = [Ixx2 0 0;
0 Iyy2 0;
0 0 Izz2];
% Matrices necesarias para calculo
R0 = [R01;R02];
Jv = [Jv1;Jv2];
Jw = [Jw1;Jw2];
I = [I1;I2];
%-------------------------------------------------------------------------
% Matriz Euler-Lagrange
M = simplify(matrizMasa(m,I,R0,Jv,Jw))
C = simplify(matrizCoriolis(q,dq,M))
G = simplify(matrizGravedad(m,g0,Jv))
% Ecuacion Euler-Lagrange
tau = M*ddq+C*dq+G
%-------------------------------------------------------------------------

Ejercicio 2. Robot RP.

MDH =
[ q1 + pi/2, 0, 0, pi/2]
[ 0, q2, 0, 0]
R00 =
1 0 0
0 1 0
0 0 1
R01 =
[ -sin(q1), 0, cos(q1)]
[ cos(q1), 0, sin(q1)]
[ 0, 1, 0]
R02 =
[ -sin(q1), 0, cos(q1)]
[ cos(q1), 0, sin(q1)]
[ 0, 1, 0]
Pcm1 =
L*cos(q1)
L*sin(q1)
0
Pcm2 =
q2*cos(q1)
q2*sin(q1)
0
Jv1 =
[ -L*sin(q1), 0]
[ L*cos(q1), 0]
[ 0, 0]
Jv2 =
[ -q2*sin(q1), cos(q1)]
[ q2*cos(q1), sin(q1)]
[ 0, 0]
Jw1 =
0 0
0 0
1 0
Jw2 =
0 0
0 0
1 0
M =
[ m1*L^2 + m2*q2^2 + Iyy1 + Iyy2, 0]
[ 0, m2]
C =
[ dq2*m2*q2, dq1*m2*q2]
[ -dq1*m2*q2, 0]
G =
g*cos(q1)*(L*m1 + m2*q2)
g*m2*sin(q1)
tau =
ddq1*(m1*L^2 + m2*q2^2 + Iyy1 + Iyy2) + g*cos(q1)*(L*m1 + m2*q2) + 2*dq1*dq2*m2*q2
- m2*q2*dq1^2 + ddq2*m2 + g*m2*sin(q1)

clc, clear, close all

disp(' ');
disp('Ejercicio 3. Manipulador RR.');
disp(' ');

syms q1 q2 dq1 dq2 ddq1 ddq2 L1 L2 Lc1 Lc2 m1 m2 g pi

syms Ixx1 Iyy1 Izz1 Ixy1 Ixz1 Iyz1
syms Ixx2 Iyy2 Izz2 Ixy2 Ixz2 Iyz2
g0 = [0;-g;0];
m = [m1;m2];
q = [q1;q2];
dq = [dq1;dq2];
ddq = [ddq1;ddq2];
%-------------------------------------------------------------------------
% Parámetros de Denavit Hartenberg
theta = [q1;q2];
d = [0;0];
a = [L1;L2];
alpha = [pi/2;0];
MDH = [theta d a alpha] % Tabla de parámetros de DH.
%-------------------------------------------------------------------------
% Matriz de Transformación Homogénea entre sistemas de coordenadas
T01 = simplify(denavit(theta(1),d(1),a(1),alpha(1)));
T12 = simplify(denavit(theta(2),d(2),a(2),alpha(2)));
T02 = simplify(simplify(T01*T12));
% Matrices de Rotación
R00 = eye(3)
R01 = T01(1:3,1:3)
R02 = T02(1:3,1:3)
% MTH del centro de masa de cada barra respecto a base
Pcm1 = T01(1:3,4)
Pcm2 = T02(1:3,4)
%-------------------------------------------------------------------------
% Jacobianos de Velocidad lineal
Jv1 = [diff(Pcm1(1),q1) 0; % Jacobiano eslabon 1
 diff(Pcm1(2),q1) 0;
diff(Pcm1(3),q1) 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2); % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2);
diff(Pcm2(3),q1) diff(Pcm2(3),q2)]
% Columnas para matriz Jacobiana de velocidad angular
z00 = zeros(3,1); % Vector Cero.
z0 = R00(1:3,3); % Vector Columna de matriz R00
z1 = R01(1:3,3); % Vector Columna de matriz R01
% Jacobianos de Velocidad angular
Jw1 = [z0 z00] % Jacobiano eslabon 1
Jw2 = [z0 z1] % Jacobiano eslabon 2
% Tensores
I1 = [Ixx1 0 0;
0 Iyy1 0;
0 0 Izz1];
I2 = [Ixx2 0 0;
0 Iyy2 0;
0 0 Izz2];
% Matrices necesarias para calculo
R0 = [R01;R02];
Jv = [Jv1;Jv2];
Jw = [Jw1;Jw2];
I = [I1;I2];
%-------------------------------------------------------------------------
% Matriz Euler-Lagrange
M = simplify(matrizMasa(m,I,R0,Jv,Jw))
C = simplify(matrizCoriolis(q,dq,M))
G = simplify(matrizGravedad(m,g0,Jv))
% Ecuacion Euler-Lagrange
tau = M*ddq+C*dq+G
%-------------------------------------------------------------------------

Ejercicio 3. Manipulador RR.

MDH =
[ q1, 0, L1, pi/2]
[ q2, 0, L2, 0]
R00 =
1 0 0
0 1 0
0 0 1
R01 =
[ cos(q1), 0, sin(q1)]
[ sin(q1), 0, -cos(q1)]
[ 0, 1, 0]
R02 =
[ cos(q1)*cos(q2), -cos(q1)*sin(q2), sin(q1)]
[ cos(q2)*sin(q1), -sin(q1)*sin(q2), -cos(q1)]
[ sin(q2), cos(q2), 0]
Pcm1 =
L1*cos(q1)
L1*sin(q1)
0
Pcm2 =
cos(q1)*(L1 + L2*cos(q2))
sin(q1)*(L1 + L2*cos(q2))
 L2*sin(q2)
Jv1 =
[ -L1*sin(q1), 0]
[ L1*cos(q1), 0]
[ 0, 0]
Jv2 =
[ -sin(q1)*(L1 + L2*cos(q2)), -L2*cos(q1)*sin(q2)]
[ cos(q1)*(L1 + L2*cos(q2)), -L2*sin(q1)*sin(q2)]
[ 0, L2*cos(q2)]
Jw1 =
0 0
0 0
1 0
Jw2 =
[ 0, sin(q1)]
[ 0, -cos(q1)]
[ 1, 0]
M =
[ Ixx2 + Iyy1 + L1^2*m1 + L1^2*m2 - Ixx2*cos(q2)^2 + Iyy2*cos(q2)^2 + L2^2*m2*cos(q2)^2
+ 2*L1*L2*m2*cos(q2), 0]
[
0, m2*L2^2 + Izz2]
C =
[ -dq2*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2 +
(Iyy2*sin(2*q2))/2), -dq1*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2
+ (Iyy2*sin(2*q2))/2)]
[ dq1*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2 +
(Iyy2*sin(2*q2))/2),
0]
G =
g*cos(q1)*(L1*m1 + L1*m2 + L2*m2*cos(q2))
-L2*g*m2*sin(q1)*sin(q2)
tau =
ddq1*(Ixx2 + Iyy1 + L1^2*m1 + L1^2*m2 - Ixx2*cos(q2)^2 + Iyy2*cos(q2)^2 +
L2^2*m2*cos(q2)^2 + 2*L1*L2*m2*cos(q2)) + g*cos(q1)*(L1*m1 + L1*m2 + L2*m2*cos(q2)) -
2*dq1*dq2*((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2 +
(Iyy2*sin(2*q2))/2)

((m2*sin(2*q2)*L2^2)/2 + L1*m2*sin(q2)*L2 - (Ixx2*sin(2*q2))/2 +

(Iyy2*sin(2*q2))/2)*dq1^2 + ddq2*(m2*L2^2 + Izz2) - L2*g*m2*sin(q1)*sin(q2)

clc, clear, close all

disp(' ');
disp('Ejercicio 4a. Robot SCARA de 4GDL.');
disp(' ');

syms q1 q2 q3 q4 q1p q2p q3p q4p q1pp q2pp q3pp q4pp

syms L1 L2 L3 L4 Lc1 Lc2 Lc3 Lc4 m1 m2 m3 m4 g pi
syms Ixx1 Iyy1 Izz1 Ixy1 Ixz1 Iyz1
syms Ixx2 Iyy2 Izz2 Ixy2 Ixz2 Iyz2
syms Ixx3 Iyy3 Izz3 Ixy3 Ixz3 Iyz3
syms Ixx4 Iyy4 Izz4 Ixy4 Ixz4 Iyz4
g0 = [0;-g;0];
m = [m1;m2;m3;m4];
q = [q1;q2;q3;q4];
dq = [q1p;q2p;q3p;q4p];
ddq = [q1pp;q2pp;q3pp;q4pp];
%-------------------------------------------------------------------------
% Parámetros de Denavit Hartenberg
theta = [q1;q2;0;q4];
d = [L1;0;-q3;-L4];
a = [L2;L3;0;0];
alpha = [0;0;0;0];
MDH = [theta d a alpha] % Tabla de parámetros de DH.
%-------------------------------------------------------------------------
% Matriz de Transformación Homogénea entre sistemas de coordenadas
T01 = denavit(theta(1),d(1),a(1),alpha(1));
T12 = denavit(theta(2),d(2),a(2),alpha(2));
T23 = denavit(theta(3),d(3),a(3),alpha(3));
T34 = denavit(theta(4),d(4),a(4),alpha(4));
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
% Matrices de Rotación
R00 = eye(3)
R01 = T01(1:3,1:3)
R02 = T02(1:3,1:3)
R03 = T03(1:3,1:3)
R04 = T04(1:3,1:3)
% MTH del centro de masa de la barra 1 respecto a base
Tcm01 = simplify(denavit(q1,L1/2,L2/2,0));
Pcm1 = Tcm01(1:3,4)
% MTH del centro de masa de la barra 2 respecto a base
Tcm12 = simplify(denavit(q2,0,L3/2,0));
Tcm02 = simplify(T01*Tcm12);
Pcm2 = Tcm02(1:3,4)
% MTH del centro de masa de la barra 3 respecto a base
Tcm23 = simplify(denavit(0,-q3/2,0,0));
Tcm03 = simplify(T01*T12*Tcm23);
Pcm3 = Tcm03(1:3,4)
% MTH del centro de masa de la barra 4 respecto a base
Tcm34 = simplify(denavit(q4,-L4/2,0,0));
Tcm04 = simplify(T01*T12*T23*Tcm34);
Pcm4 = Tcm04(1:3,4)
%-------------------------------------------------------------------------
% Jacobianos de Velocidad lineal
Jv1 = [diff(Pcm1(1),q1) 0 0 0; % Jacobiano eslabon 1
diff(Pcm1(2),q1) 0 0 0;
diff(Pcm1(3),q1) 0 0 0]
Jv2 = [diff(Pcm2(1),q1) diff(Pcm2(1),q2) 0 0; % Jacobiano eslabon 2
diff(Pcm2(2),q1) diff(Pcm2(2),q2) 0 0;
diff(Pcm2(3),q1) diff(Pcm2(3),q2) 0 0]
Jv3 = [diff(Pcm3(1),q1) diff(Pcm3(1),q2) diff(Pcm3(1),q3) 0; % Jacobiano eslabon 3
diff(Pcm3(2),q1) diff(Pcm3(2),q2) diff(Pcm3(2),q3) 0;
diff(Pcm3(3),q1) diff(Pcm3(3),q2) diff(Pcm3(3),q3) 0]
Jv4 = [diff(Pcm4(1),q1) diff(Pcm4(1),q2) diff(Pcm4(1),q3) diff(Pcm4(1),q4); %
Jacobiano eslabon 4
diff(Pcm4(2),q1) diff(Pcm4(2),q2) diff(Pcm4(2),q3) diff(Pcm4(2),q4);
diff(Pcm4(3),q1) diff(Pcm4(3),q2) diff(Pcm4(3),q3) diff(Pcm4(3),q4)]
% Columnas para matriz Jacobiana de velocidad angular
z00 = zeros(3,1); % Vector Cero.
z0 = R00(1:3,3); % Vector Columna de matriz R00
z1 = R01(1:3,3); % Vector Columna de matriz R01
z2 = R02(1:3,3); % Vector Columna de matriz R02
z3 = R03(1:3,3); % Vector Columna de matriz R03
% Jacobianos de Velocidad angular
Jw1 = [z0 z00 z00 z00] % Jacobiano eslabon 1
Jw2 = [z0 z1 z00 z00] % Jacobiano eslabon 2
Jw3 = [z0 z1 z00 z00] % Jacobiano eslabon 3
Jw4 = [z0 z1 z00 z3] % Jacobiano eslabon 4
% Tensores
I1 = [Ixx1 Ixy1 Ixz1;
Ixy1 Iyy1 Iyz1;
Ixz1 Iyz1 Izz1];
I2 = [Ixx2 Ixy2 Ixz2;
Ixy2 Iyy2 Iyz2;
Ixz2 Iyz2 Izz2];
I3 = [Ixx3 Ixy3 Ixz3;
Ixy3 Iyy3 Iyz3;
Ixz3 Iyz3 Izz3];
I4 = [Ixx4 Ixy4 Ixz4;
Ixy4 Iyy4 Iyz4;
Ixz4 Iyz4 Izz4];
% Matrices necesarias para calculo
R0 = [R01;R02;R03;R04];
Jv = [Jv1;Jv2;Jv3;Jv4];
Jw = [Jw1;Jw2;Jw3;Jw4];
I = [I1;I2;I3;I4];
%-------------------------------------------------------------------------
% Matriz Euler-Lagrange
M = simplify(matrizMasa(m,I,R0,Jv,Jw))
C = simplify(matrizCoriolis(q,dq,M))
G = simplify(matrizGravedad(m,g0,Jv))
% Ecuacion Euler-Lagrange
tau = M*ddq+C*dq+G
%-------------------------------------------------------------------------

Ejercicio 4a. Robot SCARA de 4GDL.

MDH =
[ q1, L1, L2, 0]
[ q2, 0, L3, 0]
[ 0, -q3, 0, 0]
[ q4, -L4, 0, 0]
R00 =
1 0 0
0 1 0
0 0 1
R01 =
[ cos(q1), -sin(q1), 0]
[ sin(q1), cos(q1), 0]
[ 0, 0, 1]
R02 =
[ cos(q1)*cos(q2) - sin(q1)*sin(q2), - cos(q1)*sin(q2) - cos(q2)*sin(q1), 0]
[ cos(q1)*sin(q2) + cos(q2)*sin(q1), cos(q1)*cos(q2) - sin(q1)*sin(q2), 0]
[ 0, 0, 1]
R03 =
[ cos(q1)*cos(q2) - sin(q1)*sin(q2), - cos(q1)*sin(q2) - cos(q2)*sin(q1), 0]
[ cos(q1)*sin(q2) + cos(q2)*sin(q1), cos(q1)*cos(q2) - sin(q1)*sin(q2), 0]
[ 0, 0, 1]
R04 =
[ cos(q4)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - sin(q4)*(cos(q1)*sin(q2) +
cos(q2)*sin(q1)), - cos(q4)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) -
sin(q4)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)), 0]
[ cos(q4)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) + sin(q4)*(cos(q1)*cos(q2) -
sin(q1)*sin(q2)), cos(q4)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) -
sin(q4)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)), 0]
[
0,
0, 1]
Pcm1 =
(L2*cos(q1))/2
(L2*sin(q1))/2
L1/2
Pcm2 =
(L3*cos(q1 + q2))/2 + L2*cos(q1)
(L3*sin(q1 + q2))/2 + L2*sin(q1)
L1
Pcm3 =
L3*cos(q1 + q2) + L2*cos(q1)
L3*sin(q1 + q2) + L2*sin(q1)
L1 - q3/2
Pcm4 =
L3*cos(q1 + q2) + L2*cos(q1)
L3*sin(q1 + q2) + L2*sin(q1)
L1 - L4/2 - q3
Jv1 =
[ -(L2*sin(q1))/2, 0, 0, 0]
[ (L2*cos(q1))/2, 0, 0, 0]
[ 0, 0, 0, 0]
Jv2 =
[ - (L3*sin(q1 + q2))/2 - L2*sin(q1), -(L3*sin(q1 + q2))/2, 0, 0]
[ (L3*cos(q1 + q2))/2 + L2*cos(q1), (L3*cos(q1 + q2))/2, 0, 0]
[ 0, 0, 0, 0]
Jv3 =
[ - L3*sin(q1 + q2) - L2*sin(q1), -L3*sin(q1 + q2), 0, 0]
[ L3*cos(q1 + q2) + L2*cos(q1), L3*cos(q1 + q2), 0, 0]
[ 0, 0, -1/2, 0]
Jv4 =
[ - L3*sin(q1 + q2) - L2*sin(q1), -L3*sin(q1 + q2), 0, 0]
[ L3*cos(q1 + q2) + L2*cos(q1), L3*cos(q1 + q2), 0, 0]
[ 0, 0, -1, 0]
Jw1 =
0 0 0 0
0 0 0 0
1 0 0 0
Jw2 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 0]
Jw3 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 0]
Jw4 =
[ 0, 0, 0, 0]
[ 0, 0, 0, 0]
[ 1, 1, 0, 1]
M =
[ Izz1 + Izz2 + Izz3 + Izz4 + (L2^2*m1)/4 + L2^2*m2 + L2^2*m3 + (L3^2*m2)/4 + L2^2*m4 +
L3^2*m3 + L3^2*m4 + L2*L3*m2*cos(q2) + 2*L2*L3*m3*cos(q2) + 2*L2*L3*m4*cos(q2), Izz2 +
Izz3 + Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4 + (L2*L3*m2*cos(q2))/2 + L2*L3*m3*cos(q2)
+ L2*L3*m4*cos(q2), 0, Izz4]
[ Izz2 + Izz3 + Izz4 + (L3^2*m2)/4 +
L3^2*m3 + L3^2*m4 + (L2*L3*m2*cos(q2))/2 + L2*L3*m3*cos(q2) + L2*L3*m4*cos(q2),
Izz2 + Izz3 + Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4, 0, Izz4]
[
0,
0, m3/4 + m4, 0]
[
Izz4,
Izz4, 0, Izz4]
C =
[ -(L2*L3*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2, -(L2*L3*sin(q2)*(q1p + q2p)*(m2 + 2*m3 +
2*m4))/2, 0, 0]
[ (L2*L3*q1p*sin(q2)*(m2 + 2*m3 + 2*m4))/2,
0, 0, 0]
[ 0,
0, 0, 0]
[ 0,
0, 0, 0]
G =
g*m3*(L3*cos(q1 + q2) + L2*cos(q1)) + g*m4*(L3*cos(q1 + q2) + L2*cos(q1)) +
g*m2*((L3*cos(q1 + q2))/2 + L2*cos(q1)) + (L2*g*m1*cos(q1))/2

(L3*g*cos(q1 + q2)*(m2 + 2*m3 + 2*m4))/2

0
tau =
Izz4*q4pp + q2pp*(Izz2 + Izz3 + Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4 +
(L2*L3*m2*cos(q2))/2 + L2*L3*m3*cos(q2) + L2*L3*m4*cos(q2)) + q1pp*(Izz1 + Izz2 + Izz3 +
Izz4 + (L2^2*m1)/4 + L2^2*m2 + L2^2*m3 + (L3^2*m2)/4 + L2^2*m4 + L3^2*m3 + L3^2*m4 +
L2*L3*m2*cos(q2) + 2*L2*L3*m3*cos(q2) + 2*L2*L3*m4*cos(q2)) + g*m3*(L3*cos(q1 + q2) +
L2*cos(q1)) + g*m4*(L3*cos(q1 + q2) + L2*cos(q1)) + g*m2*((L3*cos(q1 + q2))/2 +
L2*cos(q1)) + (L2*g*m1*cos(q1))/2 - (L2*L3*q1p*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2 -
(L2*L3*q2p*sin(q2)*(q1p + q2p)*(m2 + 2*m3 + 2*m4))/2

(L2*L3*sin(q2)*(m2 + 2*m3 + 2*m4)*q1p^2)/2 + Izz4*q4pp + q1pp*(Izz2 + Izz3 + Izz4 +

(L3^2*m2)/4 + L3^2*m3 + L3^2*m4 + (L2*L3*m2*cos(q2))/2 + L2*L3*m3*cos(q2) +
L2*L3*m4*cos(q2)) + q2pp*(Izz2 + Izz3 + Izz4 + (L3^2*m2)/4 + L3^2*m3 + L3^2*m4) +
(L3*g*cos(q1 + q2)*(m2 + 2*m3 + 2*m4))/2

q3pp*(m3/4 + m4)

Izz4*q1pp + Izz4*q2pp + Izz4*q4pp

clc, clear, close all

disp(' ');
disp('Ejercicio 4b. Robot SCARA de 4GDL.');
disp(' ');

% Datos
L1 = 400/1000;
L2 = 400/1000;
L3 = 300/1000;
L4 = 30/1000;
q1 = deg2rad(30);
q2 = deg2rad(-15);
q3 = 30/1000;
q4 = deg2rad(20);
q1p = deg2rad(10);
q2p = deg2rad(-5);
q3p = -8/1000;
q4p = deg2rad(2);
q1pp = deg2rad(1.5);
q2pp = deg2rad(-1.5);
q3pp = 1/1000;
q4pp = deg2rad(1);
m1 = 1;
m2 = 1;
m3 = 1;
m4 = 1;
g = 9.81;
Ixx1 = 1; Ixx2 = 1; Ixx3 = 1; Ixx4 = 1;
Iyy1 = 1; Iyy2 = 1; Iyy3 = 1; Iyy4 = 1;
Izz1 = 1; Izz2 = 1; Izz3 = 1; Izz4 = 1;
Ixy1 = 1; Ixy2 = 1; Ixy3 = 1; Ixy4 = 1;
Ixz1 = 1; Ixz2 = 1; Ixz3 = 1; Ixz4 = 1;
Iyz1 = 1; Iyz2 = 1; Iyz3 = 1; Iyz4 = 1;
% Matrices
g0 = [0;-g;0];
m = [m1;m2;m3;m4];
q = [q1;q2;q3;q4];
dq = [q1p;q2p;q3p;q4p];
ddq = [q1pp;q2pp;q3pp;q4pp];
%-------------------------------------------------------------------------
% Parámetros de Denavit Hartenberg
theta = [q1;q2;0;q4];
d = [L1;0;-q3;-L4];
a = [L2;L3;0;0];
alpha = [0;0;0;0];
MDH = [theta d a alpha] % Tabla de parámetros de DH.
%-------------------------------------------------------------------------
% Matriz de Transformación Homogénea entre sistemas de coordenadas
T01 = denavit(theta(1),d(1),a(1),alpha(1));
T12 = denavit(theta(2),d(2),a(2),alpha(2));
T23 = denavit(theta(3),d(3),a(3),alpha(3));
T34 = denavit(theta(4),d(4),a(4),alpha(4));
T02 = T01*T12;
T03 = T01*T12*T23;
T04 = T01*T12*T23*T34;
% Matrices de Rotación
R00 = eye(3)
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R34 = T34(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R04 = T04(1:3,1:3);
R30 = R03.';
R20 = R02.';
R10 = R01.';
% MTH del centro de masa de la barra 1 respecto a base
Tcm01 = denavit(q1,L1/2,L2/2,0);
Pcm1 = Tcm01(1:3,4)
% MTH del centro de masa de la barra 2 respecto a base
Tcm12 = denavit(q2,0,L3/2,0);
Tcm02 = T01*Tcm12;
Pcm2 = Tcm02(1:3,4)
% MTH del centro de masa de la barra 3 respecto a base
Tcm23 = denavit(0,-q3/2,0,0);
Tcm03 = T01*T12*Tcm23;
Pcm3 = Tcm03(1:3,4)
% MTH del centro de masa de la barra 4 respecto a base
Tcm34 = denavit(q4,-L4/2,0,0);
Tcm04 = T01*T12*T23*Tcm34;
Pcm4 = Tcm04(1:3,4)
%-------------------------------------------------------------------------
% Jacobianos de Velocidad lineal
Jv1 = [ -(L2*sin(q1))/2 0 0 0;
(L2*cos(q1))/2 0 0 0;
0 0 0 0]
Jv2 = [ - (L3*sin(q1 + q2))/2 - L2*sin(q1) -(L3*sin(q1 + q2))/2 0 0;
(L3*cos(q1 + q2))/2 + L2*cos(q1) (L3*cos(q1 + q2))/2 0 0;
0 0 0 0]
Jv3 = [ - L3*sin(q1 + q2) - L2*sin(q1) -L3*sin(q1 + q2) 0 0;
L3*cos(q1 + q2) + L2*cos(q1) L3*cos(q1 + q2) 0 0;
0 0 -1/2 0]
Jv4 = [ - L3*sin(q1 + q2) - L2*sin(q1) -L3*sin(q1 + q2) 0 0;
L3*cos(q1 + q2) + L2*cos(q1) L3*cos(q1 + q2) 0 0;
0 0 -1 0]
% Columnas para matriz Jacobiana de velocidad angular
z00 = zeros(3,1); % Vector Cero.
z0 = R00(1:3,3); % Vector Columna de matriz R00
z1 = R01(1:3,3); % Vector Columna de matriz R01
z2 = R02(1:3,3); % Vector Columna de matriz R02
z3 = R03(1:3,3); % Vector Columna de matriz R03
% Jacobianos de Velocidad angular
Jw1 = [z0 z00 z00 z00] % Jacobiano eslabon 1
Jw2 = [z0 z1 z00 z00] % Jacobiano eslabon 2
Jw3 = [z0 z1 z00 z00] % Jacobiano eslabon 3
Jw4 = [z0 z1 z00 z3] % Jacobiano eslabon 4
% Tensores
I1 = [Ixx1 Ixy1 Ixz1;
Ixy1 Iyy1 Iyz1;
Ixz1 Iyz1 Izz1];
I2 = [Ixx2 Ixy2 Ixz2;
Ixy2 Iyy2 Iyz2;
Ixz2 Iyz2 Izz2];
I3 = [Ixx3 Ixy3 Ixz3;
Ixy3 Iyy3 Iyz3;
Ixz3 Iyz3 Izz3];
I4 = [Ixx4 Ixy4 Ixz4;
Ixy4 Iyy4 Iyz4;
Ixz4 Iyz4 Izz4];
% Matrices necesarias para calculo
R0 = [R01;R02;R03;R04];
Jv = [Jv1;Jv2;Jv3;Jv4];
Jw = [Jw1;Jw2;Jw3;Jw4];
I = [I1;I2;I3;I4];
%-------------------------------------------------------------------------
% Matriz Euler-Lagrange
M = matrizMasa(m,I,R0,Jv,Jw)
C = [ -(L2*L3*q2p*sin(q2)*(m2 + 2*m3 + 2*m4))/2 -(L2*L3*sin(q2)*(q1p + q2p)*(m2 + 2*m3 +
2*m4))/2 0 0;
(L2*L3*q1p*sin(q2)*(m2 + 2*m3 + 2*m4))/2
0 0 0;
0
0 0 0;
0
0 0 0]
G = matrizGravedad(m,g0,Jv)
% Ecuacion Euler-Lagrange
tau = M*ddq+C*dq+G

Ejercicio 4b. Robot SCARA de 4GDL.

MDH =
0.5236 0.4000 0.4000 0
-0.2618 0 0.3000 0
0 -0.0300 0 0
0.3491 -0.0300 0 0
R00 =
1 0 0
0 1 0
0 0 1
Pcm1 =
0.1732
0.1000
0.2000
Pcm2 =
0.4913
0.2388
0.4000
Pcm3 =
0.6362
0.2776
0.3850
Pcm4 =
0.6362
0.2776
0.3550
Jv1 =
-0.1000 0 0 0
0.1732 0 0 0
0 0 0 0
Jv2 =
-0.2388 -0.0388 0 0
0.4913 0.1449 0 0
0 0 0 0
Jv3 =
-0.2776 -0.0776 0 0
 0.6362 0.2898 0 0
0 0 -0.5000 0
Jv4 =
-0.2776 -0.0776 0 0
0.6362 0.2898 0 0
0 0 -1.0000 0
Jw1 =
0 0 0 0
0 0 0 0
1 0 0 0
Jw2 =
0 0 0 0
0 0 0 0
1 1 0 0
Jw3 =
0 0 0 0
0 0 0 0
1 1 0 0
Jw4 =
0 0 0 0
0 0 0 0
1 1 0 1
M =
5.3021 3.4923 0 1.0000
3.4923 3.2025 0 1.0000
0 0 1.2500 0
1.0000 1.0000 0 1.0000
C =
-0.0068 0.0068 0 0
-0.0136 0 0 0
0 0 0 0
0 0 0 0
G =
19.0008
7.1068
0
0
tau =
19.0639
7.1295
0.0013
0.0175

Published with MATLAB® R2018a