clc; clear all; close all;
format short g
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 sistema dinámico lineal
[t,x]=ode45('cap8_robot2gdl',t,ci,opciones);
   
    q1=x(:,1);    q2=x(:,2);
    qp1=x(:,3);   qp2=x(:,4);
    
   [n,m]=size(t);
   tau1=zeros(n,1);   tau2=zeros(n,1); 
qtilde1=zeros(n,1); qtilde2=zeros(n,1);
   xef=zeros(n,1);   yef=zeros(n,1);
   
   beta1=0.15; beta2=0.15; l1=0.45; l2=0.45;
   for k=1:n   
    [qt  tau] = cap8_PDrobot2gdl([q1(k);q2(k)], [qp1(k); qp2(k)]);
    tau1(k)=tau(1); tau2(k)=tau(2);
    qtilde1(k)=qt(1); qtilde2(k)=qt(2);
     [xef(k), yef(k),~ ]=cinematica_r2gdl( beta1, l1, q1(k), beta2, l2, q2(k));
       p=Rz(-pi/2)*[xef(k), yef(k), beta1+beta2]';
       xef(k)=p(1);
       yef(k)=p(2);
   end

subplot(2,2,1); plot(t,qtilde1,t,qtilde2)
subplot(2,2,2); plot( t, tau1, t,tau2)
subplot(2,2,3); plot(xef,yef)
subplot(2,2,4); plot(qtilde1, qp1, qtilde2,qp2)