function [qtildeg tau] = cap8_PDrobot3gdl(x,xp)
q1=x(1);  q2=x(2); q3=x(3); 
q = [q1; q2; q3];   %posiciones
qp1=xp(1); qp2=xp(2); qp3=xp(3); 
qp = [qp1; qp2; qp3]; %velocidades

%parámetros del robot: par gravitacional
m1=26.902; l1=0.45;  m2=30; l2=0.45; lc2=0.038; 
m3=3.880; l3=0.45; lc3=0.048; g=9.81;   

 gq11=0; 
gq21=(lc2*m1++m2*l2)*sin(q1)+m2*lc3*sin(q1+q2);
 gq31=m2*lc3*sin(q1+q2);  
gq=g*[gq11; gq21; gq31]; %par gravitacional



qd=[30; 45; 90]; %referencias o posiciones deseadas 
qtilde=qd*pi/180-q;   %error de posición qt
qtildeg=(180/pi)*qtilde; %error de posición qt en grados 
qpgrados=180*qp/pi;  %velocidad  $\qp$ en grados/segundo.

%ganancia proporcional
Kp=[1, 0,0;
     0, 2,0;
     0,0,0.15];

%ganancia derivativa
Kv=[0.3, 0,0;
     0, 0.6,0;
       0,0,0.015];
% control proporcional derivativo PD
 tau=Kp*qtildeg-Kv*qpgrados+gq;         
end