function [ dX ] = Cart2Dynamics( X, F ) %cart dynamics (Sp22 Final) % cart = 1kg % ball 1: 1kg @ 1m % ball 2: 1kg @ 1.3 m % X = [x, q1, q2, dx, dq1, dq2] x = X(1); q1 = X(2); q2 = X(3); dx = X(4); dq1 = X(5); dq2 = X(6); g = 9.8; M = [3 cos(q1) 1.3*cos(q2); cos(q1) 1 0 ; 1.3*cos(q2) 0 1.3^2]; C = [sin(q1)*dq1*dq1 + 1.3*sin(q2)*dq2*dq2 ; 0 ; 0 ]; G = [0 ; -g*sin(q1) ; -1.3*g*sin(q2)]; B = [1 ; 0 ; 0]; d2X = inv(M) * (C + G + B*F); dX = [dx; dq1; dq2 ; d2X]; end