function dotq = closed_link_2DOF_PD_params (t, q, param, gravity, thetad, Kp, Kd, alpha)
% equation of motion of 2DOF closed link mechanism under PD control
    theta = q(1:4);
    omega = q(5:8);
    
    dottheta = omega;
    
    theta12 = theta(1:2);
    omega12 = omega(1:2);
    [inertia_matrix_12, joint_torques_12] = ...
        open_link_2DOF_dynamics(theta12, omega12, param(1:2), gravity);    
    theta34 = theta(3:4);
    omega34 = omega(3:4);
    [inertia_matrix_34, joint_torques_34] = ...
        open_link_2DOF_dynamics(theta34, omega34, param(3:4), gravity);
    
    tau = [ -Kp(1)*(theta(1) - thetad(1)) - Kd(1)*omega(1); 0;
            -Kp(2)*(theta(3) - thetad(2)) - Kd(2)*omega(3); 0 ];
    
    [x12, y12, gx12, gy12, Qx12, Qy12] = open_link_2DOF_geometry (theta12, param(1:2));
    [x34, y34, gx34, gy34, Qx34, Qy34] = open_link_2DOF_geometry (theta34, param(3:4));
    
    X = x12 - x34;
    Y = y12 - y34;
    Cx = omega12'*Qx12*omega12 + omega34'*(-Qx34)*omega34 + ...
        2*alpha*(gx12'*omega12 + (-gx34)'*omega34) + alpha^2*X;
    Cy = omega12'*Qy12*omega12 + omega34'*(-Qy34)*omega34 + ...
        2*alpha*(gy12'*omega12 + (-gy34)'*omega34) + alpha^2*Y;
    A = [ inertia_matrix_12, zeros(2,2), -gx12, -gy12;
          zeros(2,2), inertia_matrix_34,  gx34,  gy34;
          -gx12',  gx34', 0, 0;
          -gy12',  gy34', 0, 0 ];
    b = [ joint_torques_12 + tau(1:2);
          joint_torques_34 + tau(3:4);
          Cx ;
          Cy ];
    p = A \ b;
    dotomega = p(1:4);
    
    dotq = [dottheta; dotomega];
end
