% Program 5.3
% Double pendulum
% Dynamics 
% Newton-Euler equations of motion

clear; clc; close;

L1 = 1; L2 = 0.5; m1 = 1; m2 = 1; g = 10;  

t = sym('t','real');

xB = L1*cos(sym('q1(t)'));
yB = L1*sin(sym('q1(t)'));
rB = [xB yB 0];
rC1 = rB/2;
vC1 = diff(rC1,t); % differentiates rC1 with respect to t
aC1 = diff(vC1,t);

xD = xB + L2*cos(sym('q2(t)'));
yD = yB + L2*sin(sym('q2(t)'));
rD = [xD yD 0];
rC2 = (rB + rD)/2;
vC2 = diff(rC2,t);
aC2 = diff(vC2,t);

omega1 = [0 0 diff('q1(t)',t)];
alpha1 = diff(omega1,t);
omega2 = [0 0 diff('q2(t)',t)];
alpha2 = diff(omega2,t);

G1 = [0 -m1*g 0];
G2 = [0 -m2*g 0];

IC1 = m1*L1^2/12;
IA = IC1 + m1*(L1/2)^2;
IC2 = m2*L2^2/12;

% LINK 2
% Sum F for link 2:
% -m2 aC2 + G2 + (-F21) = 0 => F21 = -m2 aC2 + G2
F21 = -m2*aC2 + G2;

% LINK 1
% Sum M for 1 wrt A:"
% -IA alpha1 + AB x F21 + AC1 x G1 = 0
EqA = -IA*alpha1 + cross(rB, F21) + cross(rC1, G1);

% LINK 2
% Sum M for 2 wrt C2:
% -IC2 alpha2 + C2B x (-F21) = 0
Eq2 = -IC2*alpha2 + cross(rB - rC2, -F21);

% list for the symbolical variables 
slist={diff('q1(t)',t,2),diff('q2(t)',t,2),...
       diff('q1(t)',t),diff('q2(t)',t),'q1(t)','q2(t)'};
nlist = {'ddq1', 'ddq2', 'x(2)', 'x(4)', 'x(1)','x(3)'};
% diff('q1(t)',t,2) will be replaced by 'ddq1'
% diff('q2(t)',t,2) will be replaced by 'ddq2'
% diff('q1(t)',t) will be replaced by 'x(2)'
% diff('q2(t)',t) will be replaced by 'x(4)'
% 'q1(t)' will be replaced by 'x(1)'
% 'q2(t)' will be replaced by 'x(3)'

eq1 = subs(EqA(3),slist,nlist);
eq2 = subs(Eq2(3),slist,nlist);

sol = solve(eq1,eq2,'ddq1, ddq2');

dx1 = sym('x(2)');
dx2 = sol.ddq1;
dx3 = sym('x(4)');
dx4 = sol.ddq2;

dx1dt = char(dx1); 
dx2dt = char(dx2);
dx3dt = char(dx3); 
dx4dt = char(dx4);

g = inline(sprintf('[%s;%s;%s;%s]', dx1dt, dx2dt, dx3dt, dx4dt),'t','x');

t0 = 0;  tf = 5; time = [0 tf];

x0 = [-pi/4; 0; -pi/3; 0]; % initial conditions

[t,xs] = ode45(g, time, x0);

x1 = xs(:,1); 
x2 = xs(:,2);
x3 = xs(:,3); 
x4 = xs(:,4);  

subplot(2,1,1),plot(t,x1*180/pi,'r'),...
xlabel('t (s)'),ylabel('q1 (deg)'),grid,...
subplot(2,1,2),plot(t,x3*180/pi,'b'),...
xlabel('t (s)'),ylabel('q2 (deg)'),grid

%[ts,xs] = ode45(g,0:1:5,x0); 
%[ts,xs]