X0 = [-pi/2;0.01;0;0];
[tu,xu] = ode45(@twolink_ode,[0 10],X0);
figure(1); clf
n=1;
[p1,p2,m1,m2] = twolink_ani(xu(n,1),xu(n,2));
dt = .02;
for tsim = 0:dt:max(tu)
    theta1 = interp1(tu,xu(:,1),tsim);
    theta2 = interp1(tu,xu(:,2),tsim);
    [p1,p2,m1,m2] = twolink_ani(theta1,theta2,p1,p2,m1,m2);
    drawnow
    pause(.015)
end
