A_CG(1,1) = -CG*(ddq2*cos(q2)*sin(q3) + ddq3*cos(q3)*sin(q2) - dq2^2*sin(q2)*sin(q3) - dq3^2*sin(q2)*sin(q3) + 2*dq2*dq3*cos(q2)*cos(q3));
A_CG(2,1) = CG*(ddq2*sin(q2) + dq2^2*cos(q2));
A_CG(3,1) = CG*(ddq3*sin(q2)*sin(q3) - ddq2*cos(q2)*cos(q3) + dq2^2*cos(q3)*sin(q2) + dq3^2*cos(q3)*sin(q2) + 2*dq2*dq3*cos(q2)*sin(q3));