% state vector:  x = [q1; q2; v1; v2]

% coefficient of gravity
gravity = 9.8;

% inertia matrix
M = [3 + 2*cos(x(2)), 1+cos(x(2)); 1+cos(x(2)), 1];

% vector of Coriolis, centripetal and graviational forces
c(1,1) = x(4)*(2*x(3)+x(4))*sin(x(2)) + 2*gravity*sin(x(1)) + gravity*sin(x(1)+x(2));
c(2,1) = -x(3)^2*sin(x(2)) + gravity*sin(x(1)+x(2));
 
% hand position (forward kinematics)
y(1,1) = cos(x(1)+pi/2) + cos(x(1)+x(2)+pi/2);
y(2,1) = sin(x(1)+pi/2) + sin(x(1)+x(2)+pi/2);
