clc;

clear all;
close all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%Eslabon cero a eslabon 1
ANG1=input('Ingrese el angulo Theta1:')
theta1=(180+ANG1);
d1=400;
a1=-180;
alp1=-90;
A1=[cosd(theta1)
-cosd(alp1)*sin(theta1) sind(alp1)*sind(theta1)
a1*cosd(theta1);...
;sind(theta1)
cosd(alp1)*cos(theta1) -sind(alp1)*cosd(theta1)
a1*sind(theta1);...
;0
sind(alp1)
cosd(alp1)
d1
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 0 A 1')
Ai1=A1
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%Eslabon 1 a Eslabon 2
ANG2=input('Ingrese el angulo Theta2:')
theta2=(90+ANG2);
d2=0;
a2=-600;
alp2=0;
A2=[cosd(theta2)
-cosd(alp2)*sind(theta2) sind(alp2)*sind(theta2)
a2*cosd(theta2);...
;sind(theta2)
cosd(alp2)*cosd(theta2) -sind(alp2)*cosd(theta2)
a2*sind(theta2);...
;0
sind(alp2)
cosd(alp2)
d2
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 1 A 2')
Ai2=A2
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%
%Eslabon 2 a Eslabon 3
ANG3=input('Ingrese el angulo Theta3:')
theta3=(ANG3);
d3=0;
a3=-170;
alp3=-90;
A3=[cosd(theta3)
-cosd(alp3)*sind(theta3) sind(alp3)*sind(theta3)
a3*cosd(theta3);...
;sind(theta3)
cosd(alp3)*cosd(theta3) -sind(alp3)*cosd(theta3)
a3*sind(theta3);...
;0
sind(alp3)
cosd(alp3)
d3
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 2 A 3')

Ai3=A3
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%
%Eslabon 3 a Eslabon 4
ANG4=input('Ingrese el angulo Theta4:')
theta4=(ANG4);
d4=620;
a4=0;
alp4=-90;
A4=[cosd(theta4)
-cosd(alp4)*sind(theta4) sind(alp4)*sind(theta4)
a4*cosd(theta4);...
;sind(theta4)
cosd(alp4)*cosd(theta4) -sind(alp4)*cosd(theta4)
a4*sind(theta4);...
;0
sind(alp4)
cosd(alp4)
d4
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 3 A 4')
Ai4=A4
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%
%Eslabon 4 a Eslabon 5
ANG5=input('Ingrese el angulo Theta5:')
theta5=(ANG5);
d5=0;
a5=0;
alp5=-90;
A5=[cosd(theta5)
-cosd(alp5)*sind(theta5) sind(alp5)*sind(theta5)
a5*cosd(theta5);...
;sind(theta5)
cosd(alp5)*cosd(theta5) -sind(alp5)*cosd(theta5)
a5*sind(theta5);...
;0
sind(alp5)
cosd(alp5)
d5
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 4 A 5')
Ai5=A5
% % %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%
%Eslabon 5 a Eslabon 6
ANG6=input('Ingrese el angulo Theta6:')
theta6=(ANG6);
d6=-200;
a6=0;
alp6=0;
A6=[cosd(theta6)
-cosd(alp6)*sind(theta6) sind(alp6)*sind(theta6)
a6*cosd(theta6);...
;sind(theta6)
cosd(alp6)*cosd(theta6) -sind(alp6)*cosd(theta6)
a6*sind(theta6);...
;0
sind(alp6)
cosd(alp6)
d6
;...
;0
0
0
1]
;
disp('MATRIZ DE TRANSFORMACION HOMOGENEA DE ESLABON 4 A 5')
Ai6=A6

disp('MULTIPLCACION DE TODAS LAS MATRICES')

AT=A1*A2*A3*A4*A5*A6