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

% Laboratorio 4
% Robótica
% Tema: Estatica del Robot.
% Integrantes:
% - Aguilar Valdiviezo, Hans Andy
% - Juarez Mercedes, Luis Alexander
% Fecha: 05/29/2019
%-------------------------------------------------------------------------
% EJERCICIOS
%-------------------------------------------------------------------------
clc, clear, close all
% Ejercicio 1.
disp(' ');
disp('Ejercicio 1.');
disp(' ');

syms q1 q2 L1 L2 Fx Fy Fz Mx My Mz m1 m2 g pi
% Parámetros de Denavit Hartenberg
theta = [q1;q2];
d = [0;0];
a = [L1;L2];
alpha = [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))
T02 = simplify(T01*T12)
% Posicion:
P02 = T02(1:3,4)
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R02 = T02(1:3,1:3);
R10 = R01.'

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Valores de entrada en el efector final
% DATOS:
F23 = [Fx; Fy; Fz]
M23 = [Mx; My; Mz]
% Gravedad
%g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F12 = simplify(R12*F23 - m2*R10*g0)
F01 = simplify(R01*F12 - m1*eye(3)*g0)
%-------------------------------------------------------------------------
% Cálculo de Pares.
M12 = simplify(R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23))
M01 = simplify(R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12))
% Tau final.
tau = [M01;M12]
%-------------------------------------------------------------------------

Ejercicio 1.

MDH =
[ q1, 0, L1, 0]
[ q2, 0, L2, 0]
T01 =
[ cos(q1), -sin(q1), 0, L1*cos(q1)]
[ sin(q1), cos(q1), 0, L1*sin(q1)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T12 =
[ cos(q2), -sin(q2), 0, L2*cos(q2)]
[ sin(q2), cos(q2), 0, L2*sin(q2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T02 =
[ cos(q1 + q2), -sin(q1 + q2), 0, L2*cos(q1 + q2) + L1*cos(q1)]
[ sin(q1 + q2), cos(q1 + q2), 0, L2*sin(q1 + q2) + L1*sin(q1)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
P02 =
L2*cos(q1 + q2) + L1*cos(q1)
L2*sin(q1 + q2) + L1*sin(q1)
0
R10 =
[ cos(q1), sin(q1), 0]
[ -sin(q1), cos(q1), 0]
[ 0, 0, 1]
F23 =
Fx
Fy
Fz
M23 =
Mx
My
Mz
r12 =
L2
0
0
r01 =
L1
0
0
r1c2 =
L2/2
0
0
r0c1 =
L1/2
0
0
F12 =
Fx*cos(q2) - Fy*sin(q2) + g*m2*sin(q1)
Fy*cos(q2) + Fx*sin(q2) + g*m2*cos(q1)
Fz
F01 =
Fx*cos(q1 +
q2) - Fy*sin(q1 + q2)
g*m1 + cos(q1)*(Fy*cos(q2) + Fx*sin(q2) + g*m2*cos(q1)) + sin(q1)*(Fx*cos(q2) -
Fy*sin(q2) + g*m2*sin(q1))

Fz
M12 =
Mx*cos(q2) - My*sin(q2) + Fz*L2*sin(q2)
My*cos(q2) + Mx*sin(q2) - Fz*L2*cos(q2)
Mz + Fy*L2 + (L2*g*m2*cos(q1 + q2))/2
M01 =
cos(q1)*(Mx*cos(q2) - My*sin(q2) + Fz*L2*sin(q2)) - sin(q1)*(My*cos(q2) + Mx*sin(q2) -
Fz*L2*cos(q2)) + Fz*L1*sin(q1)
cos(q1)*(My*cos(q2) + Mx*sin(q2) - Fz*L2*cos(q2)) + sin(q1)*(Mx*cos(q2) - My*sin(q2) +
Fz*L2*sin(q2)) - Fz*L1*cos(q1)
Mz + Fy*L2 + Fy*L1*cos(q2) + Fx*L1*sin(q2) + (L2*g*m2*cos(q1 + q2))/2 +
(L1*g*m1*cos(q1))/2 + L1*g*m2*cos(q1)
tau =
cos(q1)*(Mx*cos(q2) - My*sin(q2) + Fz*L2*sin(q2)) - sin(q1)*(My*cos(q2) + Mx*sin(q2) -
Fz*L2*cos(q2)) + Fz*L1*sin(q1)
cos(q1)*(My*cos(q2) + Mx*sin(q2) - Fz*L2*cos(q2)) + sin(q1)*(Mx*cos(q2) - My*sin(q2) +
Fz*L2*sin(q2)) - Fz*L1*cos(q1)
Mz + Fy*L2 + Fy*L1*cos(q2) + Fx*L1*sin(q2) + (L2*g*m2*cos(q1 + q2))/2 +
(L1*g*m1*cos(q1))/2 + L1*g*m2*cos(q1)

Mx*cos(q2) - My*sin(q2) + Fz*L2*sin(q2)

My*cos(q2) + Mx*sin(q2) - Fz*L2*cos(q2)

Mz +
Fy*L2 + (L2*g*m2*cos(q1 + q2))/2

clc, clear, close all

% Ejercicio 2.
disp(' ');
disp('Ejercicio 2.');
disp(' ');

syms q1 q2 L Fx Fy Fz Mx My Mz m1 m2 g pi
% Parámetros de Denavit Hartenberg
theta = [q1;0];
d = [0;q2];
a = [L;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 = denavit(theta(1),d(1),a(1),alpha(1))
T12 = denavit(theta(2),d(2),a(2),alpha(2))
T02 = simplify(T01*T12)
% Posicion:
P02 = T02(1:3,4)
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R02 = T02(1:3,1:3);
R10 = R01.'

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Valores de entrada en el efector final
% DATOS:
Fz = 0;
Mx = 0;
My = 0;
Mz = 0;
F23 = [Fx; Fy; Fz]
M23 = [Mx; My; Mz]
% Gravedad
%g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F12 = simplify(R12*F23 - m2*R10*g0)
F01 = simplify(R01*F12 - m1*eye(3)*g0)
%-------------------------------------------------------------------------
% Cálculo de Pares.
M12 = simplify(R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23))
M01 = simplify(R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12))
% Tau final.
tau = [M01;M12]
%-------------------------------------------------------------------------

Ejercicio 2.

MDH =
[ q1, 0, L, -pi/2]
[ 0, q2, 0, 0]
T01 =
[ cos(q1), 0, -sin(q1), L*cos(q1)]
[ sin(q1), 0, cos(q1), L*sin(q1)]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
T12 =
[ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, q2]
[ 0, 0, 0, 1]
T02 =
[ cos(q1), 0, -sin(q1), L*cos(q1) - q2*sin(q1)]
[ sin(q1), 0, cos(q1), L*sin(q1) + q2*cos(q1)]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
P02 =
L*cos(q1) - q2*sin(q1)
L*sin(q1) + q2*cos(q1)
0
R10 =
[ cos(q1), sin(q1), 0]
[ 0, 0, -1]
[ -sin(q1), cos(q1), 0]
F23 =
Fx
Fy
0
M23 =
0
0
0
r12 =
0
0
q2
r01 =
L
0
0
r1c2 =
0
0
q2/2
r0c1 =
L/2
0
0
F12 =
Fx + g*m2*sin(q1)
Fy
g*m2*cos(q1)
F01 =
Fx*cos(q1)
g*m1 + g*m2 + Fx*sin(q1)
-Fy
M12 =
-Fy*q2
(q2*(2*Fx + g*m2*sin(q1)))/2
0
M01 =
-Fy*(L*sin(q1) + q2*cos(q1))
Fy*(L*cos(q1) - q2*sin(q1))
(L*g*m1*cos(q1))/2 - Fx*q2 + L*g*m2*cos(q1) - (g*m2*q2*sin(q1))/2
tau =
-Fy*(L*sin(q1) + q2*cos(q1))
Fy*(L*cos(q1) - q2*sin(q1))
(L*g*m1*cos(q1))/2 - Fx*q2 + L*g*m2*cos(q1) - (g*m2*q2*sin(q1))/2
-Fy*q2
(q2*(2*Fx + g*m2*sin(q1)))/2
0
clc, clear, close all
% Ejercicio 3a.
disp(' ');
disp('Ejercicio 3a.');
disp(' ');

syms q1 q2 q3 L1 L2 L3 Fx Fy Fz Mx My Mz m1 m2 m3 g pi
% Parámetros de Denavit Hartenberg
theta = [q1;q2;q3];
d = [0;0;0];
a = [L1;L2;L3];
alpha = [pi/2;0;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)))
T23 = simplify(denavit(theta(3),d(3),a(3),alpha(3)))
T02 = simplify(T01*T12)
T03 = simplify(T01*T12*T23)
% Posicion:
P03 = T03(1:3,4)
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R20 = R02.'
R10 = R01.'

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Valores de entrada en el efector final
% DATOS:
F34 = [Fx; Fy; Fz]
M34 = [Mx; My; Mz]
% Gravedad
%g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F23 = simplify(R23*F34 - m3*R20*g0)
F12 = simplify(R12*F23 - m2*R10*g0)
F01 = simplify(R01*F12 - m1*eye(3)*g0)
%-------------------------------------------------------------------------
% Cálculo de Pares.
M23 = simplify(R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34))
M12 = simplify(R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23))
M01 = simplify(R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12))
% Tau final.
tau = [M01;M12;M23]
%-------------------------------------------------------------------------

