% Obtencin de la matriz Cinemtica a partir de los parmetros DH
% Diseado por: Ing. Gabriel Pool
close all;
clear all;
i = 2; % Apuntador del vector, empezar con 2 para obtener 0-1 T
% Matlab no acepta el vector a(0), genera errores de compilacin
syms a alfa d teta L1 L2 L3 t1 t2 t3 pi
a(1) = 0; % Equivalente a a(0)= dist Z0 => Z1 sobre X0
a(2) = L1; % Equivalente a a(1)
a(3) = L2; % Equivalente a a(2)
a(4) = L3; % Equivalente a a(3)
a(5) = 0; % Equivalente a a(4)
alfa(1) = 0; % Equivalente a alfa(0) = Angulo Z0 => Z1 sobre X0
alfa(2) = 0; % Equivalente a alfa(1)
alfa(3) = 0; % Equivalente a alfa(2)
alfa(4) = 0; % Equivalente a alfa(3)
alfa(5) = 0; % Equivalente a alfa(4)
d(2) = 0; % Equivalente a d(1)= dist X0 => X1 sobre Z1
d(3) = 0; % Equivalente a d(2)
d(4) = 0; % Equivalente a d(3)
d(5) = 0; % Equivalente a d(4)
d(6) = 0; % Equivalente a d(5)
teta(2) = t1; % Equivalente a teta(1) = = Angulo X0 => X1 sobre Z1
teta(3) = t2; % Equivalente a teta(2)
teta(4) = t3; % Equivalente a teta(3)
teta(5) = 0; % Equivalente a teta(4)
teta(6) = 0; % Equivalente a teta(5)
T = [sym(cos (teta(i))), sym(-sin (teta(i))), 0, a(i-1);
sym(sin (teta(i))* cos (alfa(i-1))), sym(cos (teta(i)) * cos(alfa(i-1))), sym(-sin (alfa(i-1))), sym(-d(i) * sin (alfa(i-1)));
sym(sin (teta(i)) * sin (alfa(i-1))), sym(cos (teta(i)) * sin(alfa(i-1))), sym(cos (alfa(i-1))), sym(d(i) * cos (alfa(i-1)));
0, 0, 0, 1]
R = T;
i = i + 1;
T = [sym(cos (teta(i))), sym(-sin (teta(i))), 0, a(i-1);
sym(sin (teta(i))* cos (alfa(i-1))), sym(cos (teta(i)) * cos(alfa(i-1))), sym(-sin (alfa(i-1))), sym(-d(i) * sin (alfa(i-1)));
sym(sin (teta(i)) * sin (alfa(i-1))), sym(cos (teta(i)) * sin(alfa(i-1))), sym(cos (alfa(i-1))), sym(d(i) * cos (alfa(i-1)));
0, 0, 0, 1]
S = T;
i = i + 1;
T = [sym(cos (teta(i))), sym(-sin (teta(i))), 0, a(i-1);
sym(sin (teta(i))* cos (alfa(i-1))), sym(cos (teta(i)) * cos(alfa(i-1))), sym(-sin (alfa(i-1))), sym(-d(i) * sin (alfa(i-1)));
sym(sin (teta(i)) * sin (alfa(i-1))), sym(cos (teta(i)) * sin(alfa(i-1))), sym(cos (alfa(i-1))), sym(d(i) * cos (alfa(i-1)));
0, 0, 0, 1]
Q = T;
% W = R * S * Q;
% W = simple (W); % Simplifica a su mnima expresin la matriz W
% display (W); % Muestra el resultado
% En caso de tener menos eslabones comente lo siguiente y descomente las tres lneas anteriores:
i = i + 1;
T = [sym(cos (teta(i))), sym(-sin (teta(i))), 0, a(i-1);
sym(sin (teta(i))* cos (alfa(i-1))), sym(cos (teta(i)) * cos(alfa(i-1))), sym(-sin (alfa(i-1))), sym(-d(i) * sin (alfa(i-1)));
sym(sin (teta(i)) * sin (alfa(i-1))), sym(cos (teta(i)) * sin(alfa(i-1))), sym(cos (alfa(i-1))), sym(d(i) * cos (alfa(i-1)));
0, 0, 0, 1]
W = R * S * Q * T;
W = simple (W); % Simplifica a su mnima expresin la matriz W
display (W); % Muestra el resultado
