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