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

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 = [ 4 + 2*c2 , 1+c2 ; 1+c2 , 1];
C = [ 2*s2*dq1*dq2 + s2*dq2*dq2 ; -s2*dq1*dq1 ];
G = [3*g*c1 + g*c12 ; +g*c12 ];
ddQ = inv(M) * ( T + C + G );
end