Está en la página 1de 42

Instituto Tecnolgico de la Laguna

Departamento Metal-Mecnica
Robtica Aplicada I

Trabajo Final

Emplazamiento ptimo de un manipulador industrial 6R


(Staubli 170L) bajo el criterio de la manipulabilidad
traslacional

Nombre: Jess Javier Prado Vega

Nmero de Control: 11130632

Facilitador: Ing. J. Alfonso Pmanes Garca


Introduccin: Dentro de este trabajo se podrn observar
imgenes de una animacin hecha en el software Matlab, as
como el cdigo del programa para poder hacer esto posible;
sta animacin consiste en trazar una trayectoria que
describa un movimiento definido deseado, dicha trayectoria
ser descrita por la mueca del rgano terminal del robot
manipulador Staubli modelo 170L, este programa tambin
tiene la finalidad de obtener la manipulabilidad ptima del
robot, es decir la mejor postura que puede llegar a tener el
robot y en la cual pueda trabajar de la mejor manera posible.

Entre los robots considerados de ms utilidad en la actualidad


se encuentran los robots industriales o manipuladores. Existen
ciertas dificultades a la hora de establecer una definicin
formal de lo que es un robot industrial. La primera de ellas
surge de la diferencia conceptual entre el mercado japons y
el euro-americano de lo que es un robot y lo que es un
manipulador. As, mientras que para los japoneses un robot
industrial es cualquier dispositivo mecnico dotado de
articulaciones mviles destinado a la manipulacin, el
mercado occidental es ms restrictivo, exigiendo una mayor
complejidad, sobre todo en lo relativo al control. En segundo
lugar, y centrndose ya en el concepto occidental, aunque
existe una idea comn acerca de lo que es un robot industrial,
no es fcil ponerse de acuerdo a la hora de determinar una
definicin formal. Adems, la evolucin de la robtica ha ido
obligando a diferentes actualizaciones de su definicin.

La definicin ms comnmente aceptada posiblemente sea la


de la Asociacin de Industrias de Robtica (RIA, Robotic
Industry Association), segn la cual:

"Un robot industrial es un manipulador multifuncional


reprogramable, capaz de mover materias, piezas,
herramientas, o dispositivos especiales, segn trayectorias
variables, programadas para realizar tareas diversas"

Esta definicin, ligeramente modificada, ha sido adoptada por


la Organizacin Internacional de Estndares (ISO) que define
al robot industrial como:

"Manipulador multifuncional reprogramable con varios grados


de libertad, capaz de manipular materias, piezas,
herramientas o dispositivos especiales segn trayectorias
variables programadas para realizar tareas diversas"

Comn en todas las definiciones anteriores es la aceptacin


del robot industrial como un brazo mecnico con capacidad de
manipulacin y que incorpora un control ms o menos
complejo. Un sistema robotizado, en cambio, es un concepto
ms amplio. Engloba todos aquellos dispositivos que realizan
tareas de forma automtica en sustitucin de un ser humano
y que pueden incorporar o no a uno o varios robots, siendo
esto ltimo lo ms frecuente.

El uso y desarrollo de los robots son cada vez mayores y esto


se debe a las ventajas que ofrecen. Para que un robot realice
las actividades que deseamos, es necesario conocer su
funcionamiento que est regido por sus caractersticas fsicas
(arquitectura, configuracin, grados de libertad, tipo de
control, etc.). Al conocer estas caractersticas se puede
proseguir a realizar un estudio de su manipulabilidad,
desplazamientos y rotaciones de sus ejes.

Cuando un diseador se dispone a estudiar la cuestin


cinemtica de un robot es esencial que analice la
manipulabilidad del mismo, la cual es uno de los ms
importantes parmetros de funcionalidad de un robot; de
hecho, este concepto tiene gran repercusin en el diseo
puesto que gracias a l se pueden definir varios tipos de
ndices de comportamiento cinemtico que permiten
optimizar las dimensiones del robot.

Si pensamos en manipulabilidad, el nombre que nos viene a la


mente es el del experto en robtica Tsuneo Yoshikawa. El
trabajo de Yoshikawa establece las bases sobre la
manipulabilidad y todava sigue siendo un referente clsico
para diversos trabajos sobre ndices de manipulabilidad en
robots.

Es cierto que el estudio de la manipulabilidad ayuda a


optimizar el diseo de un sistema robtico pero su obtencin
puede llegar a ser compleja. Esto es as puesto que para su
clculo es necesario disponer de la matriz Jacobiana en su
forma analtica.

Para un robot serial (como son los antropomorficos) no


supone un gran problema puesto que aplicando teora
clsica es posible calcular un ndice de manipulabilidad. Sin
embargo, para otro tipo de robots, como son los robots
paralelos, es difcil obtener una solucin analtica y
deben resolverse empleando soluciones numricas.

En el siguiente trabajo se resume los procedimientos


necesarios para lograr que el emplazamiento de la tarea de
un robot manipulador de seis grados de libertad considerando
que la manipulabilidad se lleve a su mejor rendimiento.

Planteamiento del problema: Determinar el


emplazamiento de la tarea del manipulador (Staubli 170L)
mostrada en la figura 2 que optimice manipulabilidad
traslacional del manipulador.
Zp Marco de la tarea
Z0, Ze Yp
Y0, Ye
Xp
Zb
Xb
Yb
X0, Xe
La orientacin de la herramienta es constante en toda
la tarea.
Xb=?
Yb=? Para trsl
ptima
Zb=?

Cada segmento rectilneo se recorre con un


movimiento cicloidal de 5 seg.

Figura 2Zp

Yp

Op
Xp Q2
Q1

Q3 Yh
Zh
Xh
Oh
Q1Q2 = Q2Q3 = Q3Q1 = 40 cm
Trayectoria triangular en el plano Xp - Yp
=60
A continuacin se muestran las matrices de
emplazamiento, del marco de la herramienta y del
marco de la tarea.

e
p R
Xb
0 0
Yb
Zb

0
e
pT =
[ ]

[ ]
1 0 0 0
e 0 1 0 0
0T =
0 0 1 0
0 0 0 1
[ ]
1 0 0 Xo h
p 0 0 1 Yo h
h T d=
0 1 0 Zo h
0 0 0 1

[ ]
1 0 0 0
6 0 1 0 0
hT =
0 0 1 dhh
0 0 0 1

p
h
[
1 0 0
R= 0 0 1
0 1 0 ]
e
p
[ ]
1 0 0
R= 0 1 0
0 0 1

Desarrollo de la resolucin: Utilizando los datos


mencionados en el planteamiento del problema y con ayuda
de algunos programas que ya se tenan previamente, se
procedi a realizar este nuevo programa para as poder
realizar la trayectoria deseada de la mueca del manipulador.

Resultados:
Vista isomtrica
Vista plano X-Y
Vista plano X-Z

Vista plano Y-Z