Ejercicio 3a.

MDH =
[ q1, 0, L1, pi/2]
[ q2, 0, L2, 0]
[ q3, 0, L3, 0]
T01 =
[ cos(q1), 0, sin(q1), L1*cos(q1)]
[ sin(q1), 0, -cos(q1), L1*sin(q1)]
[ 0, 1, 0, 0]
[ 0, 0, 0, 1]
T12 =
[ cos(q2), -sin(q2), 0, L2*cos(q2)]
[ sin(q2), cos(q2), 0, L2*sin(q2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T23 =
[ cos(q3), -sin(q3), 0, L3*cos(q3)]
[ sin(q3), cos(q3), 0, L3*sin(q3)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T02 =
[ cos(q1)*cos(q2), -cos(q1)*sin(q2), sin(q1), cos(q1)*(L1 + L2*cos(q2))]
[ cos(q2)*sin(q1), -sin(q1)*sin(q2), -cos(q1), sin(q1)*(L1 + L2*cos(q2))]
[ sin(q2), cos(q2), 0, L2*sin(q2)]
[ 0, 0, 0, 1]
T03 =
[ cos(q2 + q3)*cos(q1), -sin(q2 + q3)*cos(q1), sin(q1), cos(q1)*(L1 + L3*cos(q2 + q3) +
L2*cos(q2))]
[ cos(q2 + q3)*sin(q1), -sin(q2 + q3)*sin(q1), -cos(q1), sin(q1)*(L1 + L3*cos(q2 + q3) +
L2*cos(q2))]
[ sin(q2 + q3), cos(q2 + q3), 0, L3*sin(q2 + q3)
+ L2*sin(q2)]
[ 0, 0, 0,
1]
P03 =
cos(q1)*(L1 + L3*cos(q2 + q3) + L2*cos(q2))
sin(q1)*(L1 + L3*cos(q2 + q3) + L2*cos(q2))
L3*sin(q2 + q3) + L2*sin(q2)
R20 =
[ cos(q1)*cos(q2), cos(q2)*sin(q1), sin(q2)]
[ -cos(q1)*sin(q2), -sin(q1)*sin(q2), cos(q2)]
[ sin(q1), -cos(q1), 0]
R10 =
[ cos(q1), sin(q1), 0]
[ 0, 0, 1]
[ sin(q1), -cos(q1), 0]
F34 =
Fx
Fy
Fz
M34 =
Mx
My
Mz
r23 =
L3
0
0
r12 =
L2
0
0
r01 =
L1
0
0
r2c3 =
L3/2
0
0
r1c2 =
L2/2
0
0
r0c1 =
L1/2
0
0
F23 =
Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q2)*sin(q1)
Fy*cos(q3) + Fx*sin(q3) - g*m3*sin(q1)*sin(q2)
Fz - g*m3*cos(q1)
F12 =
cos(q2)*(Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q2)*sin(q1)) - sin(q2)*(Fy*cos(q3) +
Fx*sin(q3) - g*m3*sin(q1)*sin(q2)) + g*m2*sin(q1)

Fy*cos(q2 + q3) + Fx*sin(q2 + q3)

Fz - g*m2*cos(q1) - g*m3*cos(q1)
F01 =
cos(q1)*(cos(q2)*(Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q2)*sin(q1)) -
sin(q2)*(Fy*cos(q3) + Fx*sin(q3) - g*m3*sin(q1)*sin(q2)) + g*m2*sin(q1)) -
sin(q1)*(g*m2*cos(q1) - Fz + g*m3*cos(q1))
g*m1 + cos(q1)*(g*m2*cos(q1) - Fz + g*m3*cos(q1)) + sin(q1)*(cos(q2)*(Fx*cos(q3) -
Fy*sin(q3) + g*m3*cos(q2)*sin(q1)) - sin(q2)*(Fy*cos(q3) + Fx*sin(q3) -
g*m3*sin(q1)*sin(q2)) + g*m2*sin(q1))

