function dotq = one_dim_link_PD (t, q)
    global J P Kp Kd thetad
    theta = q(1);
    omega = q(2);
    dottheta = omega;
    dotomega = (1/J)*(-P*cos(theta) -Kp*(theta-thetad) -Kd*omega);
    dotq = [ dottheta; dotomega ];
end
