clear all

clc
syms q1 q2 q3 q4 q5 q6 l1 l2 l3 l4 d2 d3;

%theta d a alpha
dh=[ q1 l1 0 -pi/2 ;
q2 0 l2 0;
q3 0 0 pi/2;
q4 l3 0 -pi/2
q5 0 0 pi/2
q6 l4 0 0 ];
% Ahora se realizar? el ingreso de la Matriz TH
for j=1:6
theta_i=dh(j,1);
d_i =dh(j,2);
a_i =dh(j,3);
alpha_i=dh(j,4);

A(:,:,j)=[cos(theta_i) -cos(alpha_i)*sin(theta_i) sin(alpha_i)*sin(theta_i)

a_i*cos(theta_i);
sin(theta_i) cos(alpha_i)*cos(theta_i) -sin(alpha_i)*cos(theta_i)
a_i*sin(theta_i);
0 sin(alpha_i) cos(alpha_i)
d_i ;
0 0 0
1 ] ;
end
A(:,:,1);
A(:,:,2);
A(:,:,3);
A(:,:,4);
A(:,:,5);
A(:,:,6);
T=A(:,:,1)*A(:,:,2)*A(:,:,3)*A(:,:,4)*A(:,:,5)*A(:,:,6);
simplify(T)