Fy*cos(q2 + q3) + Fx*sin(q2 + q3)

M23 =
Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2
My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2
Mz + Fy*L3 - (L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 - (L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2
M12 =
cos(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) -
sin(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) +
L2*sin(q2)*(Fz - g*m3*cos(q1)) - (L2*g*m2*cos(q1)*sin(q2))/2
cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) +
sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz - g*m3*cos(q1)) + (L2*g*m2*cos(q1)*cos(q2))/2
Mz + Fy*L3 + Fy*L2*cos(q3) +
Fx*L2*sin(q3) - (L2*g*m2*sin(q1)*sin(q2))/2 - L2*g*m3*sin(q1)*sin(q2) -
(L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 - (L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2
M01 =
sin(q1)*(Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*sin(q1)*sin(q2))/2 -
L2*g*m3*sin(q1)*sin(q2) - (L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 -
(L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2) - cos(q1)*(sin(q2)*(My*cos(q3) + Mx*sin(q3) -
Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) - cos(q2)*(Mx*cos(q3) - My*sin(q3) +
Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) - L2*sin(q2)*(Fz - g*m3*cos(q1)) +
(L2*g*m2*cos(q1)*sin(q2))/2) + L1*sin(q1)*(Fy*cos(q2 + q3) + Fx*sin(q2 + q3))

Mx*cos(q2)*cos(q3)*sin(q1) - Fy*L3*cos(q1) - Mz*cos(q1) - My*cos(q2)*sin(q1)*sin(q3) -

My*cos(q3)*sin(q1)*sin(q2) - Mx*sin(q1)*sin(q2)*sin(q3) - Fy*L2*cos(q1)*cos(q3) -
Fx*L2*cos(q1)*sin(q3) + Fz*L2*sin(q1)*sin(q2) - Fy*L1*cos(q1)*cos(q2)*cos(q3) -
Fx*L1*cos(q1)*cos(q2)*sin(q3) - Fx*L1*cos(q1)*cos(q3)*sin(q2) +
Fy*L1*cos(q1)*sin(q2)*sin(q3) + Fz*L3*cos(q2)*sin(q1)*sin(q3) +
Fz*L3*cos(q3)*sin(q1)*sin(q2)

My*cos(q2)*cos(q3) - Fz*L1 + Mx*cos(q2)*sin(q3) + Mx*cos(q3)*sin(q2) -

My*sin(q2)*sin(q3) - Fz*L2*cos(q2) + (L1*g*m1*cos(q1))/2 + L1*g*m2*cos(q1) +
L1*g*m3*cos(q1) - Fz*L3*cos(q2)*cos(q3) + Fz*L3*sin(q2)*sin(q3) +
(L2*g*m2*cos(q1)*cos(q2))/2 + L2*g*m3*cos(q1)*cos(q2) -
(L3*g*m3*cos(q1)*sin(q2)*sin(q3))/2 + (L3*g*m3*cos(q1)*cos(q2)*cos(q3))/2
tau =
sin(q1)*(Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*sin(q1)*sin(q2))/2 -
L2*g*m3*sin(q1)*sin(q2) - (L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 -
(L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2) - cos(q1)*(sin(q2)*(My*cos(q3) + Mx*sin(q3) -
Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) - cos(q2)*(Mx*cos(q3) - My*sin(q3) +
Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) - L2*sin(q2)*(Fz - g*m3*cos(q1)) +
(L2*g*m2*cos(q1)*sin(q2))/2) + L1*sin(q1)*(Fy*cos(q2 + q3) + Fx*sin(q2 + q3))

Mx*cos(q2)*cos(q3)*sin(q1) - Fy*L3*cos(q1) - Mz*cos(q1) - My*cos(q2)*sin(q1)*sin(q3) -

My*cos(q3)*sin(q1)*sin(q2) - Mx*sin(q1)*sin(q2)*sin(q3) - Fy*L2*cos(q1)*cos(q3) -
Fx*L2*cos(q1)*sin(q3) + Fz*L2*sin(q1)*sin(q2) - Fy*L1*cos(q1)*cos(q2)*cos(q3) -
Fx*L1*cos(q1)*cos(q2)*sin(q3) - Fx*L1*cos(q1)*cos(q3)*sin(q2) +
Fy*L1*cos(q1)*sin(q2)*sin(q3) + Fz*L3*cos(q2)*sin(q1)*sin(q3) +
Fz*L3*cos(q3)*sin(q1)*sin(q2)

My*cos(q2)*cos(q3) - Fz*L1 + Mx*cos(q2)*sin(q3) + Mx*cos(q3)*sin(q2) -

My*sin(q2)*sin(q3) - Fz*L2*cos(q2) + (L1*g*m1*cos(q1))/2 + L1*g*m2*cos(q1) +
L1*g*m3*cos(q1) - Fz*L3*cos(q2)*cos(q3) + Fz*L3*sin(q2)*sin(q3) +
(L2*g*m2*cos(q1)*cos(q2))/2 + L2*g*m3*cos(q1)*cos(q2) -
(L3*g*m3*cos(q1)*sin(q2)*sin(q3))/2 + (L3*g*m3*cos(q1)*cos(q2)*cos(q3))/2

cos(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) -

sin(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) +
L2*sin(q2)*(Fz - g*m3*cos(q1)) - (L2*g*m2*cos(q1)*sin(q2))/2

cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2) +

sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz - g*m3*cos(q1)) + (L2*g*m2*cos(q1)*cos(q2))/2

Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*sin(q1)*sin(q2))/2 -

L2*g*m3*sin(q1)*sin(q2) - (L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 -
(L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2

Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) - (L3*g*m3*cos(q1)*sin(q3))/2

My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) + (L3*g*m3*cos(q1)*cos(q3))/2

Mz + Fy*L3 - (L3*g*m3*cos(q2)*sin(q1)*sin(q3))/2 - (L3*g*m3*cos(q3)*sin(q1)*sin(q2))/2

clc, clear, close all

% Ejercicio 3b.
disp(' ');
disp('Ejercicio 3b.');
disp(' ');

% DATOS:
L1 = 100 % cm
L2 = 80 % cm
L3 = 25 % cm
q1 = deg2rad(0) % deg
q2 = deg2rad(30) % deg
q3 = deg2rad(12) % deg
m1 = 1 % kg
m2 = 1 % kg
m3 = 1 % kg
F34 = [5; 8; 6] % N
M34 = [2; 1.5; 0.8] % N*cm
% Parámetros de Denavit Hartenberg
theta = [q1;q2;q3];
d = [0;0;0];
a = [L1;L2;L3];
alpha = [pi/2;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));
T02 = T01*T12;
T03 = T01*T12*T23;
% Posicion:
P03 = T03(1:3,4);
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R20 = R02.';
R10 = R01.';

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Gravedad
g = 9.81 % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F23 = R23*F34 - m3*R20*g0
F12 = R12*F23 - m2*R10*g0
F01 = R01*F12 - m1*eye(3)*g0
%-------------------------------------------------------------------------
% Cálculo de Pares.
M23 = R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34)
M12 = R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23)
M01 = R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12)
% Tau final.
tau = [M01;M12;M23]
%-------------------------------------------------------------------------

