function dX = twolink_ode(t,X)

theta1 = X(1);
theta2 = X(2);
dtheta1 = X(3);
dtheta2 = X(4);

L1 = 1;
L2 = 1;
L1m = .6;
L2m = 1;
m1 = 1;
m2 = 2;
g = 9.81;

for d2theta1 = [0, 1]
    sum1(d2theta1+1) = (d2theta1*m1*(2*L1m^2*cos(theta1)^2 + ...
        2*L1m^2*sin(theta1)^2))/2 + ...
        g*m2*(L2m*cos(theta1 + theta2) + L1*cos(theta1)) + ...
        L1m*g*m1*cos(theta1);
end
a1 = sum1(2)-sum1(1);
b1 = sum1(1);
c1 = 0; % Eqn1_RHS
% a1*d2theta1 + b1 = c1;
d2theta1 = (c1-b1)/a1;

for d2theta2 = [0, 1]
    sum2(d2theta2+1) = (d2theta2*m2*(2*L2m^2*cos(theta1 + theta2)^2 + ...
        2*L2m^2*sin(theta1 + theta2)^2))/2 + ...
        L2m*g*m2*cos(theta1 + theta2);
end
a2 = sum2(2)-sum2(1);
b2 = sum2(1);
% Control law 1: slowly swing up
theta2_des = sign(dtheta1)*(40*pi/180);
if mod(theta1,2*pi)<pi
    theta2_des = -.1*theta2_des;
end
tau = 60*(theta2_des-theta2) + 3*(0-dtheta2);
% Control law 2: elbow bending
%theta2_des = -sign(dtheta1)*(60*pi/180);
%tau = 30*(theta2_des-theta2) + 5*(0-dtheta2);
c2 = tau; % Eqn2_RHS
% a2*d2theta2 + b2 = c2;
d2theta2 = (c2-b2)/a2;

dX = [dtheta1; dtheta2; d2theta1; d2theta2];


