function [ dX ] = GantryDynamics( X, F ) % gantryu dynamics (Sp25 version) % cart = 1.5kg % ball = 3kg % length = 1.5m % X = [x, q, dx, dq] x = X(1); q = X(2); dx = X(3); dq = X(4); g = -9.8; M = [4.5, 4.5*cos(q); 4.5*cos(q), 6.75]; A = [4.5*dq*dq*sin(q); 4.5*g*sin(q)]; B = [1;0]; d2X = inv(M) * (A + B*F); dX = [dx; dq; d2X]; end