function examen

%%En estar tarea vemos el concepto de sistema solidario al eslabon y

%Denavit y Hartenberg
%% Aqui empieza
LoadData
MostrarPiezas
InitHome
%------n=200;
III=zeros(9,1);
%% construimos los paneles
% % Panel de juego
% J_p = uipanel(fig_1,...
%
'units','pixels',...
%
'Position',[20 40 265 150],...
%
'Title','Comandos de tres en raya','FontSize',11,'BackgroundColor','cyan')
;
% Panel de Cinemtica directa
K_p = uipanel(fig_1,...
'units','pixels',...
'Position',[20 40+200 265 150],...
'Title','Cinemtica Directa','FontSize',11,'BackgroundColor','green');
% Panel de Cinemtica inversa
KI_p = uipanel(fig_1,...
'units','pixels',...
'Position',[20 40+400 265 150],...
'Title','Cinemtica Inversa','FontSize',11,'BackgroundColor','green');
flagjugador=1;
LD = 43; %105; % Left, used to set the GUI.
HT = 18; % Height
BT = 80; % Bottom
%% Poner los sliders
d1_slider = uicontrol(K_p,'style','slider',...
'Max',360,'Min',0,'Value',0,...
'SliderStep',[0.01 0.1],...
'callback',@d1_slider_button_press,...
'Position',[LD BT+30 120 HT]);
d2_slider = uicontrol(K_p,'style','slider',...
'Max',1,'Min',0,'Value',0,...
'SliderStep',[0.01 0.1],...
'callback',@d2_slider_button_press,...
'Position',[LD BT+30-HT 120 HT]);
d3_slider = uicontrol(K_p,'style','slider',...
'Max',1,'Min',0,'Value',0,...
'SliderStep',[0.01 0.1],...
'callback',@d3_slider_button_press,...
'Position',[LD BT+30-2*HT 120 HT]);
%% Poner los edits
t1_edit = uicontrol(KI_p,'style','edit','String',0,'Position',[LD BT 50
L, B, W, H
t1_text = uicontrol(KI_p,'style','text','String','X','BackgroundColor',
ition',[LD+11 BT+HT 20 HT]); % L+11, B+H, W, H
t2_edit = uicontrol(KI_p,'style','edit','String',0,'Position',[LD+60 BT
% L, B, W, H
t2_text = uicontrol(KI_p,'style','text','String','Y','BackgroundColor',
ition',[LD+60+11 BT+HT 20 HT]); % L+11, B+H, W, H

HT]); %
'g','Pos
50 HT]);
'g','Pos

t3_edit = uicontrol(KI_p,'style','edit','String',0,'Position',[LD+120 BT 50 HT])

; % L, B, W, H
t3_text = uicontrol(KI_p,'style','text','String','Z','BackgroundColor', 'g','Pos
ition',[LD+120+11 BT+HT 20 HT]); % L+11, B+H, W, H
target = uicontrol(KI_p,'String','Ir','callback',@Ir_button_press,...
'Position',[LD+60 BT-2*HT 50 HT]);
%%
function T = tmat(alpha, a, d, theta)
% tmat(alpha, a, d, theta) (T-Matrix used in Robotics)
% The homogeneous transformation called the "T-MATRIX"
% as used in the Kinematic Equations for robotic type
% systems (or equivalent).
%
% This is equation 3.6 in Craig's "Introduction to Robotics."
% alpha, a, d, theta are the Denavit-Hartenberg parameters.
%
% (NOTE: ALL ANGLES MUST BE IN DEGREES.)
%
alpha = alpha*pi/180;
%Note: alpha is in radians.
theta = theta*pi/180;
%Note: theta is in radians.
ct = cos(theta);
st = sin(theta);
ca = cos(alpha);
sa = sin(alpha);
%T = [c -s 0 a; s*ca c*ca -sa -sa*d; s*sa c*sa ca ca*d; 0 0 0 1];
T = [ct -ca*st sa*st a*ct; st ca*ct -sa*ct a*st;0 sa ca d; 0 0 0 1];
end
function r = rotx(alpha)
alpha = alpha*pi/180;
ca = cos(alpha);
sa = sin(alpha);
r =
[1
0
0
0 ca
-sa
0 sa
ca];
% Trotx =
[rotx(alpha) [0 0 0]'; 0 0 0 1];
end
function r = roty(phi)
phi = phi*pi/180;
cp = cos(phi);
sp = sin(phi);
r =
[cp
0
sp
0 1
0
-sp 0
cp];
% Troty =
[rotx(phi) [0 0 0]'; 0 0 0 1];
end
function r = rotz(theta)
theta = theta*pi/180;
ct = cos(theta);
st = sin(theta);
r =
[ct
-st
0
st ct
0
0 0
1];
% Trotz =
[rotz(theta) [0 0 0]'; 0 0 0 1];
end
function Tp = transl(x, y, z)
t = [x; y; z];
Tp =
[eye(3)
0 0
0
end

t;
1];

%%
function InitHome
set(0,'Units','pixels')
dim = get(0,'ScreenSize');
fig_1 = figure('doublebuffer','on','Position',[0,35,dim(3)-200,dim(4)-11
0],...
'Name',' 3D Cartesian Robot Graphical Demo',...
'NumberTitle','off','CloseRequestFcn',@del_app);
hold on;
%Creamos las partes

XB=getappdata(0,'XB');
YB=getappdata(0,'YB');
ZB=getappdata(0,'ZB');
% La base, vertices y caras
VB1=getappdata(0,'VB1');
VB2=getappdata(0,'VB2');
FB=getappdata(0,'FB');
% Primer eglabon, vrticeg y caras
V1=getappdata(0,'V1');
F1=getappdata(0,'F1');
% Segundo eslabon, vrtices y caras
V2=getappdata(0,'V2');
F2=getappdata(0,'F2');
% Tercer eslabon, vrtices y caras
V3=getappdata(0,'V3');
F3=getappdata(0,'F3');
% Transformacin homogenea
TH=getappdata(0,'TH');
%------------------------------------Link01= (TH*VB1')';
Link02= (TH*VB2')';
% set(hB1,'vertices',Link01(:,1:3));
% set(hB2,'vertices',Link01(:,1:3));
d1ini=0;
d2ini=0;
d3ini=0;
% ALFHA,A,D,TETHA
T_01=tmat(d1ini,0,0,90);
T_12=tmat(-90,0,d2ini,90);
T_23=tmat(0,0,d3ini,0);
T_02 = T_01*T_12;
T_03=T_02*T_23;
Link1 = (T_01*V1')';
Link2 = (T_02*V2')';
Link3 = (T_03*V3')';
Link1H = (TH*Link1')';
Link2H = (TH*Link2')';
Link3H = (TH*Link3')';
[FFB,VVB]=surf2patch(XB,YB,ZB);
n=size(VVB,1);
VVB=[VVB ones(n,1)];
% cameratoolbar
% dibuja vertices y tapas
LB = patch('faces', FFB, 'vertices' ,VVB(:,1:3));
LB1=patch(XB(1,:),YB(1,:),ZB(1,:),'b');
LB2=patch(XB(2,:),YB(2,:),ZB(2,:),'b');
% graficamos la cancha y fichas

L01 = patch('faces', FB, 'vertices' ,Link01(:,1:3));

L02 = patch('faces', FB, 'vertices' ,Link01(:,1:3));
L1 = patch('faces', F1, 'vertices' ,Link1H(:,1:3));
L2 = patch('faces', F2, 'vertices' ,Link2H(:,1:3));
L3 = patch('faces', F3, 'vertices' ,Link3H(:,1:3));
view(-15,64)
light('Position',[0 0 1],'Style','infinite');
xlabel('X'),ylabel('Y'),zlabel('Z');
axis([-1.5 1.5 -1.5 1.5 -.5 5/2])
grid
daspect([1 1 1])
set(LB, 'facec', 'b');
set(L01, 'facec', 'y');
set(L02, 'facec', 'r');
set(L1, 'facec', 'y');
set(L2, 'facec', 'g');
set(L3, 'facec', 'b');
setappdata(0,'dOld',[d1ini d2ini d3ini]);
setappdata(0,'L1',L1);
setappdata(0,'L2',L2);
setappdata(0,'L3',L3);
end
%%
function del_app(varargin)
rmappdata(0,'VB1');
rmappdata(0,'VB2');
rmappdata(0,'FB');
% Primer eslabon, vrtices y caras
rmappdata(0,'V1');
rmappdata(0,'F1');
% Segundo eslabon, vrtices y caras
rmappdata(0,'V2');
rmappdata(0,'F2');
% Tercer eslabon, vrtices y caras
rmappdata(0,'V3');
rmappdata(0,'F3');
% Transformacin homogenea
rmappdata(0,'TH');
delete(fig_1);
end
%%
function LoadData
rB=.3; % radio de la base
lb=.2;%altura de la base
[XB,YB,ZB]=cylinder(rB);
ZB=ZB*lb -lb; % Dsplazamos el cilindro
setappdata(0,'XB',XB);
setappdata(0,'YB',YB);
setappdata(0,'ZB',ZB);
% Creamos los vertices en los sistemas solidarios necesarios
% para la base dos rieles de tamao uno
VB1=[ .05 .05 -.30 1;
-.05 .05 -.30 1;
-.05 -.05 -.30 1;
.05 -.05 -.30 1;

.05 .05 1 1;
-.05 .05 1 1;
-.05 -.05 1 1;
.05 -.05 1 1];
TB=tmat(0,1,0,0);
VB2=(TB*VB1')';
% rotacion y traslacion por z
alpha = 90;
% T=tmat(0,0 ,-.15, 0);
Rotx=tmat(alpha,0,0,0);
VB1=(Rotx*(VB1)')';
FB=[ 1 2 3 4;
5 6 7 8;
1 5 8 4;
2 6 7 3;
1 5 6 2;
4 8 7 3];
V1=[ .05 .05 -.25 1;
-.05 .05 -.25 1;
-.05 -.05 -.25 1;
.05 -.05 -.25 1;
.05 .05
1 1;
-.05 .05
1 1;
-.05 -.05
1 1;
.05 -.05
1 1];
F1=[ 1 2 3 4;
5 6 7 8;
1 5 8 4;
2 6 7 3;
1 5 6 2;
4 8 7 3];
V2=[ .075 .05 .05 1;
.075 -.05 .05 1;
.075 -.05 -.05 1;
.075 .05 -.05 1;
-.075 .05 .05 1;
-.075 -.05 .05 1;
-.075 -.05 -.05 1;
-.075 .05 -.05 1];
F2=F1;
V3=[ .005 .005 0 1;
-.005 .005 0 1;
-.005 -.005 0 1;
.005 -.005 0 1;
.005 .005 -1.3 1;
-.005 .005 -1.3 1;
-.005 -.005 -1.3 1;
.005 -.005 -1.3 1];
F3=F2;
%% Creacmos las fichas, en coordenadas de ficha
VF=[ 0 0 0 1;
1/3 0 0 1;
1/3 0 1/3 1;
0 0 1/3 1;
0 1/3 0 1;
1/3 1/3 0 1;
1/3 1/3 1/3 1;
0 1/3 1/3 1];

%Creamos las partes

% creamos la base
setappdata(0,'VB1',VB1);
setappdata(0,'VB2',VB2);
setappdata(0,'FB',FB);
% Primer eslabon, vrtices y caras
setappdata(0,'V1',V1);
setappdata(0,'F1',F1);
% Segundo eslabon, vrtices y caras
setappdata(0,'V2',V2);
setappdata(0,'F2',F2);
% Tercer eslabon, vrtices y caras
setappdata(0,'V3',V3);
setappdata(0,'F3',F3);
% Calculamos la tranformacin homogenea
alpha=90;
phi=90;
p=[0 1 0]';
Trotx =
[rotx(alpha) [0 0 0]'; 0 0 0 1];
Troty =
[roty(phi) [0 0 0]'; 0 0 0 1];
Tp=[eye(3) p; 0 0 0 1];
TH=Trotx*Troty*Tp;
setappdata(0,'TH',TH);
end
%
function MostrarPiezas
set(0,'Units','pixels')
dim = get(0,'ScreenSize');
fig_P = figure('Position',[0,35,dim(3)-200,dim(4)-110],...
'Name',' Piezas del Robot Polar',...
'NumberTitle','off');
hold on;
% Recuperemos los datos geomtricos
XB=getappdata(0,'XB');
YB=getappdata(0,'YB');
ZB=getappdata(0,'ZB');
% Recuperemos los datos geomtricos
VB1=getappdata(0,'VB1');
FB=getappdata(0,'FB');
%-------------------V1=getappdata(0,'V1');
F1=getappdata(0,'F1');
%-------------------V2=getappdata(0,'V2');
F2=getappdata(0,'F2');
V3=getappdata(0,'V3');
F3=getappdata(0,'F3');
%---------------------subplot(2,2,1)
subplot(2,2,1)
surf(XB,YB,ZB);
patch(XB(1,:),YB(1,:),ZB(1,:),'b');
patch(XB(2,:),YB(2,:),ZB(2,:),'b');
patch('faces',FB, 'vertices' ,VB1(:,1:3),'FaceColor', 'r');
X=[0;1];
Y=[0;0];
Z=[0;0];
line(X,Y,Z,'Color','r');

X=[0;0];
Y=[0;1];
Z=[0;0];
line(X,Y,Z,'Color','g');
X=[0;0];
Y=[0;0];
Z=[0;1];
line(X,Y,Z,'Color','b');
xlabel('X'),ylabel('Y'),zlabel('Z');
axis([-3 3 -3 3 -3 3])
grid on
subplot(2,2,2)
patch('faces',F1, 'vertices' ,V1(:,1:3),'FaceColor', 'y');
view(3)
X=[0;1];
Y=[0;0];
Z=[0;0];
line(X,Y,Z,'Color','r');
X=[0;0];
Y=[0;1];
Z=[0;0];
line(X,Y,Z,'Color','g');
X=[0;0];
Y=[0;0];
Z=[0;1];
line(X,Y,Z,'Color','b');
xlabel('X'),ylabel('Y'),zlabel('Z');
axis([-3 3 -3 3 -3 3])
grid on
subplot(2,2,3)
patch('faces',F2, 'vertices' ,V2(:,1:3),'FaceColor', 'g');
view(3)
X=[0;1];
Y=[0;0];
Z=[0;0];
line(X,Y,Z,'Color','r');
X=[0;0];
Y=[0;1];
Z=[0;0];
line(X,Y,Z,'Color','g');
X=[0;0];
Y=[0;0];
Z=[0;1];
line(X,Y,Z,'Color','b');
xlabel('X'),ylabel('Y'),zlabel('Z');
axis([-3 3 -3 3 -3 3])
grid on
subplot(2,2,4)
patch('faces',F3, 'vertices' ,V3(:,1:3),'FaceColor', 'b');
view(3)
X=[0;1];
Y=[0;0];
Z=[0;0];
line(X,Y,Z,'Color','r');
X=[0;0];
Y=[0;1];
Z=[0;0];

line(X,Y,Z,'Color','g');
X=[0;0];
Y=[0;0];
Z=[0;1];
line(X,Y,Z,'Color','b');
xlabel('X'),ylabel('Y'),zlabel('Z');
axis([-3 3 -3 3 -3 3])
grid on
end% MostrarPiezas
%%
function CartAni(d1fin,d2fin,d3fin,n)
dOld=getappdata(0,'dOld');
d1ini=dOld(1);
d2ini=dOld(2);
d3ini=dOld(3);
% % por el primer eje
V1=getappdata(0,'V1');
% Segundo eslabon, vrtices y caras
V2=getappdata(0,'V2');
% Tercer eslabon, vrtices y caras
V3=getappdata(0,'V3');
% el TH
TH=getappdata(0,'TH');
% recuperar los plots
L1=getappdata(0,'L1');
L2=getappdata(0,'L2');
L3=getappdata(0,'L3');
II=linspace(d1ini,d1fin,n);
for ii=II,
T_01=tmat(ii,0,0,90);
T_12=tmat(-90,0,d2ini,90);
T_23=tmat(0,0,d3ini,0);
T_02 = T_01*T_12;
T_03=T_02*T_23;
Link1 = (T_01*V1')';
Link2 = (T_02*V2')';
Link3 = (T_03*V3')';
Link1H = (TH*Link1')';
Link2H = (TH*Link2')';
Link3H = (TH*Link3')';
set(L1, 'Vertices', Link1H(:,1:3));
set(L2, 'Vertices', Link2H(:,1:3));
set(L3, 'Vertices', Link3H(:,1:3));
drawnow
end
% por el segundo eje
JJ=linspace(d2ini,d2fin,n);
for jj=JJ,
T_12=tmat(-90,0,jj,90);
T_02 = T_01*T_12;
T_03=T_02*T_23;
Link2 = (T_02*V2')';
Link3 = (T_03*V3')';
Link2H = (TH*Link2')';
Link3H = (TH*Link3')';
set(L2, 'Vertices', Link2H(:,1:3));

set(L3, 'Vertices', Link3H(:,1:3));

drawnow
end
KK=linspace(d3ini,d3fin,n);
for kk=KK,
T_23=tmat(0,0,kk,0);
T_03=T_02*T_23;
Link3 = (T_03*V3')';
Link3H = (TH*Link3')';
set(L3, 'Vertices', Link3H(:,1:3));
drawnow
end
setappdata(0,'dOld',[d1fin d2fin d3fin]);
end
%%
function CartPos(nf,n)
dOld=getappdata(0,'dOld');
CC=getappdata(0,'CC');
d1ini=dOld(1);
d2ini=dOld(2);
d3ini=dOld(3);
d3fin=0.05;
d1fin=CC(nf,1);
d2fin=CC(nf,2);
% % por el primer eje
V1=getappdata(0,'V1');
% Segundo eslabon, vrtices y caras
V2=getappdata(0,'V2');
% Tercer eslabon, vrtices y caras
V3=getappdata(0,'V3');
% el TH
TH=getappdata(0,'TH');
% recuperar los plots
L1=getappdata(0,'L1');
L2=getappdata(0,'L2');
L3=getappdata(0,'L3');
FB=getappdata(0,'FB');
FR=getappdata(0,'FR');
% Levantar el cubo
KK=linspace(d3ini,d3fin,n);
deltaz=KK(2)-KK(1);
T_01=tmat(90,0,d1ini,90);
T_12=tmat(-90,0,d2ini,90);
T_02 = T_01*T_12;
for kk=KK,
T_23=tmat(0,0,kk,0);
T_03=T_02*T_23;
Link3 = (T_03*V3')';
Link3H = (TH*Link3')';
set(L3, 'Vertices', Link3H(:,1:3));
drawnow
end
% retroceder el cubo por x
II=linspace(d1ini,d1fin,n);
for ii=II,

T_01=tmat(90,0,ii,90);
T_23=tmat(0,0,d3fin,0);
T_02 = T_01*T_12;
T_03=T_02*T_23;
Link1 = (T_01*V1')';
Link2 = (T_02*V2')';
Link3 = (T_03*V3')';
Link1H = (TH*Link1')';
Link2H = (TH*Link2')';
Link3H = (TH*Link3')';
set(L1, 'Vertices', Link1H(:,1:3));
set(L2, 'Vertices', Link2H(:,1:3));
set(L3, 'Vertices', Link3H(:,1:3));
drawnow
end
JJ=linspace(d2ini,d2fin,n);
for jj=JJ,
T_12=tmat(-90,0,jj,90);
T_02 = T_01*T_12;
T_03=T_02*T_23;
Link2 = (T_02*V2')';
Link3 = (T_03*V3')';
Link2H = (TH*Link2')';
Link3H = (TH*Link3')';
set(L2, 'Vertices', Link2H(:,1:3));
set(L3, 'Vertices', Link3H(:,1:3));
drawnow
end
end
%%
function d1_slider_button_press(h,dummy)
dOld=getappdata(0,'dOld');
d2fin=dOld(2);d3fin=dOld(3);
d1fin = get(h,'Value'); % rescatamos que valor tiene el slider
%
set(t5_edit,'string',slider_value);
CartAni(d1fin,d2fin,d3fin,n/10)
setappdata(0,'dOld',[d1fin d2fin d3fin]);
end
function d2_slider_button_press(h,dummy)
dOld=getappdata(0,'dOld');
d1fin=dOld(1);d3fin=dOld(3);
d2fin = get(h,'Value'); % rescatamos que valor tiene el slider
%
set(t5_edit,'string',slider_value);
CartAni(d1fin,d2fin,d3fin,n/10)
setappdata(0,'dOld',[d1fin d2fin d3fin]);
end
function d3_slider_button_press(h,dummy)
dOld=getappdata(0,'dOld');
d1fin=dOld(1);d2fin=dOld(2);
d3fin = get(h,'Value'); % rescatamos que valor tiene el slider
%
set(t5_edit,'string',slider_value);
CartAni(d1fin,d2fin,d3fin,n/10)
setappdata(0,'dOld',[d1fin d2fin d3fin]);
end
function Ir_button_press(h,dummy)
%
dOld=getappdata(0,'dOld');
%
d1ini=dOld(1);d2ini=dOld(2); d3ini=dOld(3);
d1fin=str2num(get(t1_edit,'String'));%x
d2fin=str2num(get(t2_edit,'String'));%y

d3fin=1-str2num(get(t3_edit,'String'));%1-z
CartAni(d1fin,d2fin,d3fin,n/2)
setappdata(0,'dOld',[d1fin d2fin d3fin]);
end
end