function [qtilde tau] = cap8_PDrobot2gdl(x,xp)
q1=x(1);  q2=x(2); q = [q1; q2]; %vector de posiciones

qp1=xp(1); qp2=xp(2); qp = [qp1; qp2]; %vector de velocidades

%parámetros del robot: par gravitacional
m1=23.902; lc1=0.091; l1=0.45;
m2=3.880; lc2=0.048; g=9.81;   
  theta4=g*(lc1*m1+m2*l1);
  theta5=g*m2*lc2;
  
%par gravitacional
  par_grav = [theta4*sin(q1)+theta5*sin(q1+q2);
              g*m2*lc2];
          
%ganancia proporcional
kp1=3; kp2=0.15;
Kp=[kp1, 0; 
     0, kp2];  %ganancia proporcional

%ganancia derivativa
kv1=0.20*kp1; kv2=0.20*kp2;
Kv=[kv1,  0; 
    0, kv2]; %ganancia derivativa

qd1=45; qd2=90; 
qd=[qd1; 
    qd2];  %vector de referencias

qtilde=pi/180*qd-q;
qtgrados=(180/pi)*qtilde; %error de posición en grados 
  
qpgrados=180*qp/pi; %velocidad en grados/segundo.
tau=Kp*qtgrados-Kv*qpgrados+par_grav;
end