clc;clear ;close all;
q1=pi/2;
q2=pi/2;
dq1=0;
dq2=0;
m1=1;
m2=2;
g=9.86;
L1=1;
L2=1;

dt=0.005;
x=[0 L1*sin(q1)];
x=[x,x(2)+L2*sin(q2)];
y=[0 -L1*cos(q1)];
y=[y,y(2)-L2*cos(q2)];
pid=plot(x,y,'o-');
ddq1=0; ddq2=0;

grid on
axis([-2 2 -2 2]);
daspect([1 1 1]);
for ii=0:0.01:20
q1=q1+dq1*dt;
q2=q2+dq2*dt;
dq1=dq1+ddq1*dt;
dq2=dq2+ddq2*dt;
gamy=(2*m1+m2-m2*cos(2*q1-2*q2));
ddq1=(-g*(2*m1+m2)*sin(q1)-m2*g*sin(q1-2*q2)-2*sin(q1-q2)*m2*(dq2^2*L2+dq1^2*L1*cos(q1-q2)))/gamy/L1;
ddq2=2*sin(q1-q2)*(dq1^2*L1*(m1+m2)+g*(m1+m2)*cos(q1)+dq2^2*L2*m2*cos(q1-q2))/gamy/L2;
x(2)=L1*sin(q1);
x(3)=x(2)+L2*sin(q2);
y(2)=-L1*cos(q1);
y(3)=y(2)-L2*cos(q2);

set(pid,'XData',x,'YData',y);

pause(0.05)

end