function dotq = open_link_2DOF_PD_params (t, q, param, gravity, thetad, Kp, Kd)
% equation of motion of 2DOF open link mechanism under PD control
    theta = q(1:2);
    omega = q(3:4);
    
    dottheta = omega;    
    [inertia_matrix, joint_torques] = open_link_2DOF_dynamics (theta, omega, param, gravity);
    tau = -Kp.*(theta - thetad) - Kd.*omega;
    dotomega = inertia_matrix \ (joint_torques + tau);
    
    dotq = [dottheta; dotomega];
end
