clc; clear all; 
close all; 
format short g
global tau1 tau2
ti=0; h=0.001; tf = 5; 
t=ti:h:tf;  %vector tiempo 
ci=[0; 0; 0; 0]; %condiciones iniciales
opciones=odeset('RelTol', 1e-3, 'InitialStep',1e-3,'MaxStep',1e-3);
%solución numérica del robot de 2 gdl
disp('Advertencia: la simulación puede durar varios segundos......')
disp('Espere por favor..............')
[t,x]=ode45('cap7_iderobot2gdl',t,ci,opciones);
   
   
q1=x(:,1); q2=x(:,2); %vector de posición
qp1=x(:,3); qp2=x(:,4); %vector de velocidad
    
[m n]=size(t); 
qpp1=zeros(m,1); qpp2=zeros(m,1);%vectores de aceleración
 u1=zeros(m,1); u2=zeros(m,1); %vectores de pares aplicados
    
 for k=1:m %registro de la aceleración $\qpp$ y par aplicado tau
           xp=cap7_iderobot2gdl(t(k),[x(k,1),x(k,2),x(k,3), x(k,4)]);
           qpp1(k,1)=xp(3,1); %información  de aceleración qp_1(t)
           qpp2(k,1)=xp(4,1); %información  de aceleración qpp_2(t)
           u1(k,1) =tau1;  %par aplicado tau_1(t)
           u2(k,1) =tau2; %par aplicado tau_2(t)
 end
 y=qp1.*u1+qp2.*u2;%regresor  y
 %regresor psi
 fi11=qp1.*qpp1;
 fi12=-sin(q2).*qp2.*qp1.*(qp1+qp2)+cos(q2).*qpp1.*(qp1+qp2)+cos(q2).*qp1.*(qpp1+qpp2);
 fi13=qpp2.*((1/2)*qp2+qp1)+qp2.*((1/2)*qpp2+qp1)+qp2.*((1/2)*qp2+qpp1);      
 fi14=sin(q1).*qp1; 
 fi15=sin(q1+q2).*(qp1+qp2);  
 fi16=qp1.*qp1; 
 fi17=qp2.*qp2;
 fi18=abs(qp1); 
 fi19=abs(qp2); 
 fi110=qp1.*(1-abs(sign(qp1)));  
 fi111=qp2.*(1-abs(sign(qp2)));
 
 fi=[fi11, fi12, fi13, fi14, fi15, fi16, fi17, fi18, fi19, fi110, fi111];
 [r,theta] =mincuad(y,fi);
    theta