Grficas de las manipulabilidades
Conclusin: De acuerdo con los resultados obtenidos, y con
las nuevas especificaciones que se daban en esta tarea, se
tena que lograr la trayectoria deseada de la mueca del
manipulador Staubli, al cabo de varios intentos de estar
cambiando ciertas cosas del cdigo del programa, finalmente
se pudo obtener el resultado esperado, que estbamos
deseando.

Apndice: Cdigo del programa.


Programa fnemptrian
function fk = fnemptrian(x)
clear
clc
%Parmetros del robot
%Longitud de cada eslabn
d2 = 100;
d3 = 850;
d4 = 0;
r4 = 1050;
dhh = 400;
%Nmeros de puntos de graficacin
np = 50;

%Caracteristicas de la trayectoria
T=15;
T1=T/3;
T2=T/3;
T3=T/3;

%Arreglos para variables articulares (ngulos)


th1g = zeros([1,np]);
th2g = zeros([1,np]);
th3g = zeros([1,np]);
th4g = zeros([1,np]);
th5g = zeros([1,np]);
th6g = zeros([1,np]);

% MtrH= zeros(1,np);
% MarH= zeros(1,np);

%Arreglo de la Base
sp1 = [ 0 0 0 1]';
sp2 = [ 0 0 150 1]';
p1b = [-150 150 0 1]';
p2b = [ 150 150 0 1]';
p3b = [-150 -150 0 1]';
p4b = [ 150 -150 0 1]';

%Especificacion de la posicion del vertice


%de la placa base c.r. al marco 0:
ss1 = [0 0 -400 1]';

%Especificacion de las posiciones de los vertices


%de la placa base c.r. al marco 0:
ss2=[ 200 200 -400 1]';
ss3=[-200 200 -400 1]';
ss4=[ 200 -200 -400 1]';
ss5=[-200 -200 -400 1]';

p0 =[ 0 0 200 1]';
p0p=[ 0 0 400 1]';
p1 =[ 100 50 200 1]';
p2 =[ 100 50 400 1]';
p3 =[ 100 -50 200 1]';
p4 =[ 100 -50 400 1]';
p5 =[-100 50 200 1]';
p6 =[-100 50 400 1]';
p7 =[-100 -50 200 1]';
p8 =[-100 -50 400 1]';

ss6 = [300 0 0 1]';


ss7 = [0 300 0 1]';
ss8 = [0 0 300 1]';

ee6x = [300 0 0 1]';


ee6y = [0 300 0 1]';
ee6z = [0 0 300 1]';

%Especificacion del emplazamiento de la base del robot


%Base del robot (marco 0) c.r. al marco de la estacion
%de trabajo (marco E):
xaa=-600;%1000;
yaa=-680;%-500;
zaa=-280; %200;

%Especificacion del emplazamiento de la base del robot


%Base del robot (marco P) c.r. al marco de la estacion
%de trabajo (marco E):
xbb= 500;%1000;
ybb= 500;%750;
zbb= 500;%1000;

pnx1= zeros(1,np);
pny1= zeros(1,np);
pnz1= zeros(1,np);

pnx2= zeros(1,np);
pny2= zeros(1,np);
pnz2= zeros(1,np);

Wtrsl= zeros(1,np);
Wrtsl= zeros(1,np);
time= zeros(1,np);

%Matriz de emplazamiento
Te0 = [1 0 0 xaa;
0 1 0 yaa;
0 0 1 zaa;
0 0 0 1];

T0e = (Te0)^(-1);

%Ubicacin del marco de la tarea (Matriz de e a p)


Tep = [1 0 0 xbb;
0 1 0 ybb;
0 0 1 zbb;
0 0 0 1];

T0p = T0e*Tep;

%Matriz rotacional
R0p = [T0p(1,1) T0p(1,2) T0p(1,3);
T0p(2,1) T0p(2,2) T0p(2,3);
T0p(3,1) T0p(3,2) T0p(3,3)];

%Matriz del marco de la herramienta


T6h = [1 0 0 0 ;
0 1 0 0 ;
0 0 1 dhh;
0 0 0 1 ];
Th6 = (T6h)^(-1);

e1 = 1;
e2 = 1;
e3 = -1;
e5 = -1;

iii=1;

% %Especificacion de la parametros de la herramienta


% T= 5;
% xinih= 0;
% delx= 0;
% yinih= 150;
% dely= 0;
% zinih= 0;
% delz= 1500;
%
% %Especificacion de la parametros de orientacion
% alfaini = deg2rad( -90);
% deltaalfa = deg2rad( 0);
% betaini = deg2rad( 180);
% deltabeta = deg2rad(-180);
% gammaini = 0;
% deltagamma= 0;

phi=deg2rad(60);

%Graficacin de la ruta
for i = 0 : np

%Valor del tiempo para cada iteracin


t = T*(i/np);
time(i+1) = t;
%Variables articulares (t/T-1/2)*sin?(2t/T)
%ft = (t/T)-(1/(2*pi))*(sin((2*pi*t)/T));

%Ruta
if t<=T1
ft = (t/T1)-((1/(2*pi))*(sin(2*pi*t/T1)));

Rphd=[-1 0 0;0 0 1;0 1 0];

% alfa = alfaini + deltaalfa*(ft);


% beta = betaini + deltabeta*(ft);
% gamma= gammaini + deltagamma*(ft);
%
% calf=cos(alfa);
% salf=sin(alfa);
% cbet=cos(beta);
% sbet=sin(beta);
% cgam=cos(gamma);
% sgam=sin(gamma);
%
%
% Rphd=[calf*cbet calf*sbet*sgam-salf*cgam calf*sbet*cgam+salf*sgam;
% salf*cbet salf*sbet*sgam+calf*cgam salf*sbet*cgam-calf*sgam;
% -sbet cbet*sgam cbet*cgam];

%Posicin de la matriz de p a h
xph=-400*cos(phi)*ft;
yph= 400*cos(phi)*ft;
zph= 0;

xp1=xph;
yp1=yph;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];

% %Matriz deseada del marco 6


% U0 = T0e*Tep*TPhd*Th6;

%Clculo del modelo inverso de posicin


%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

if t>T1 && t<=(T1+T2)


ft = (((t-T1)/T2)-((1/(2*pi))*(sin(2*pi*(t-T1)/T2))));

Rphd=[-1 0 0;0 0 1;0 1 0];

%Posicin de la matriz de p a h
xph= xp1 + 400*ft;
yph= yp1;
zph= 0;

xp2=xph;
yp2=yph;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];

%Clculo del modelo inverso de posicin


%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

if t>(T1+T2) && t<=(T1+T2+T3)


ft = (((t-T1-T2)/T3)-((1/(2*pi))*(sin(2*pi*(t-T1-T2)/T3))));

Rphd=[-1 0 0;0 0 1;0 1 0];

%Posicin de la matriz de p a h
xph= xp2 - 400*cos(phi)*ft;
yph= yp2 - 400*cos(phi)*ft;
zph= 0;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];
%Clculo del modelo inverso de posicin
%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

