clc; clear all; close all; format short g
global tau1 tau2 tau3
ti=0; h=0.001; tf = 5; t=ti:h:tf;  %vector tiempo 
ci=[0; 0; 0; 0;0; 0]; %condiciones iniciales
opciones=odeset('RelTol',1e-3,'InitialStep',1e-3,'MaxStep',1e-3);
%solución numérica del robot cartesiano de 3 gdl
[t,x]=ode45('cap7_idecartesiano3gdl',t,ci,opciones);
   
d1=x(:,1); d2=x(:,2); d3=x(:,3);  %posiciones articulares
dp1=x(:,4); dp2=x(:,5); dp3=x(:,6); %velocidades articulares
    
[m n]=size(t); % dimensión del vector tiempo
dpp1=zeros(m,1); dpp2=zeros(m,1); dpp3=zeros(m,1); %registro para d_pp
u1=zeros(m,1); u2=zeros(m,1); u3=zeros(m,1); %registros para pares aplicados tau
    
for k=1:m
    xp=cap7_idecartesiano3gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4), x(k,5), x(k,6)]);
    dpp1(k,1)=xp(4,1);  
    dpp2(k,1)=xp(5,1); 
    dpp3(k,1)=xp(6,1);
    u1(k,1) =tau1;    
     u2(k,1) =tau2; 
     u3(k,1) =tau3;
end
 tau=[u1; u2; u3];%vector de pares aplicados
 fi11=dpp1;   
 fi12=zeros(m,1);  
 fi13=zeros(m,1);  
 fi14=dp1;  
fi15=zeros(m,1);   
fi16=zeros(m,1); 
fi17=sign(dp1);  
fi18=zeros(m,1);   
fi19=zeros(m,1);  
fi110=ones(m,1);

fi21=zeros(m,1); 
fi22=dpp2; 
fi23=zeros(m,1); 
fi24=zeros(m,1); 
fi25=dp2; 
fi26=zeros(m,1); 
fi27=zeros(m,1); 
fi28=sign(dp2); 
fi29=zeros(m,1); 
fi210=zeros(m,1); 
 
 
 
fi31=zeros(m,1); 
fi32=zeros(m,1); 
fi33=dpp3; 
fi34=zeros(m,1); 
fi35=zeros(m,1); 
fi36=dp3;
fi37=zeros(m,1); 
fi38=zeros(m,1); 
fi39=sign(dp3); 
fi310=zeros(m,1);
 
fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110; 
     fi21, fi22, fi23, fi24, fi25, fi26, fi27, fi28, fi29, fi210;
     fi31, fi32, fi33, fi34, fi35, fi36, fi37, fi38, fi39, fi310];
 theta=mincuadm(tau,fi,m,10,3);
 theta