function qddot=dd_2R(u)
%direct dynamics of the 2R robot
%q=u(1:2);
%qdot=u(3:4);
%tau=u(5:6);
%m=u(7:8); %faults

global a1 a2 a3 a4 a5 %parameters of 2R dynamics
global l1 l2

q=u(1:2);
qdot=u(3:4);
tau=u(5:6);
F=u(7:8); %faults

c1=cos(q(1));
s1=sin(q(1));
c2=cos(q(2));
s2=sin(q(2));
c12=cos(q(1)+q(2));
s12=sin(q(1)+q(2));
%inertia
B=[a1+2*a2*c2 a3+a2*c2; a3+a2*c2 a3];

%centrifugal + potential terms
N=[-a2*(qdot(2).^2+2*qdot(1)*qdot(2))*s2; a2*(qdot(1).^2)*s2]+...
  [a4*cos(q(1))+a5*c12; a5*c12];

J=[-l1*s1-l2*s12 -l2*s12; l1*c1+l2*c12 l2*c12];

%accelerations
qddot=inv(B)*(-N+tau+J'*F);
%mod(q, 2*pi), B, inv(B)