% Linearization of the acrobot
% without the torque

m1 = 1.0;
m2 = 1.0;
l1 = 1.0;
l2 = 1.0;

lc1 = l1/2.0;
lc2 = l2/2.0;
I1 = 1/12.0 * m1 * l1 * l1; % Inertia moment
I2 = 1.0/12.0 * m2 * l2 * l2; % Inertia moment 
g = 9.8;

syms q1 q2 dq1 dq2 t1 t2 q1b;

d11 = m1 * lc1^2 + m2*(l1^2 + lc2^2 + 2.0 * l1 * lc2 * cos(q2)) + I1 + I2;
d22 = m2 * lc2^2 + I2;
d12 = m2 * (lc2^2 + l1*lc2*cos(q2)) + I2;
d21 = m2 * (lc2^2 + l1*lc2*cos(q2)) + I2;

D = [d11 , d12;d21,d22];
h1 = -m2 * l1 * lc2 * sin(q2) * dq2^2 - 2 * m2*l1*lc2 * sin(q2)*dq2*dq1;
h2 = m2 * l1 * lc2 * sin(q2) * dq1^2;
phi1 = (m1 * lc1 + m2*l1) * g * cos(q1+pi/2.0) + m2 * lc2 * g * cos(q1+pi/2.0+q2);
phi2 = m2 * lc2 * g * cos(q1+pi/2.0 +q2);

ddq_notorque = D \ [-h1-phi1; -h2-phi2]; % the D \ [..]  computes inv(D)*[..]
ddq1_notorque = ddq_notorque(1);
ddq2_notorque = ddq_notorque(2);

% Now we can write the evolution function :
% [dq1 ; dq2 ; ddq1 ; ddq2] = f(q1,q2,dq1,dq2)

f = [dq1; dq2 ; ddq1_notorque ; ddq2_notorque];
J = jacobian(f, [q1,q2,dq1,dq2]);

% The linearization around X0 = [pi/2, 0, 0, 0] therefore reads :
% dx/dt = J * X
J=subs(J, q1 , 0);
J=subs(J, q2 , 0);
J=subs(J, dq1 , 0);
J=subs(J, dq2 , 0)

% We now compute the matrix with the torque
ddq_torque = D \ [0 ; t2];
f_torque = [dq1; dq2 ; ddq_torque];
f_torque=subs(f_torque, q1 , 0);
f_torque=subs(f_torque, q2 , 0);
f_torque=subs(f_torque, dq1 , 0);
f_torque=subs(f_torque, dq2 , 0)