% Program 6.1
% RR robot arm
% Lagrange's e.o.m 

clear; clc; close;

syms t L1 L2  m1 m2 m3 g real

q1 = sym('q1(t)');
q2 = sym('q2(t)');

c1 = cos(q1); s1 = sin(q1);
c2 = cos(q2); s2 = sin(q2);

xB = L1*c1; yB = L1*s1; rB = [xB yB 0];
rC1 = rB/2; vC1 = diff(rC1,t)
xD = xB + L2*c2; yD = yB + L2*s2; rD = [xD yD 0];
rC2 = (rB + rD)/2; vC2 = diff(rC2,t)
omega1 = [0 0 diff(q1,t)]; 
omega2 = [0 0 diff(q2,t)]; 

IA = m1*L1^2/3; IC2 = m2*L2^2/12;

% kinetic energy of the link 1
T1 = IA*omega1*omega1.'/2

% .' array transpose 
% A.' is the array transpose of A

% kinetic energy of the link 2
T2 = m2*vC2*vC2.'/2 + IC2*omega2*omega2.'/2;
T2 = simple(T2);

% total kinetic energy
T = expand(T1 + T2);

% deriv(f, g(t)) differentiates f with respect to g(t)

% dT/d(dq)
Tdq1 = deriv(T, diff(q1,t));
Tdq2 = deriv(T, diff(q2,t));

% d(dT/d(dq))/dt
Tt1 = diff(Tdq1, t);
Tt2 = diff(Tdq2, t);

% dT/dq
Tq1 = deriv(T, q1);
Tq2 = deriv(T, q2);

% left hand side of Lagrange's eom
LHS1 = Tt1 - Tq1;
LHS2 = Tt2 - Tq2;

% generalized active forces

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

syms T01z T12z real

% contact torque of 0 that acts on link 1 
T01 = [0 0 T01z];

% contact torque of link 1 that acts on link 2 
T12 = [0 0 T12z];

% partial derivatives
rC1_1 = deriv(rC1, q1); rC2_1 = deriv(rC2, q1);
rC1_2 = deriv(rC1, q2); rC2_2 = deriv(rC2, q2);

w1_1 = deriv(omega1, diff(q1,t)); w2_1 = deriv(omega2, diff(q1,t));
w1_2 = deriv(omega1, diff(q2,t)); w2_2 = deriv(omega2, diff(q2,t));

% generalized active force Q1
Q1 = rC1_1*G1.'+w1_1*T01.'+w1_1*(-T12.')+rC2_1*G2.'+w2_1*T12.'

% generalized active force Q2
Q2 = rC1_2*G1.'+w1_2*T01.'+w1_2*(-T12.')+rC2_2*G2.'+w2_2*T12.'

% first Lagrange's equation of motion
Lagrange1 = LHS1-Q1;
% second Lagrange's equation of motion
Lagrange2 = LHS2-Q2;

% control torques
b01 = 450; g01 = 300;
b12 = 200; g12 = 300;

q1f = pi/6; 
q2f = pi/3;

T01zc = -b01*diff(q1,t)-g01*(q1-q1f)+0.5*g*L1*m1*c1+g*L1*m2*c1;
T12zc = -b12*diff(q2,t)-g12*(q2-q2f)+0.5*g*L2*m2*c2;

tor  = {T01z, T12z};
torf = {T01zc,T12zc};

Lagrang1 = subs(Lagrange1, tor, torf);
Lagrang2 = subs(Lagrange2, tor, torf);

data = {L1, L2, m1, m2, g};
datn = {1 , 1 , 1 , 1 , 9.81};
% L1 = 1; L2 = 1; m1 = 1; m2 = 1; g = 9.81; 

Lagran1 = subs(Lagrang1, data, datn);
Lagran2 = subs(Lagrang2, data, datn);

ql = {diff(q1,t,2), diff(q2,t,2), diff(q1,t), diff(q2,t), q1, q2};    
qf = {'ddq1', 'ddq2', 'x(2)', 'x(4)', 'x(1)', 'x(3)'};

% ql                    qf
%----------------------------
% diff('q1(t)',t,2) -> 'ddq1'
% diff('q2(t)',t,2) -> 'ddq2'
%   diff('q1(t)',t) -> 'x(2)'
%   diff('q2(t)',t) -> 'x(4)'
%           'q1(t)' -> 'x(1)'
%           'q2(t)' -> 'x(3)' 


Lagra1 = subs(Lagran1, ql, qf);
Lagra2 = subs(Lagran2, ql, qf);

% solve e.o.m. for ddq1, ddq2
sol = solve(Lagra1,Lagra2,'ddq1, ddq2');
Lagr1 = sol.ddq1;
Lagr2 = sol.ddq2;

% system of ODE 
dx2dt = char(Lagr1); 
dx4dt = char(Lagr2);

fid = fopen('RR_Lagr.m','w+'); 
fprintf(fid,'function dx = RR_Lagr(t,x)\n');
fprintf(fid,'dx = zeros(4,1);\n');
fprintf(fid,'dx(1) = x(2);\n');
fprintf(fid,'dx(2) = ');
fprintf(fid,dx2dt);
fprintf(fid,';\n');
fprintf(fid,'dx(3) = x(4);\n');
fprintf(fid,'dx(4) = ');
fprintf(fid,dx4dt);
fprintf(fid,';');
fclose(fid); 

% insert cell divider

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

x0 = [-pi/18 0 pi/6 0]; 

[t,xs] = ode45(@RR_Lagr, 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(@RR_Lagr,0:1:5,x0);

fprintf('Results \n'); fprintf('\n');
fprintf('    t(s)     q1(rad)  dq1(rad/s) q2(rad)  dq2(rad/s) \n'); 
[ts,xs]