Ejercicio 3b.

L1 =
100
L2 =
80
L3 =
25
q1 =
0
q2 =
0.5236
q3 =
0.2094
m1 =
1
m2 =
1
m3 =
1
F34 =
5
8
6
M34 =
2.0000
1.5000
0.8000
g =
9.8100
r23 =
25
0
0
r12 =
80
0
0
r01 =
100
 0
0
r2c3 =
12.5000
0
0
r1c2 =
40
0
0
r0c1 =
50
0
0
F23 =
3.2274
8.8647
-3.8100
F12 =
-1.6373
9.2908
-13.6200
F01 =
-1.6373
23.4300
9.2908
M23 =
7.3360
-24.8937
200.8000
M12 =
-329.8000
585.9023
909.9791
M01 =
1.0e+03 *
-0.3298
-1.8391
2.4384
tau =
1.0e+03 *
-0.3298
-1.8391
2.4384
-0.3298
0.5859
0.9100
0.0073
-0.0249
0.2008

clc, clear, close all

% Ejercicio 4a.
disp(' ');
disp('Ejercicio 4a.');
disp(' ');
syms q1 q2 q3 L1 L2 L3 Fx Fy Fz Mx My Mz m1 m2 m3 g pi
% Parámetros de Denavit Hartenberg
theta = [q1+pi/2;q2;q3];
d = [L1;0;0];
a = [0;L2;L3];
alpha = [pi/2;0;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)))
T23 = simplify(denavit(theta(3),d(3),a(3),alpha(3)))
T02 = simplify(T01*T12)
T03 = simplify(T01*T12*T23)
% Posicion:
P03 = T03(1:3,4)
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R20 = R02.'
R10 = R01.'

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Valores de entrada en el efector final
% DATOS:
F34 = [Fx; Fy; Fz]
M34 = [Mx; My; Mz]
% Gravedad
%g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F23 = simplify(R23*F34 - m3*R20*g0)
F12 = simplify(R12*F23 - m2*R10*g0)
F01 = simplify(R01*F12 - m1*eye(3)*g0)
%-------------------------------------------------------------------------
% Cálculo de Pares.
M23 = simplify(R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34))
M12 = simplify(R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23))
M01 = simplify(R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12))
% Tau final.
tau = [M01;M12;M23]
%-------------------------------------------------------------------------