%Clculo de cada th
%Theta1:
th1 = atan2(e1*PY,e1*PX);

s1 = sin(th1);
c1 = cos(th1);

%Theta2:
B2=c1*PX+s1*PY-d2;
X=-2*PZ*d3;
Y=-2*B2*d3;
Z=r4*r4-d3*d3-B2*B2-PZ*PZ;
D=X*X+Y*Y;
W=sqrt(D-Z*Z);
C2=(Y*Z-e2*X*W)/D;
S2=(X*Z+e2*Y*W)/D;
th2=atan2(S2,C2);

S3=-(PZ*S2+B2*C2-d3)/r4;
C3=-(B2*S2-PZ*C2)/r4;
th3=atan2(S3,C3);
c23=cos(th2+th3);
s23=sin(th2+th3);

if e5==+1
th5=acos(s23*c1*AX+s23*s1*AY-c23*AZ);
else
th5=-(acos(s23*c1*AX+s23*s1*AY-c23*AZ));
end

s5=sin(th5);
c5=cos(th5);
B2=s1*AX-c1*AY;
B4=c23*c1*AX+c23*s1*AY+s23*AZ;
S4=-B2/s5;
C4=+B4/s5;

th4=atan2(S4,C4);

F2=-s23*c1*NX-s23*s1*NY+c23*NZ;
F4=-s23*c1*SX-s23*s1*SY+c23*SZ;
S6=-F2/s5;
C6=F4/s5;

th6=atan2(S6,C6);

th12 = th1+th2;
th14 = th1+th4;
th123 = th1+th2+th3;
th1234 = th123+th4;
th12345 = th1234+th5;
th123456 = th12345+th6;
th235 = th2+th3+th5;

c1 = cos(th1);
s1 = sin(th1);
c2 = cos(th2);
s2 = sin(th2);
c3 = cos(th3);
s3 = sin(th3);
c4 = cos(th4);
s4 = sin(th4);
c5 = cos(th5);
s5 = sin(th5);
c6 = cos(th6);
s6 = sin(th6);
s12 = sin(th12);
c12 = cos(th12);
s14 = sin(th14);
c14 = cos(th14);
s23 = sin(th2+th3);
c23 = cos(th2+th3);
s235 = sin(th235);
c235 = cos(th235);
s123=sin(th123);
c123=cos(th123);
s1234=sin(th1234);
c1234=sin(th1234);
s12345=sin(th12345);
c12345=sin(th12345);
s123456=sin(th123456);
c123456=sin(th123456);
%Obtencin del modelo directo de posicin (Jacobiana),
%por medio de la derivada parcial con respecto a los ngulos (theta1,
theta2, theta3, theta4, theta5, theta6)

J11=+r4*s1*s23-d2*s1-d3*c2*s1;
J21=-r4*c1*s23+d2*c1+d3*c1*c2;
J31=0;
J41=0;
J51=0;
J61=1;

J12=-r4*c1*c2*c3+r4*c1*s2*s3-d3*c1*s2;
J22=-r4*s1*c2*c3+r4*s1*s2*s3;
J32=-r4*s2*c3-r4*c2*s3+d3*c2;
J42=s1;
J52=-c1;
J62=0;

J13=-r4*c1*c23;
J23= -r4*c23*s1;
J33=-r4*s23;
J43=s1;
J53=-c1;
J63=0;

J14=0;
J24=0;
J34=0;
J44=-c1*s23;
J54=-s1*s23;
J64=c23;

J15=0;
J25=0;
J35=0;
J45=s4*c1*c23+s1*c4;
J55=s4*s1*c23-c1*c4;
J65=s23*s4;

J16=0;
J26=0;
J36=0;
J46=c1*c23*c4*s5-s1*s4*s5+c1*s23*c5;
J56=s1*c23*c4*s5+c1*s4*s5+s1*s23*c5;
J66=s23*c4*s5-c23*c5;

J =[J11 J12 J13 J14 J15 J16;


J21 J22 J23 J24 J25 J26;
J31 J32 J33 J34 J35 J36;
J41 J42 J43 J44 J45 J46;
J51 J52 J53 J54 J55 J56;
J61 J62 J63 J64 J65 J66];

%Clculo de la manipulabilidad rotacional y traslacional


JTA = [J11 J12 J13;
J21 J22 J23;
J31 J32 J33];

JRW = [J44 J45 J46;


J54 J55 J56;
J64 J65 J66];

%fnor=1.25e+009;
fnor=1.5e+09;

