function [ ddQ ] = RR_Dynamics( Q, dQ, T )
% Two-link robot dynamics
% L1 = L2 = 1m
% m1 = 2kg
% m2 = 2kg
% rev 6/5/20

q1 = Q(1);
q2 = Q(2);
dq1 = dQ(1);
dq2 = dQ(2);
c1 = cos(q1);
s1 = sin(q1);
s2 = sin(q2);
c2 = cos(q2);
s12 = sin(q1+q2);
c12 = cos(q1+q2);
g = -9.8;
M = [ 6 + 4*c2 , 2+c2 ; 2+c2 , 2];
C = [ 4*s2*dq1*dq2 + 2*s2*dq2*dq2 ; -2*s2*dq1*dq1 ];
G = [4*g*c1 + 2*g*c12 ; + 2*g*c12 ];
ddQ = inv(M) * ( T + C + G );
end