function qdot = manip_pid2(q, t) 
global Kp Kd Ki;
m1 = 1; m2 = 1; l1 = 0.3; l2 = 0.3;
I1 = 0.03; I2 = 0.03; s1 = 0.15; s2 = 0.15; g = 9.8;
cos2 = cos(q(2)); sin1 = sin(q(1)); sin2 = sin(q(2)); 
sin12 = sin(q(1)+q(2));
M = [I1 + I2 + m2*l1*l1 + 2*m2*l1*s2*cos2, ...
I2+m2*l1*s2*cos2;
I2+m2*l1*s2*cos2, I2];
h = [-m2*l1*s2*sin2*(2*q(3)+q(4))*q(4)- ...
(m1*s1+m2*l1)*g*sin1-m2*s2*g*sin12;
m2*l1*s2*sin2*q(3)*q(3)-m2*s2*g*sin12];
u = - Kp*(q(1:2) - qd(t)) - Kd*(q(3:4) -vd(t)) - ...
Ki*q(5:6);
mh = inv(M)*(u - h);
qdot(1) = q(3);
qdot(2) = q(4);
qdot(3) = mh(1);
qdot(4) = mh(2);
qdot(5:6) = q(1:2) - qd(t);
endfunction