Wtrsl(i+1)= sqrt(det(JTA*JTA'))/fnor;

Wrtsl(i+1) = sqrt(det(JRW*JRW'));

npnn=np;

if Z*Z > D
Wtrsl(i+1)=0;
Wrtsl(i+1)=0;
end
%
% Wglob(i+1) = Wa(i+1);
% Wglob(np+i+1) = Ww(i+1);

%Matrices elementales

T01 = [c1 -s1 0 0 ;


s1 c1 0 0 ;
0 0 1 0 ;
0 0 0 1];

T12 = [c2 -s2 0 d2;


0 0 -1 0 ;
s2 c2 0 0 ;
0 0 0 1];

%T02 = T01*T12;

T23 = [c3 -s3 0 d3;


s3 c3 0 0 ;
0 0 1 0 ;
0 0 0 1];

T34 = [c4 -s4 0 d4 ;


0 0 -1 -r4;
s4 c4 0 0 ;
0 0 0 1];

%matrices de transformacion homogeneas


T02=[c1*c2 -c1*s2 s1 d2*c1;
c2*s1 -s1*s2 -c1 d2*s1;
s2 c2 0 0;
0 0 0 1];
T03=[c1*c23 -c1*s23 s1 d2*c1+d3*c1*c2;
c23*s1 -s1*s23 -c1 d2*s1+d3*c2*s1;
s23 c23 0 d3*s2;
0 0 0 1];

vector=[d4 0 0 1]';

T04=T03*vector;

T05=[(c1*c23*c4-s1*s4)*c5-c1*s23*s5 -(c1*c23*c4-s1*s4)*s5-c1*s23*c5
-(-c1*c23*s4-s1*c4) -c1*s23*r4+c1*(d2+d3*c2);
(c23*s1*c4+c1*s4)*c5-s1*s23*s5 -(s1*c23*c4+c1*s4)*s5-s1*s23*c5
-(-s1*c23*s4+c1*c4) -s1*s23*r4+s1*(d2+d3*c2);
c4*s23*c5+c23*s5 -s23*c4*s5+c23*c5
s23*s4 d3*s2+r4*c23;
0 0
0 1 ];

t06_11 = ((c1*c23*c4-s1*s4)*c5-c1*s23*s5)*c6+(-(-c1*c23*s4-
s1*c4))*s6;
t06_21 = ((s1*c23*c4+c1*s4)*c5-s1*s23*s5)*c6+(-(-
s1*c23*s4+c1*c4))*s6;
t06_31 = (s23*c4*c5+c23*s5)*c6+s23*s4*s6;
t06_12 = -((c1*c23*c4-s1*s4)*c5-c1*s23*s5)*s6+(-(-c1*c23*s4-
s1*c4))*c6;
t06_22 = -((s1*c23*c4+c1*s4)*c5-s1*s23*s5)*s6+(-(-
s1*c23*s4+c1*c4))*c6;
t06_32 = -(s23*c4*c5+c23*s5)*s6+s23*s4*c6;
t06_13 = -(-(c1*c23*c4-s1*s4)*s5-c1*s23*c5);
t06_23 = -(-(s1*c23*c4+c1*s4)*s5-s1*s23*c5);
t06_33 = -(-s23*c4*s5+c23*c5);
t06_14 = -c1*s23*r4+c1*(d2+d3*c2);
t06_24 = -s1*s23*r4+s1*(d2+d3*c2);
t06_34 = c23*r4+d3*s2;

T06=[t06_11 t06_12 t06_13 t06_14;


t06_21 t06_22 t06_23 t06_24;
t06_31 t06_32 t06_33 t06_34;
0 0 0 1 ];

%Calculos de las posiciones de las articulaciones


TE1=Te0*T01;
TE2=Te0*T02;
TE3=Te0*T03;
TE4=Te0*T04;
TE5=Te0*T05;
TE6=Te0*T06;
s1n=Te0*ss1;
%s2n=Te0*sp2;

%Separacion de las coordenadas de las articulaciones


%con el robot en el emplazamiento especificado:
x0 = TE1(1,4);
y0 = TE1(2,4);
z0 = TE1(3,4);
x1 = TE2(1,4);
y1 = TE2(2,4);
z1 = TE2(3,4);
x2 = TE3(1,4);
y2 = TE3(2,4);
z2 = TE3(3,4);
x3 = TE4(1);
y3 = TE4(2);
z3 = TE4(3);
x4 = TE5(1,4);
y4 = TE5(2,4);
z4 = TE5(3,4);

%Declaracion de arreglos para los plots:


% s1x = [s1n(1) s2n(1)];
% s1y = [s1n(2) s2n(2)];
% s1z = [s1n(3) s2n(3)];

s1x = [Te0(1,4) s1n(1)];


s1y = [Te0(2,4) s1n(2)];
s1z = [Te0(3,4) s1n(3)];

r1x = [Te0(1,4) x1];


r1y = [Te0(2,4) y1];
r1z = [Te0(3,4) z1];

r2x = [x1 x2];


r2y = [y1 y2];
r2z = [z1 z2];

r3x = [x2 x3];


r3y = [y2 y3];
r3z = [z2 z3];

r4x = [x3 x4];


r4y = [y3 y4];
r4z = [z3 z4];

%Transformacin de los vrtices de la pinza del marco 6 al marco e:


zz0 =TE6*p0;
zz0p=TE6*p0p;
zz1 =TE6*p1;
zz2 =TE6*p2;
zz3 =TE6*p3;
zz4 =TE6*p4;
zz5 =TE6*p5;
zz6 =TE6*p6;
zz7 =TE6*p7;
zz8 =TE6*p8;

% s1x=[Te0(1,4) xx1(1)];
% s1y=[Te0(2,4) xx1(2)];
% s1z=[Te0(3,4) xx1(3)];

%Arreglos de la base
xx1=Te0*ss1;
xx2=Te0*ss2;
xx3=Te0*ss3;
xx4=Te0*ss4;
xx5=Te0*ss5;

%Base
b1x=[xx2(1) xx3(1)];%12
b1y=[xx2(2) xx3(2)];
b1z=[xx2(3) xx3(3)];

b2x=[xx3(1) xx5(1)];%24
b2y=[xx3(2) xx5(2)];
b2z=[xx3(3) xx5(3)];

b3x=[xx5(1) xx4(1)];%43
b3y=[xx5(2) xx4(2)];
b3z=[xx5(3) xx4(3)];

b4x=[xx4(1) xx2(1)];%31
b4y=[xx4(2) xx2(2)];
b4z=[xx4(3) xx2(3)];

%Arreglos para las aristas de la herramienta


d01x=[x4 zz0(1)];
d01y=[y4 zz0(2)];
d01z=[z4 zz0(3)];

d12x=[zz1(1) zz2(1)];
d12y=[zz1(2) zz2(2)];
d12z=[zz1(3) zz2(3)];

d24x=[zz2(1) zz4(1)];
d24y=[zz2(2) zz4(2)];
d24z=[zz2(3) zz4(3)];

d13x=[zz1(1) zz3(1)];
d13y=[zz1(2) zz3(2)];
d13z=[zz1(3) zz3(3)];

d34x=[zz3(1) zz4(1)];
d34y=[zz3(2) zz4(2)];
d34z=[zz3(3) zz4(3)];

d15x=[zz1(1) zz5(1)];
d15y=[zz1(2) zz5(2)];
d15z=[zz1(3) zz5(3)];

d37x=[zz3(1) zz7(1)];
d37y=[zz3(2) zz7(2)];
d37z=[zz3(3) zz7(3)];

d57x=[zz5(1) zz7(1)];
d57y=[zz5(2) zz7(2)];
d57z=[zz5(3) zz7(3)];
d78x=[zz7(1) zz8(1)];
d78y=[zz7(2) zz8(2)];
d78z=[zz7(3) zz8(3)];

d86x=[zz8(1) zz6(1)];
d86y=[zz8(2) zz6(2)];
d86z=[zz8(3) zz6(3)];

d65x=[zz6(1) zz5(1)];
d65y=[zz6(2) zz5(2)];
d65z=[zz6(3) zz5(3)];

iii=1;

pnx1((iii-1)*np+i+1)=zz0p(1);
pny1((iii-1)*np+i+1)=zz0p(2);
pnz1((iii-1)*np+i+1)=zz0p(3);

% figure(1)
% clf
% hold on
%
% %Graficacin de la ruta
% inn=(iii-1)*np+i;
% for j = 2 : inn
% plot3([pnx1(j-1),pnx1(j)],[pny1(j-1),pny1(j)],[pnz1(j-
1),pnz1(j)],'y','linewidth',2);
% %plot3([pnx2(j-1),pnx2(j)],[pny2(j-1),pny2(j)],[pnz2(j-
1),pnz2(j)],'b','linewidth',2);
%
% hold on
% end
%
%
% %Graficacin del robot
%
% %Base del robot
%
plot3(b1x,b1y,b1z,'k',b2x,b2y,b2z,'k',b3x,b3y,b3z,'k',b4x,b4y,b4z,'k','Li
neWidth',2)
%
% %Vstago de la base
% plot3(s1x,s1y,s1z,'k','MarkerSize',5,'LineWidth',2);
%
% %Eslabones
% plot3(r1x,r1y,r1z,'k-o',r2x,r2y,r2z,'k-o',r3x,r3y,r3z,'k-
o',r4x,r4y,r4z,'k-o',d01x,d01y,d01z,'r','LineWidth',2)
%
% %Pinza
%
plot3(d12x,d12y,d12z,'r',d24x,d24y,d24z,'r',d13x,d13y,d13z,'r','LineWidth
',2)
%
plot3(d34x,d34y,d34z,'r',d15x,d15y,d15z,'r',d37x,d37y,d37z,'r','LineWidth
',2)
%
plot3(d57x,d57y,d57z,'r',d78x,d78y,d78z,'r',d86x,d86y,d86z,'r','LineWidth
',2)
% plot3(d65x,d65y,d65z,'r','LineWidth',2)
%
% hold on
%
% title('Staubli');
% xlabel('Eje x');
% ylabel('Eje y');
% zlabel('Eje z');
%
% axis([-1500,1500,-1500,1500,-1500,1500])
% view([1,-2,1])
% grid on
% pause (0.1)

end

Wmed = mean(Wtrsl);
Wstd = std(Wtrsl,1);

fk = (Wstd - Wmed)*10000

% %Graficacin de la manipulabilidad rotacional y traslacional


% figure(4)
%
% subplot(2,1,1)
% plot(time,Wa,'r','linewidth',2);
% title('Manipulabilidad Traslacional');
% xlabel('Time (Seconds)');
% ylabel('Magnitud');
% hold on
% axis ([0,T,0,2])
% grid on
%
% subplot(2,1,2)
% plot(time,Ww,'r','linewidth',2);
% title('Manipulabilidad Rotacional');
% xlabel('Time (Seconds)');
% ylabel('Magnitud');
% hold on
% axis ([0,T,0,2])
% grid on

Programa ProgPrinTrian
%Parmetros de emplazamiento
x=[xa xb xc]';
A=[];
%Coordenadas de emplazamiento
xa=-600;
xb=-680;
xc=-280;
%Clculo del valor mnimo
Aeq = [];
beq = []';
b = []';
ceq = []';
%Lmite inferior y superior
lb = [-600 -600 -300]';
ub = [ 600 600 300]';

xop = fmincon('fnemptrian',x,A,b,Aeq,beq,lb,ub);

%Emplazamiento ptimo
xsol = xop

np=50;

Programa TrayecTrian
%Parmetros del robot
%Longitud de cada eslabn
d2 = 100;
d3 = 850;
d4 = 0;
r4 = 1050;
dhh = 400;

%Nmeros de puntos de graficacin


np = 50;

%Caracteristicas de la trayectoria
T=15;
T1=T/3;
T2=T/3;
T3=T/3;

%Arreglos para variables articulares (ngulos)


th1g = zeros([1,np]);
th2g = zeros([1,np]);
th3g = zeros([1,np]);
th4g = zeros([1,np]);
th5g = zeros([1,np]);
th6g = zeros([1,np]);

% MtrH= zeros(1,np);
% MarH= zeros(1,np);

pnx1= zeros(1,np);
pny1= zeros(1,np);
pnz1= zeros(1,np);
pnx2= zeros(1,np);
pny2= zeros(1,np);
pnz2= zeros(1,np);

Wtrsl= zeros(1,np);
Wrtsl= zeros(1,np);
time= zeros(1,np);

%Arreglo de la Base
sp1 = [ 0 0 0 1]';
sp2 = [ 0 0 150 1]';
p1b = [-150 150 0 1]';
p2b = [ 150 150 0 1]';
p3b = [-150 -150 0 1]';
p4b = [ 150 -150 0 1]';

%Especificacion de la posicion del vertice


%de la placa base c.r. al marco 0:
ss1 = [0 0 -400 1]';

%Especificacion de las posiciones de los vertices


%de la placa base c.r. al marco 0:
ss2=[ 200 200 -400 1]';
ss3=[-200 200 -400 1]';
ss4=[ 200 -200 -400 1]';
ss5=[-200 -200 -400 1]';

p0 =[ 0 0 200 1]';
p0p=[ 0 0 400 1]';
p1 =[ 100 50 200 1]';
p2 =[ 100 50 400 1]';
p3 =[ 100 -50 200 1]';
p4 =[ 100 -50 400 1]';
p5 =[-100 50 200 1]';
p6 =[-100 50 400 1]';
p7 =[-100 -50 200 1]';
p8 =[-100 -50 400 1]';

ss6 = [300 0 0 1]';


ss7 = [0 300 0 1]';
ss8 = [0 0 300 1]';

ee6x = [300 0 0 1]';


ee6y = [0 300 0 1]';
ee6z = [0 0 300 1]';
%Coordenadas del marco 0
ex0 = [0 0 0 1]';
ey0 = [0 0 0 1]';
ez0 = [0 0 0 1]';

%Marco de la herramienta
hx0 = [300 0 0 1]';
hy0 = [0 300 0 1]';
hz0 = [0 0 300 1]';
%Marco de la pieza
px0 = [300 0 0 1]';
py0 = [0 300 0 1]';
pz0 = [0 0 300 1]';

%Especificacion del emplazamiento de la base del robot


%Base del robot (marco 0) c.r. al marco de la estacion
%de trabajo (marco E):
xaa= -599.01;%-450;
yaa= -599.01;%0;
zaa= -280;%250;

%Especificacion del emplazamiento de la base del robot


%Base del robot (marco P) c.r. al marco de la estacion
%de trabajo (marco E):
xbb= 500;%1000;
ybb= 500;%750;
zbb= 500;%1000;

%Matriz de emplazamiento
Te0 = [1 0 0 xaa;
0 1 0 yaa;
0 0 1 zaa;
0 0 0 1];

T0e = (Te0)^(-1);

%Ubicacin del marco de la tarea (Matriz de e a p)


Tep = [1 0 0 xbb;
0 1 0 ybb;
0 0 1 zbb;
0 0 0 1];

T0p = T0e*Tep;

%Matriz rotacional
R0p = [T0p(1,1) T0p(1,2) T0p(1,3);
T0p(2,1) T0p(2,2) T0p(2,3);
T0p(3,1) T0p(3,2) T0p(3,3)];

%Matriz del marco de la herramienta


T6h = [1 0 0 0 ;
0 1 0 0 ;
0 0 1 dhh;
0 0 0 1 ];

Th6 = (T6h)^(-1);

e1 = 1;
e2 = 1;
e3 = -1;
e5 = -1;

iii=1;
% %Especificacion de la parametros de la herramienta
% T= 5;
% xinih= 0;
% delx= 0;
% yinih= 150;
% dely= 0;
% zinih= 0;
% delz= 1500;
%
% %Especificacion de la parametros de orientacion
% alfaini = deg2rad( -90);
% deltaalfa = deg2rad( 0);
% betaini = deg2rad( 180);
% deltabeta = deg2rad(-180);
% gammaini = 0;
% deltagamma= 0;

phi=deg2rad(60);

%Graficacin de la ruta
for i = 0 : np

%Valor del tiempo para cada iteracin


t = T*(i/np);
time(i+1) = t;
%Variables articulares (t/T-1/2)*sin?(2t/T)
%ft = (t/T)-(1/(2*pi))*(sin((2*pi*t)/T));

%Ruta
if t<=T1
ft = (t/T1)-((1/(2*pi))*(sin(2*pi*t/T1)));

Rphd=[-1 0 0;0 0 1;0 1 0];

% alfa = alfaini + deltaalfa*(ft);


% beta = betaini + deltabeta*(ft);
% gamma= gammaini + deltagamma*(ft);
%
% calf=cos(alfa);
% salf=sin(alfa);
% cbet=cos(beta);
% sbet=sin(beta);
% cgam=cos(gamma);
% sgam=sin(gamma);
%
%
% Rphd=[calf*cbet calf*sbet*sgam-salf*cgam calf*sbet*cgam+salf*sgam;
% salf*cbet salf*sbet*sgam+calf*cgam salf*sbet*cgam-calf*sgam;
% -sbet cbet*sgam cbet*cgam];

%Posicin de la matriz de p a h
xph=-400*cos(phi)*ft;
yph= 400*cos(phi)*ft;
zph= 0;

xp1=xph;
yp1=yph;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];

%Matriz deseada del marco 6


U0 = T0e*Tep*TPhd*Th6;

%Clculo del modelo inverso de posicin


%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

if t>T1 && t<=(T1+T2)


ft = (((t-T1)/T2)-((1/(2*pi))*(sin(2*pi*(t-T1)/T2))));

Rphd=[-1 0 0;0 0 1;0 1 0];

%Posicin de la matriz de p a h
xph= xp1 + 400*ft;
yph= yp1;
zph= 0;

xp2=xph;
yp2=yph;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];

%Clculo del modelo inverso de posicin


%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

if t>(T1+T2) && t<=(T1+T2+T3)


ft = (((t-T1-T2)/T3)-((1/(2*pi))*(sin(2*pi*(t-T1-T2)/T3))));

Rphd=[-1 0 0;0 0 1;0 1 0];

%Posicin de la matriz de p a h
xph= xp2 - 400*cos(phi)*ft;
yph= yp2 - 400*cos(phi)*ft;
zph= 0;

%Matriz de ubicacion deseada del marco h c.r. al marco P:


TPhd=[Rphd(1,1) Rphd(1,2) Rphd(1,3) xph;
Rphd(2,1) Rphd(2,2) Rphd(2,3) yph;
Rphd(3,1) Rphd(3,2) Rphd(3,3) zph;
0 0 0 1];

%Clculo del modelo inverso de posicin


%matriz de ubicacion deseada del marco 6 c.r. al marco 0:
T0hd=T0p*TPhd;
T06d=T0hd*Th6;

%Matriz SNAP
SX=T06d(1,1);
SY=T06d(2,1);
SZ=T06d(3,1);

NX=T06d(1,2);
NY=T06d(2,2);
NZ=T06d(3,2);

AX=T06d(1,3);
AY=T06d(2,3);
AZ=T06d(3,3);

PX=T06d(1,4);
PY=T06d(2,4);
PZ=T06d(3,4);

Snap=[SX NX AX PX;
SY NY AY PY;
SZ NZ AZ PZ;
0 0 0 1];
end

%Clculo de cada th
%Theta1:
th1 = atan2(e1*PY,e1*PX);

s1 = sin(th1);
c1 = cos(th1);

%Theta2:
B2=c1*PX+s1*PY-d2;
X=-2*PZ*d3;
Y=-2*B2*d3;
Z=r4*r4-d3*d3-B2*B2-PZ*PZ;
D=X*X+Y*Y;
W=sqrt(D-Z*Z);
C2=(Y*Z-e2*X*W)/D;
S2=(X*Z+e2*Y*W)/D;
th2=atan2(S2,C2);

S3=-(PZ*S2+B2*C2-d3)/r4;
C3=-(B2*S2-PZ*C2)/r4;
th3=atan2(S3,C3);
c23=cos(th2+th3);
s23=sin(th2+th3);

if e5==+1
th5=acos(s23*c1*AX+s23*s1*AY-c23*AZ);
else
th5=-(acos(s23*c1*AX+s23*s1*AY-c23*AZ));
end

s5=sin(th5);
c5=cos(th5);
B2=s1*AX-c1*AY;
B4=c23*c1*AX+c23*s1*AY+s23*AZ;
S4=-B2/s5;
C4=+B4/s5;

th4=atan2(S4,C4);

F2=-s23*c1*NX-s23*s1*NY+c23*NZ;
F4=-s23*c1*SX-s23*s1*SY+c23*SZ;
S6=-F2/s5;
C6=F4/s5;

th6=atan2(S6,C6);

th12 = th1+th2;
th14 = th1+th4;
th123 = th1+th2+th3;
th1234 = th123+th4;
th12345 = th1234+th5;
th123456 = th12345+th6;
th235 = th2+th3+th5;

c1 = cos(th1);
s1 = sin(th1);
c2 = cos(th2);
s2 = sin(th2);
c3 = cos(th3);
s3 = sin(th3);
c4 = cos(th4);
s4 = sin(th4);
c5 = cos(th5);
s5 = sin(th5);
c6 = cos(th6);
s6 = sin(th6);
s12 = sin(th12);
c12 = cos(th12);
s14 = sin(th14);
c14 = cos(th14);
s23 = sin(th2+th3);
c23 = cos(th2+th3);
s235 = sin(th235);
c235 = cos(th235);
s123=sin(th123);
c123=cos(th123);
s1234=sin(th1234);
c1234=sin(th1234);
s12345=sin(th12345);
c12345=sin(th12345);
s123456=sin(th123456);
c123456=sin(th123456);
%Obtencin del modelo directo de posicin (Jacobiana),
%por medio de la derivada parcial con respecto a los ngulos (theta1,
theta2, theta3, theta4, theta5, theta6)

J11=+r4*s1*s23-d2*s1-d3*c2*s1;
J21=-r4*c1*s23+d2*c1+d3*c1*c2;
J31=0;
J41=0;
J51=0;
J61=1;

J12=-r4*c1*c2*c3+r4*c1*s2*s3-d3*c1*s2;
J22=-r4*s1*c2*c3+r4*s1*s2*s3;
J32=-r4*s2*c3-r4*c2*s3+d3*c2;
J42=s1;
J52=-c1;
J62=0;

J13=-r4*c1*c23;
J23= -r4*c23*s1;
J33=-r4*s23;
J43=s1;
J53=-c1;
J63=0;

J14=0;
J24=0;
J34=0;
J44=-c1*s23;
J54=-s1*s23;
J64=c23;

J15=0;
J25=0;
J35=0;
J45=s4*c1*c23+s1*c4;
J55=s4*s1*c23-c1*c4;
J65=s23*s4;

J16=0;
J26=0;
J36=0;
J46=c1*c23*c4*s5-s1*s4*s5+c1*s23*c5;
J56=s1*c23*c4*s5+c1*s4*s5+s1*s23*c5;
J66=s23*c4*s5-c23*c5;

J =[J11 J12 J13 J14 J15 J16;


J21 J22 J23 J24 J25 J26;
J31 J32 J33 J34 J35 J36;
J41 J42 J43 J44 J45 J46;
J51 J52 J53 J54 J55 J56;
J61 J62 J63 J64 J65 J66];

%Clculo de la manipulabilidad rotacional y traslacional

JTA = [J11 J12 J13;


J21 J22 J23;
J31 J32 J33];

JRW = [J44 J45 J46;


J54 J55 J56;
J64 J65 J66];
%fnor=1.25e+009;
fnor=1.5e+09;

Wtrsl(i+1) = sqrt(det(JTA*JTA'))/fnor;

Wrtsl(i+1) = sqrt(det(JRW*JRW'));

npnn=np;

if Z*Z > D
Wtrsl(i+1)=0;
Wrtsl(i+1)=0;
end

% Wglob(i+1) = Wa(i+1);
% Wglob(np+i+1) = Ww(i+1);

%Matrices elementales

T01 = [c1 -s1 0 0 ;


s1 c1 0 0 ;
0 0 1 0 ;
0 0 0 1];

T12 = [c2 -s2 0 d2;


0 0 -1 0 ;
s2 c2 0 0 ;
0 0 0 1];

%T02 = T01*T12;

T23 = [c3 -s3 0 d3;


s3 c3 0 0 ;
0 0 1 0 ;
0 0 0 1];

T34 = [c4 -s4 0 d4 ;


0 0 -1 -r4;
s4 c4 0 0 ;
0 0 0 1];

%matrices de transformacion homogeneas


T02=[c1*c2 -c1*s2 s1 d2*c1;
c2*s1 -s1*s2 -c1 d2*s1;
s2 c2 0 0;
0 0 0 1];

T03=[c1*c23 -c1*s23 s1 d2*c1+d3*c1*c2;


c23*s1 -s1*s23 -c1 d2*s1+d3*c2*s1;
s23 c23 0 d3*s2;
0 0 0 1];

vector=[d4 0 0 1]';
T04=T03*vector;

T05=[(c1*c23*c4-s1*s4)*c5-c1*s23*s5 -(c1*c23*c4-s1*s4)*s5-c1*s23*c5
-(-c1*c23*s4-s1*c4) -c1*s23*r4+c1*(d2+d3*c2);
(c23*s1*c4+c1*s4)*c5-s1*s23*s5 -(s1*c23*c4+c1*s4)*s5-s1*s23*c5
-(-s1*c23*s4+c1*c4) -s1*s23*r4+s1*(d2+d3*c2);
c4*s23*c5+c23*s5 -s23*c4*s5+c23*c5
s23*s4 d3*s2+r4*c23;
0 0
0 1 ];

t06_11 = ((c1*c23*c4-s1*s4)*c5-c1*s23*s5)*c6+(-(-c1*c23*s4-
s1*c4))*s6;
t06_21 = ((s1*c23*c4+c1*s4)*c5-s1*s23*s5)*c6+(-(-
s1*c23*s4+c1*c4))*s6;
t06_31 = (s23*c4*c5+c23*s5)*c6+s23*s4*s6;
t06_12 = -((c1*c23*c4-s1*s4)*c5-c1*s23*s5)*s6+(-(-c1*c23*s4-
s1*c4))*c6;
t06_22 = -((s1*c23*c4+c1*s4)*c5-s1*s23*s5)*s6+(-(-
s1*c23*s4+c1*c4))*c6;
t06_32 = -(s23*c4*c5+c23*s5)*s6+s23*s4*c6;
t06_13 = -(-(c1*c23*c4-s1*s4)*s5-c1*s23*c5);
t06_23 = -(-(s1*c23*c4+c1*s4)*s5-s1*s23*c5);
t06_33 = -(-s23*c4*s5+c23*c5);
t06_14 = -c1*s23*r4+c1*(d2+d3*c2);
t06_24 = -s1*s23*r4+s1*(d2+d3*c2);
t06_34 = c23*r4+d3*s2;

T06=[t06_11 t06_12 t06_13 t06_14;


t06_21 t06_22 t06_23 t06_24;
t06_31 t06_32 t06_33 t06_34;
0 0 0 1 ];

%Calculos de las posiciones de las articulaciones


TE1=Te0*T01;
TE2=Te0*T02;
TE3=Te0*T03;
TE4=Te0*T04;
TE5=Te0*T05;
TE6=Te0*T06;
s1n=Te0*ss1;
%s2n=Te0*sp2;

%Separacion de las coordenadas de las articulaciones


%con el robot en el emplazamiento especificado:
x0 = TE1(1,4);
y0 = TE1(2,4);
z0 = TE1(3,4);
x1 = TE2(1,4);
y1 = TE2(2,4);
z1 = TE2(3,4);
x2 = TE3(1,4);
y2 = TE3(2,4);
z2 = TE3(3,4);
x3 = TE4(1);
y3 = TE4(2);
z3 = TE4(3);
x4 = TE5(1,4);
y4 = TE5(2,4);
z4 = TE5(3,4);

%Declaracion de arreglos para los plots:


% s1x = [s1n(1) s2n(1)];
% s1y = [s1n(2) s2n(2)];
% s1z = [s1n(3) s2n(3)];

s1x = [Te0(1,4) s1n(1)];


s1y = [Te0(2,4) s1n(2)];
s1z = [Te0(3,4) s1n(3)];

r1x = [Te0(1,4) x1];


r1y = [Te0(2,4) y1];
r1z = [Te0(3,4) z1];

r2x = [x1 x2];


r2y = [y1 y2];
r2z = [z1 z2];

r3x = [x2 x3];


r3y = [y2 y3];
r3z = [z2 z3];

r4x = [x3 x4];


r4y = [y3 y4];
r4z = [z3 z4];

%Transformacin de los vrtices de la pinza del marco 6 al marco e:


zz0 =TE6*p0;
zz0p=TE6*p0p;
zz1 =TE6*p1;
zz2 =TE6*p2;
zz3 =TE6*p3;
zz4 =TE6*p4;
zz5 =TE6*p5;
zz6 =TE6*p6;
zz7 =TE6*p7;
zz8 =TE6*p8;

% s1x=[Te0(1,4) xx1(1)];
% s1y=[Te0(2,4) xx1(2)];
% s1z=[Te0(3,4) xx1(3)];

%Arreglos de la base
xx1=Te0*ss1;
xx2=Te0*ss2;
xx3=Te0*ss3;
xx4=Te0*ss4;
xx5=Te0*ss5;

%Base
b1x=[xx2(1) xx3(1)];%12
b1y=[xx2(2) xx3(2)];
b1z=[xx2(3) xx3(3)];

b2x=[xx3(1) xx5(1)];%24
b2y=[xx3(2) xx5(2)];
b2z=[xx3(3) xx5(3)];

b3x=[xx5(1) xx4(1)];%43
b3y=[xx5(2) xx4(2)];
b3z=[xx5(3) xx4(3)];

b4x=[xx4(1) xx2(1)];%31
b4y=[xx4(2) xx2(2)];
b4z=[xx4(3) xx2(3)];

%Arreglos para las aristas de la herramienta


d01x=[x4 zz0(1)];
d01y=[y4 zz0(2)];
d01z=[z4 zz0(3)];

d12x=[zz1(1) zz2(1)];
d12y=[zz1(2) zz2(2)];
d12z=[zz1(3) zz2(3)];

d24x=[zz2(1) zz4(1)];
d24y=[zz2(2) zz4(2)];
d24z=[zz2(3) zz4(3)];

d13x=[zz1(1) zz3(1)];
d13y=[zz1(2) zz3(2)];
d13z=[zz1(3) zz3(3)];

d34x=[zz3(1) zz4(1)];
d34y=[zz3(2) zz4(2)];
d34z=[zz3(3) zz4(3)];

d15x=[zz1(1) zz5(1)];
d15y=[zz1(2) zz5(2)];
d15z=[zz1(3) zz5(3)];

d37x=[zz3(1) zz7(1)];
d37y=[zz3(2) zz7(2)];
d37z=[zz3(3) zz7(3)];

d57x=[zz5(1) zz7(1)];
d57y=[zz5(2) zz7(2)];
d57z=[zz5(3) zz7(3)];

d78x=[zz7(1) zz8(1)];
d78y=[zz7(2) zz8(2)];
d78z=[zz7(3) zz8(3)];

d86x=[zz8(1) zz6(1)];
d86y=[zz8(2) zz6(2)];
d86z=[zz8(3) zz6(3)];

d65x=[zz6(1) zz5(1)];
d65y=[zz6(2) zz5(2)];
d65z=[zz6(3) zz5(3)];

iii=1;

pnx1((iii-1)*np+i+1)=zz0p(1);
pny1((iii-1)*np+i+1)=zz0p(2);
pnz1((iii-1)*np+i+1)=zz0p(3);
%Matriz del marco 0 al h
T0h = U0*T6h;
%Clculo de coordenadas de Oh en el marco 0
xh = T0h(1,4);
yh = T0h(2,4);
zh = T0h(3,4);
%Transformacin de coordenadas de Oh del marco 0 al marco e:
rtosOh = Te0*[xh yh zh 1]';

xhn = rtosOh(1);
yhn = rtosOh(2);
zhn = rtosOh(3);

hpnx(i+1) = xhn;
hpny(i+1) = yhn;
hpnz(i+1) = zhn;

%Arreglos para los ejes del marco h

hx0n = Te0*U0*T6h*hx0;
hy0n = Te0*U0*T6h*hy0;
hz0n = Te0*U0*T6h*hz0;

hx0x = [hx0n(1) xhn];


hx0y = [hx0n(2) yhn];
hx0z = [hx0n(3) zhn];

hy0x = [hy0n(1) xhn];


hy0y = [hy0n(2) yhn];
hy0z = [hy0n(3) zhn];

hz0x = [hz0n(1) xhn];


hz0y = [hz0n(2) yhn];
hz0z = [hz0n(3) zhn];

ex0n = Te0*ex0;
ey0n = Te0*ey0;
ez0n = Te0*ez0;

px0n = Tep*px0;
py0n = Tep*py0;
pz0n = Tep*pz0;

%Arreglos para el marco de la pieza


px0x = [px0n(1) xbb];
px0y = [px0n(2) ybb];
px0z = [px0n(3) zbb];

py0x = [py0n(1) xbb];


py0y = [py0n(2) ybb];
py0z = [py0n(3) zbb];

pz0x = [pz0n(1) xbb];


pz0y = [pz0n(2) ybb];
pz0z = [pz0n(3) zbb];

ex0x = [s1n(1) ex0n(1)];


ex0y = [s1n(2) ex0n(2)];
ex0z = [s1n(3) ex0n(3)];

ey0x = [s1n(1) ey0n(1)];


ey0y = [s1n(2) ey0n(2)];
ey0z = [s1n(3) ey0n(3)];

ez0x = [s1n(1) ez0n(1)];


ez0y = [s1n(2) ez0n(2)];
ez0z = [s1n(3) ez0n(3)];

figure(1)
clf
hold on

%Graficacin de la ruta
inn=(iii-1)*np+i;
for j = 2 : inn
plot3([pnx1(j-1),pnx1(j)],[pny1(j-1),pny1(j)],[pnz1(j-
1),pnz1(j)],'b','linewidth',2);
%plot3([pnx2(j-1),pnx2(j)],[pny2(j-1),pny2(j)],[pnz2(j-
1),pnz2(j)],'b','linewidth',2);

hold on
end

%Graficacin del robot

%Base del robot

plot3(b1x,b1y,b1z,'r',b2x,b2y,b2z,'r',b3x,b3y,b3z,'r',b4x,b4y,b4z,'r','Li
neWidth',2)

%Vstago de la base
plot3(s1x,s1y,s1z,'r','MarkerSize',5,'LineWidth',2);
%Eslabones
plot3(r1x,r1y,r1z,'k-o',r2x,r2y,r2z,'k-o',r3x,r3y,r3z,'k-
o',r4x,r4y,r4z,'k-o',d01x,d01y,d01z,'r','LineWidth',2)

%Pinza
plot3(d12x,d12y,d12z,'r',d24x,d24y,d24z,'r',d13x,d13y,d13z,'r','LineWidth
',2)
plot3(d34x,d34y,d34z,'r',d15x,d15y,d15z,'r',d37x,d37y,d37z,'r','LineWidth
',2)
plot3(d57x,d57y,d57z,'r',d78x,d78y,d78z,'r',d86x,d86y,d86z,'r','LineWidth
',2)
plot3(d65x,d65y,d65z,'r','LineWidth',2)

%Marco h
plot3(hx0x,hx0y,hx0z,'g',hy0x,hy0y,hy0z,'b',hz0x,hz0y,hz0z,'r','linewidth
',2);

%Marco 0
plot3(ex0x,ex0y,ex0z,'g',ey0x,ey0y,ey0z,'b',ez0x,ez0y,ez0z,'r','linewidth
',2);

%Marco p
plot3(px0x,px0y,px0z,'g',py0x,py0y,py0z,'b',pz0x,pz0y,pz0z,'r','linewidth
',2);

hold on

title('Staubli');
xlabel('Eje x');
ylabel('Eje y');
zlabel('Eje z');

axis([-1500,1500,-1500,1500,-1500,1500])
view([1,-2,1])
grid on
pause (0.1)

end

Wmed = mean(Wtrsl);
Wstd = std(Wtrsl,1);

fk = (Wstd - Wmed)*10000

%Graficacin de la manipulabilidad rotacional y traslacional


figure(2)
plot(time,Wtrsl,'b','Linewidth',2);
axis ([0,T,0,2])
hold on
grid on

figure(3)
plot(time,Wrtsl,'r','Linewidth',2);
hold on
axis ([0,T,0,2])
grid on