Ejercicio 4a.
MDH =
[ q1 + pi/2, L1, 0, pi/2]
[ q2, 0, L2, 0]
[ q3, 0, L3, 0]
T01 =
[ -sin(q1), 0, cos(q1), 0]
[ cos(q1), 0, sin(q1), 0]
[ 0, 1, 0, L1]
[ 0, 0, 0, 1]
T12 =
[ cos(q2), -sin(q2), 0, L2*cos(q2)]
[ sin(q2), cos(q2), 0, L2*sin(q2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T23 =
[ cos(q3), -sin(q3), 0, L3*cos(q3)]
[ sin(q3), cos(q3), 0, L3*sin(q3)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T02 =
[ -cos(q2)*sin(q1), sin(q1)*sin(q2), cos(q1), -L2*cos(q2)*sin(q1)]
[ cos(q1)*cos(q2), -cos(q1)*sin(q2), sin(q1), L2*cos(q1)*cos(q2)]
[ sin(q2), cos(q2), 0, L1 + L2*sin(q2)]
[ 0, 0, 0, 1]
T03 =
[ -cos(q2 + q3)*sin(q1), sin(q2 + q3)*sin(q1), cos(q1), -sin(q1)*(L3*cos(q2 + q3) +
L2*cos(q2))]
[ cos(q2 + q3)*cos(q1), -sin(q2 + q3)*cos(q1), sin(q1), cos(q1)*(L3*cos(q2 + q3) +
L2*cos(q2))]
[ sin(q2 + q3), cos(q2 + q3), 0, L1 + L3*sin(q2 + q3) +
L2*sin(q2)]
[ 0, 0, 0,
1]
P03 =
-sin(q1)*(L3*cos(q2 + q3) + L2*cos(q2))
cos(q1)*(L3*cos(q2 + q3) + L2*cos(q2))
L1 + L3*sin(q2 + q3) + L2*sin(q2)
R20 =
[ -cos(q2)*sin(q1), cos(q1)*cos(q2), sin(q2)]
[ sin(q1)*sin(q2), -cos(q1)*sin(q2), cos(q2)]
[ cos(q1), sin(q1), 0]
R10 =
[ -sin(q1), cos(q1), 0]
[ 0, 0, 1]
[ cos(q1), sin(q1), 0]
F34 =
Fx
Fy
Fz
M34 =
Mx
My
Mz
r23 =
L3
0
0
r12 =
L2
0
0
r01 =
0
L1
0
r2c3 =
L3/2
0
0
r1c2 =
L2/2
0
0
r0c1 =
0
L1/2
0
F23 =
Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q1)*cos(q2)
Fy*cos(q3) + Fx*sin(q3) - g*m3*cos(q1)*sin(q2)
Fz + g*m3*sin(q1)
F12 =
cos(q2)*(Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q1)*cos(q2)) - sin(q2)*(Fy*cos(q3) +
Fx*sin(q3) - g*m3*cos(q1)*sin(q2)) + g*m2*cos(q1)

Fy*cos(q2 + q3) + Fx*sin(q2 + q3)

Fz + g*m2*sin(q1) + g*m3*sin(q1)
F01 =
cos(q1)*(Fz + g*m2*sin(q1) + g*m3*sin(q1)) - sin(q1)*(cos(q2)*(Fx*cos(q3) -
Fy*sin(q3) + g*m3*cos(q1)*cos(q2)) - sin(q2)*(Fy*cos(q3) + Fx*sin(q3) -
g*m3*cos(q1)*sin(q2)) + g*m2*cos(q1))
sin(q1)*(Fz + g*m2*sin(q1) + g*m3*sin(q1)) + g*m1 + cos(q1)*(cos(q2)*(Fx*cos(q3) -
Fy*sin(q3) + g*m3*cos(q1)*cos(q2)) - sin(q2)*(Fy*cos(q3) + Fx*sin(q3) -
g*m3*cos(q1)*sin(q2)) + g*m2*cos(q1))

Fy*cos(q2 + q3) + Fx*sin(q2 + q3)

M23 =
Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2
My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2
Mz + Fy*L3 - (L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 - (L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2
M12 =
cos(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -
sin(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +
L2*sin(q2)*(Fz + g*m3*sin(q1)) + (L2*g*m2*sin(q1)*sin(q2))/2
cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +
sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz + g*m3*sin(q1)) - (L2*g*m2*cos(q2)*sin(q1))/2
Mz + Fy*L3 + Fy*L2*cos(q3) +
Fx*L2*sin(q3) - (L2*g*m2*cos(q1)*sin(q2))/2 - L2*g*m3*cos(q1)*sin(q2) -
(L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 - (L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2
M01 =
cos(q1)*(Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*cos(q1)*sin(q2))/2 -
L2*g*m3*cos(q1)*sin(q2) - (L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 -
(L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2) - L1*(sin(q1)*(Fz + g*m2*sin(q1) + g*m3*sin(q1)) +
cos(q1)*(cos(q2)*(Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q1)*cos(q2)) - sin(q2)*(Fy*cos(q3)
+ Fx*sin(q3) - g*m3*cos(q1)*sin(q2)) + g*m2*cos(q1))) - sin(q1)*(cos(q2)*(Mx*cos(q3) -
My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) - sin(q2)*(My*cos(q3) +
Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) + L2*sin(q2)*(Fz +
g*m3*sin(q1)) + (L2*g*m2*sin(q1)*sin(q2))/2) - (L1*g*m1)/2

Mz*sin(q1) + Fz*L1*cos(q1) + Fy*L3*sin(q1) + Mx*cos(q1)*cos(q2)*cos(q3) -

My*cos(q1)*cos(q2)*sin(q3) - My*cos(q1)*cos(q3)*sin(q2) - Mx*cos(q1)*sin(q2)*sin(q3) +
Fy*L2*cos(q3)*sin(q1) + Fz*L2*cos(q1)*sin(q2) + Fx*L2*sin(q1)*sin(q3) -
Fx*L1*cos(q2)*cos(q3)*sin(q1) + Fz*L3*cos(q1)*cos(q2)*sin(q3) +
Fz*L3*cos(q1)*cos(q3)*sin(q2) + Fy*L1*cos(q2)*sin(q1)*sin(q3) +
Fy*L1*cos(q3)*sin(q1)*sin(q2) + Fx*L1*sin(q1)*sin(q2)*sin(q3)

cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +

sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz + g*m3*sin(q1)) - (L2*g*m2*cos(q2)*sin(q1))/2
tau =
cos(q1)*(Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*cos(q1)*sin(q2))/2 -
L2*g*m3*cos(q1)*sin(q2) - (L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 -
(L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2) - L1*(sin(q1)*(Fz + g*m2*sin(q1) + g*m3*sin(q1)) +
cos(q1)*(cos(q2)*(Fx*cos(q3) - Fy*sin(q3) + g*m3*cos(q1)*cos(q2)) - sin(q2)*(Fy*cos(q3)
+ Fx*sin(q3) - g*m3*cos(q1)*sin(q2)) + g*m2*cos(q1))) - sin(q1)*(cos(q2)*(Mx*cos(q3) -
My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) - sin(q2)*(My*cos(q3) +
Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) + L2*sin(q2)*(Fz +
g*m3*sin(q1)) + (L2*g*m2*sin(q1)*sin(q2))/2) - (L1*g*m1)/2

Mz*sin(q1) + Fz*L1*cos(q1) + Fy*L3*sin(q1) + Mx*cos(q1)*cos(q2)*cos(q3) -

My*cos(q1)*cos(q2)*sin(q3) - My*cos(q1)*cos(q3)*sin(q2) - Mx*cos(q1)*sin(q2)*sin(q3) +
Fy*L2*cos(q3)*sin(q1) + Fz*L2*cos(q1)*sin(q2) + Fx*L2*sin(q1)*sin(q3) -
Fx*L1*cos(q2)*cos(q3)*sin(q1) + Fz*L3*cos(q1)*cos(q2)*sin(q3) +
Fz*L3*cos(q1)*cos(q3)*sin(q2) + Fy*L1*cos(q2)*sin(q1)*sin(q3) +
Fy*L1*cos(q3)*sin(q1)*sin(q2) + Fx*L1*sin(q1)*sin(q2)*sin(q3)

cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +

sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz + g*m3*sin(q1)) - (L2*g*m2*cos(q2)*sin(q1))/2

cos(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -

sin(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +
L2*sin(q2)*(Fz + g*m3*sin(q1)) + (L2*g*m2*sin(q1)*sin(q2))/2

cos(q2)*(My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2) +

sin(q2)*(Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2) -
L2*cos(q2)*(Fz + g*m3*sin(q1)) - (L2*g*m2*cos(q2)*sin(q1))/2

Mz + Fy*L3 + Fy*L2*cos(q3) + Fx*L2*sin(q3) - (L2*g*m2*cos(q1)*sin(q2))/2 -

L2*g*m3*cos(q1)*sin(q2) - (L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 -
(L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2

Mx*cos(q3) - My*sin(q3) + Fz*L3*sin(q3) + (L3*g*m3*sin(q1)*sin(q3))/2

My*cos(q3) + Mx*sin(q3) - Fz*L3*cos(q3) - (L3*g*m3*cos(q3)*sin(q1))/2

Mz + Fy*L3 - (L3*g*m3*cos(q1)*cos(q2)*sin(q3))/2 - (L3*g*m3*cos(q1)*cos(q3)*sin(q2))/2

clc, clear, close all

% Ejercicio 4b.
disp(' ');
disp('Ejercicio 4b.');
disp(' ');

% DATOS:
L1 = 0.8 % m
L2 = 0.4 % m
L3 = 0.2 % m
q1 = deg2rad(0) % deg
q2 = deg2rad(30) % deg
q3 = deg2rad(12) % deg
m1 = 1 % kg
m2 = 1 % kg
m3 = 1 % kg
F34 = [5; 8; 6] % N
M34 = [2; 1.5; 0.8]/100 % N*m
% Parámetros de Denavit Hartenberg
theta = [q1+pi/2;q2;q3];
d = [L1;0;0];
a = [0;L2;L3];
alpha = [pi/2;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));
T02 = T01*T12;
T03 = T01*T12*T23;
% Posicion:
P03 = T03(1:3,4);
% Matrices de Rotación
R01 = T01(1:3,1:3);
R12 = T12(1:3,1:3);
R23 = T23(1:3,1:3);
R02 = T02(1:3,1:3);
R03 = T03(1:3,1:3);
R20 = R02.';
R10 = R01.';

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Gravedad
g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F23 = R23*F34 - m3*R20*g0
F12 = R12*F23 - m2*R10*g0
F01 = R01*F12 - m1*eye(3)*g0
%-------------------------------------------------------------------------
% Cálculo de Pares.
M23 = R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34)
M12 = R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23)
M01 = R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12)
% Tau final.
tau = [M01;M12;M23]
%-------------------------------------------------------------------------

Ejercicio 4b.

L1 =
0.8000
L2 =
0.4000
L3 =
0.2000
q1 =
0
q2 =
0.5236
q3 =
0.2094
m1 =
1
m2 =
1
m3 =
1
F34 =
5
8
6
M34 =
0.0200
0.0150
0.0080
r23 =
0.2000
0
0
r12 =
0.4000
0
0
r01 =
0
0.8000
0.0000
r2c3 =
0.1000
0
0
r1c2 =
0.2000
0
 0
r0c1 =
0
0.4000
0.0000
F23 =
11.7232
3.9597
6.0000
F12 =
17.9827
9.2908
6.0000
F01 =
6.0000
27.7927
9.2908
M23 =
0.2659
-1.1549
0.9516
M12 =
2.0078
-2.9457
1.5545
M01 =
-16.7557
6.8078
-2.9457
tau =
-16.7557
6.8078
-2.9457
2.0078
-2.9457
1.5545
0.2659
-1.1549
0.9516

clc, clear, close all

% Ejercicio 5a.
disp(' ');
disp('Ejercicio 5a.');
disp(' ');

syms q1 q2 q3 q4 L1 L2 L3 L4 Fx Fy Fz Mx My Mz m1 m2 m3 m4 g pi
% 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 = simplify(denavit(theta(1),d(1),a(1),alpha(1)))
T12 = simplify(denavit(theta(2),d(2),a(2),alpha(2)))
T23 = simplify(denavit(theta(3),d(3),a(3),alpha(3)))
T34 = simplify(denavit(theta(4),d(4),a(4),alpha(4)))
T02 = simplify(T01*T12)
T03 = simplify(T01*T12*T23)
T04 = simplify(T01*T12*T23*T34)
% Posicion:
P04 = T04(1:3,4)
% Matrices de Rotación
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.';

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Valores de entrada en el efector final
% DATOS:
F45 = [Fx; Fy; Fz]
M45 = [Mx; My; Mz]
% Gravedad
%g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r34 = [a(4);d(4)*sin(alpha(4));d(4)*cos(alpha(4))]
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r3c4 = r34/2 % Centro de masa se encuentra en el punto medio.
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F34 = simplify(R34*F45 - m4*R30*g0)
F23 = simplify(R23*F34 - m3*R20*g0)
F12 = simplify(R12*F23 - m2*R10*g0)
F01 = simplify(R01*F12 - m1*eye(3)*g0)
%-------------------------------------------------------------------------
% Cálculo de Pares.
M34 = simplify(R34*M45-cross(R34*r3c4,m4*R30*g0)+cross(R34*r34,R34*F45))
M23 = simplify(R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34))
M12 = simplify(R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23))
M01 = simplify(R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12))
% Tau final.
tau = [M01;M12;M23;M34]
%-------------------------------------------------------------------------

Ejercicio 5a.

MDH =
[ q1, L1, L2, 0]
[ q2, 0, L3, 0]
[ 0, -q3, 0, 0]
[ q4, -L4, 0, 0]
T01 =
[ cos(q1), -sin(q1), 0, L2*cos(q1)]
[ sin(q1), cos(q1), 0, L2*sin(q1)]
[ 0, 0, 1, L1]
[ 0, 0, 0, 1]
T12 =
[ cos(q2), -sin(q2), 0, L3*cos(q2)]
[ sin(q2), cos(q2), 0, L3*sin(q2)]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
T23 =
[ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, -q3]
[ 0, 0, 0, 1]
T34 =
[ cos(q4), -sin(q4), 0, 0]
[ sin(q4), cos(q4), 0, 0]
[ 0, 0, 1, -L4]
[ 0, 0, 0, 1]
T02 =
[ cos(q1 + q2), -sin(q1 + q2), 0, L3*cos(q1 + q2) + L2*cos(q1)]
[ sin(q1 + q2), cos(q1 + q2), 0, L3*sin(q1 + q2) + L2*sin(q1)]
[ 0, 0, 1, L1]
[ 0, 0, 0, 1]
T03 =
[ cos(q1 + q2), -sin(q1 + q2), 0, L3*cos(q1 + q2) + L2*cos(q1)]
[ sin(q1 + q2), cos(q1 + q2), 0, L3*sin(q1 + q2) + L2*sin(q1)]
[ 0, 0, 1, L1 - q3]
[ 0, 0, 0, 1]
T04 =
[ cos(q1 + q2 + q4), -sin(q1 + q2 + q4), 0, L3*cos(q1 + q2) + L2*cos(q1)]
[ sin(q1 + q2 + q4), cos(q1 + q2 + q4), 0, L3*sin(q1 + q2) + L2*sin(q1)]
[ 0, 0, 1, L1 - L4 - q3]
[ 0, 0, 0, 1]
P04 =
L3*cos(q1 + q2) + L2*cos(q1)
L3*sin(q1 + q2) + L2*sin(q1)
L1 - L4 - q3
F45 =
Fx
Fy
Fz
M45 =
Mx
My
Mz
r34 =
0
0
-L4
r23 =
0
0
-q3
r12 =
L3
0
0
r01 =
L2
0
L1
r3c4 =
0
0
-L4/2
r2c3 =
0
0
-q3/2
r1c2 =
L3/2
0
0
r0c1 =
L2/2
0
L1/2
F34 =
Fx*cos(q4) - Fy*sin(q4) + g*m4*sin(q1 + q2)
Fy*cos(q4) + Fx*sin(q4) + g*m4*cos(q1 + q2)
Fz
F23 =
Fx*cos(q4) - Fy*sin(q4) + g*m3*sin(q1 + q2) + g*m4*sin(q1 + q2)
Fy*cos(q4) + Fx*sin(q4) + g*m3*cos(q1 + q2) + g*m4*cos(q1 + q2)
Fz
F12 =
Fx*cos(q2 + q4) - Fy*sin(q2 + q4) + g*m2*sin(q1) + g*m3*sin(q1) + g*m4*sin(q1)
Fy*cos(q2 + q4) + Fx*sin(q2 + q4) + g*m2*cos(q1) + g*m3*cos(q1) + g*m4*cos(q1)
Fz
F01 =
Fx*cos(q1 + q2 + q4) - Fy*sin(q1 + q2 + q4)
g*m1 + g*m2 + g*m3 + g*m4 + Fy*cos(q1 + q2 + q4) + Fx*sin(q1 + q2 + q4)
Fz
M34 =
L4*(Fy*cos(q4) + Fx*sin(q4)) + Mx*cos(q4) - My*sin(q4) + (L4*g*m4*cos(q1 + q2))/2
My*cos(q4) - L4*(Fx*cos(q4) - Fy*sin(q4)) + Mx*sin(q4) - (L4*g*m4*sin(q1 + q2))/2
Mz
M23 =
q3*(Fy*cos(q4) + Fx*sin(q4) + g*m4*cos(q1 + q2)) + L4*(Fy*cos(q4) + Fx*sin(q4)) +
Mx*cos(q4) - My*sin(q4) + (L4*g*m4*cos(q1 + q2))/2 + (g*m3*q3*cos(q1 + q2))/2
My*cos(q4) - q3*(Fx*cos(q4) - Fy*sin(q4) + g*m4*sin(q1 + q2)) - L4*(Fx*cos(q4) -
Fy*sin(q4)) + Mx*sin(q4) - (L4*g*m4*sin(q1 + q2))/2 - (g*m3*q3*sin(q1 + q2))/2

Mz
M12 =
Mx*cos(q2 + q4) - My*sin(q2 + q4) + Fy*L4*cos(q2 + q4) + Fx*L4*sin(q2 + q4) +
Fy*q3*cos(q2 + q4) + Fx*q3*sin(q2 + q4) + Fz*L3*sin(q2) + (L4*g*m4*cos(q1))/2 +
(g*m3*q3*cos(q1))/2 + g*m4*q3*cos(q1)
My*cos(q2 + q4) + Mx*sin(q2 + q4) - Fx*L4*cos(q2 + q4) + Fy*L4*sin(q2 + q4) -
Fx*q3*cos(q2 + q4) - Fz*L3*cos(q2) + Fy*q3*sin(q2 + q4) - (L4*g*m4*sin(q1))/2 -
(g*m3*q3*sin(q1))/2 - g*m4*q3*sin(q1)

Mz + Fy*L3*cos(q4) + Fx*L3*sin(q4) + (L3*g*m2*cos(q1 + q2))/2 + L3*g*m3*cos(q1 + q2) +

L3*g*m4*cos(q1 + q2)
M01 =
Mx*cos(q1 + q2 + q4) - My*sin(q1 + q2 + q4) - Fy*L1*cos(q1 + q2 + q4) + Fy*L4*cos(q1 +
q2 + q4) - Fx*L1*sin(q1 + q2 + q4) + Fx*L4*sin(q1 + q2 + q4) + Fy*q3*cos(q1 + q2 + q4) +
Fx*q3*sin(q1 + q2 + q4) - (L1*g*m1)/2 - L1*g*m2 - L1*g*m3 - L1*g*m4 + (L4*g*m4)/2 +
(g*m3*q3)/2 + g*m4*q3 + Fz*L3*sin(q1 + q2) + Fz*L2*sin(q1)

My*cos(q1 + q2 + q4) + Mx*sin(q1 + q2 + q4) + Fx*L1*cos(q1 + q2 + q4) - Fx*L4*cos(q1 +

q2 + q4) - Fy*L1*sin(q1 + q2 + q4) + Fy*L4*sin(q1 + q2 + q4) - Fx*q3*cos(q1 + q2 + q4) +
Fy*q3*sin(q1 + q2 + q4) - Fz*L3*cos(q1 + q2) - Fz*L2*cos(q1)

Mz + Fy*L2*cos(q2 + q4) + Fx*L2*sin(q2 + q4) + Fy*L3*cos(q4) + Fx*L3*sin(q4) +

(L3*g*m2*cos(q1 + q2))/2 + L3*g*m3*cos(q1 + q2) + L3*g*m4*cos(q1 + q2) +
(L2*g*m1*cos(q1))/2 + L2*g*m2*cos(q1) + L2*g*m3*cos(q1) + L2*g*m4*cos(q1)
tau =
Mx*cos(q1 + q2 + q4) - My*sin(q1 + q2 + q4) - Fy*L1*cos(q1 + q2 + q4) + Fy*L4*cos(q1 +
q2 + q4) - Fx*L1*sin(q1 + q2 + q4) + Fx*L4*sin(q1 + q2 + q4) + Fy*q3*cos(q1 + q2 + q4) +
Fx*q3*sin(q1 + q2 + q4) - (L1*g*m1)/2 - L1*g*m2 - L1*g*m3 - L1*g*m4 + (L4*g*m4)/2 +
(g*m3*q3)/2 + g*m4*q3 + Fz*L3*sin(q1 + q2) + Fz*L2*sin(q1)

My*cos(q1 + q2 + q4) + Mx*sin(q1 + q2 + q4) + Fx*L1*cos(q1 + q2 + q4) - Fx*L4*cos(q1 +

q2 + q4) - Fy*L1*sin(q1 + q2 + q4) + Fy*L4*sin(q1 + q2 + q4) - Fx*q3*cos(q1 + q2 + q4) +
Fy*q3*sin(q1 + q2 + q4) - Fz*L3*cos(q1 + q2) - Fz*L2*cos(q1)

Mz + Fy*L2*cos(q2 + q4) + Fx*L2*sin(q2 + q4) + Fy*L3*cos(q4) + Fx*L3*sin(q4) +

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

Mx*cos(q2 + q4) - My*sin(q2 + q4) + Fy*L4*cos(q2 + q4) + Fx*L4*sin(q2 + q4) +

Fy*q3*cos(q2 + q4) + Fx*q3*sin(q2 + q4) + Fz*L3*sin(q2) + (L4*g*m4*cos(q1))/2 +
(g*m3*q3*cos(q1))/2 + g*m4*q3*cos(q1)

My*cos(q2 + q4) + Mx*sin(q2 + q4) - Fx*L4*cos(q2 + q4) + Fy*L4*sin(q2 + q4) -

Fx*q3*cos(q2 + q4) - Fz*L3*cos(q2) + Fy*q3*sin(q2 + q4) - (L4*g*m4*sin(q1))/2 -
(g*m3*q3*sin(q1))/2 - g*m4*q3*sin(q1)

Mz + Fy*L3*cos(q4) + Fx*L3*sin(q4) + (L3*g*m2*cos(q1 + q2))/2 + L3*g*m3*cos(q1 + q2) +

L3*g*m4*cos(q1 + q2)

q3*(Fy*cos(q4) + Fx*sin(q4) + g*m4*cos(q1 + q2)) + L4*(Fy*cos(q4) + Fx*sin(q4)) +

Mx*cos(q4) - My*sin(q4) + (L4*g*m4*cos(q1 + q2))/2 + (g*m3*q3*cos(q1 + q2))/2

My*cos(q4) - q3*(Fx*cos(q4) - Fy*sin(q4) + g*m4*sin(q1 + q2)) - L4*(Fx*cos(q4) -

Fy*sin(q4)) + Mx*sin(q4) - (L4*g*m4*sin(q1 + q2))/2 - (g*m3*q3*sin(q1 + q2))/2

Mz

L4*(Fy*cos(q4) + Fx*sin(q4)) + Mx*cos(q4) - My*sin(q4) + (L4*g*m4*cos(q1 + q2))/2

My*cos(q4) - L4*(Fx*cos(q4) - Fy*sin(q4)) + Mx*sin(q4) - (L4*g*m4*sin(q1 + q2))/2

Mz
clc, clear, close all
% Ejercicio 5b.
disp(' ');
disp('Ejercicio 5b.');
disp(' ');

% DATOS:
L1 = 400/1000 % m
L2 = 400/1000 % m
L3 = 300/1000 % m
L4 = 30/1000 % m
q1 = deg2rad(0) % deg
q2 = deg2rad(30) % deg
q3 = 30/1000 % m
q4 = deg2rad(45) % deg
m1 = 1 % kg
m2 = 1 % kg
m3 = 1 % kg
m4 = 1 % kg
F45 = [10; 12; 4] % N
M45 = [2; 1; 3]/100 % N*m
% 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;
% Posicion:
P04 = T04(1:3,4);
% Matrices de Rotación
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.';

%-------------------------------------------------------------------------
% CÁLCULO DE LA ESTÁTICA
% Gravedad
g = 9.81; % m/s^2.
%g = 0; % Desprecio de la gravedad.
g0 = [0;-g;0];
% Vectores posicion
r34 = [a(4);d(4)*sin(alpha(4));d(4)*cos(alpha(4))]
r23 = [a(3);d(3)*sin(alpha(3));d(3)*cos(alpha(3))]
r12 = [a(2);d(2)*sin(alpha(2));d(2)*cos(alpha(2))]
r01 = [a(1);d(1)*sin(alpha(1));d(1)*cos(alpha(1))]
r3c4 = r34/2 % Centro de masa se encuentra en el punto medio.
r2c3 = r23/2 % Centro de masa se encuentra en el punto medio.
r1c2 = r12/2 % Centro de masa se encuentra en el punto medio.
r0c1 = r01/2 % Centro de masa se encuentra en el punto medio.
%-------------------------------------------------------------------------
% Cálculo de las Fuerzas.
F34 = R34*F45 - m4*R30*g0
F23 = R23*F34 - m3*R20*g0
F12 = R12*F23 - m2*R10*g0
F01 = R01*F12 - m1*eye(3)*g0
%-------------------------------------------------------------------------
% Cálculo de Pares.
M34 = R34*M45-cross(R34*r3c4,m4*R30*g0)+cross(R34*r34,R34*F45)
M23 = R23*M34-cross(R23*r2c3,m3*R20*g0)+cross(R23*r23,R23*F34)
M12 = R12*M23-cross(R12*r1c2,m2*R10*g0)+cross(R12*r12,R12*F23)
M01 = R01*M12-cross(R01*r0c1,m1*eye(3)*g0)+cross(R01*r01,R01*F12)
% Tau final.
tau = [M01;M12;M23;M34]
%-------------------------------------------------------------------------

Ejercicio 5b.

L1 =
0.4000
L2 =
0.4000
L3 =
0.3000
L4 =
0.0300
q1 =
0
q2 =
0.5236
q3 =
0.0300
q4 =
0.7854
m1 =
1
m2 =
1
m3 =
1
m4 =
1
F45 =
10
12
4
M45 =
0.0200
 0.0100
0.0300
MDH =
0 0.4000 0.4000 0
0.5236 0 0.3000 0
0 -0.0300 0 0
0.7854 -0.0300 0 0
r34 =
0
0
-0.0300
r23 =
0
0
-0.0300
r12 =
0.3000
0
0
r01 =
0.4000
0
0.4000
r3c4 =
0
0
-0.0150
r2c3 =
0
0
-0.0150
r1c2 =
0.1500
0
0
r0c1 =
0.2000
0
0.2000
F34 =
3.4908
24.0521
4.0000
F23 =
8.3958
32.5478
4.0000
F12 =
-9.0029
42.1951
4.0000
F01 =
-9.0029
52.0051
4.0000
M34 =
0.6012
-0.0099
 0.0300
M23 =
1.4502
-0.1882
0.0300
M12 =
1.9500
-0.4771
11.0687
M01 =
-16.8900
-5.6783
29.9087
tau =
-16.8900
-5.6783
29.9087
1.9500
-0.4771
11.0687
1.4502
-0.1882
0.0300
0.6012
-0.0099
0.0300

Published with MATLAB® R2018a