% Program 6.2
% RR robot arm
% Lagrange's e.o.m 
% Inverse dynamics

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;
T1 = IA*omega1*omega1.'/2;
T2 = m2*vC2*vC2.'/2 + IC2*omega2*omega2.'/2;
T = expand(T1 + T2);
% deriv(f, g(t)) differentiates f with respect to g(t)
Tdq1 = deriv(T, diff(q1,t)); Tdq2 = deriv(T, diff(q2,t));
Tt1 = diff(Tdq1, t); Tt2 = diff(Tdq2, t);
Tq1 = deriv(T, q1); Tq2 = deriv(T, q2);
LHS1 = Tt1 - Tq1; LHS2 = Tt2 - Tq2;
% deriv(f, g(t)) differentiates f with respect to g(t)
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));
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];
Q1 = rC1_1*G1.'+w1_1*T01.'+w1_1*(-T12.')+rC2_1*G2.'+w2_1*T12.'
Q2 = rC1_2*G1.'+w1_2*T01.'+w1_2*(-T12.')+rC2_2*G2.'+w2_2*T12.'
Lagrange1 = LHS1-Q1; Lagrange2 = LHS2-Q2;
data = {L1, L2, m1, m2, g};
datn = {1 , 1 , 1 , 1 , 9.81};

Lagr1 = subs(Lagrange1, data, datn);
Lagr2 = subs(Lagrange2, data, datn);

% solve for T01z T12z
sol = solve(Lagr1,Lagr2,'T01z, T12z');
T01zc = sol.T01z;
T12zc = sol.T12z;

% INVERSE DYNAMICS

q1f = pi/6 ; q2f = pi/3;
q1s = pi/18; q2s = pi/6;

Tp=15.;

q1n = q1s + (q1f-q1s)/Tp*(t - Tp/(2*pi)*sin(2*pi/Tp*t));
q2n = q2s + (q2f-q2s)/Tp*(t - Tp/(2*pi)*sin(2*pi/Tp*t));

dq1n = diff(q1n,t);
dq2n = diff(q2n,t);

ddq1n = diff(dq1n,t);
ddq2n = diff(dq2n,t);
 
ql = {diff(q1,t,2), diff(q2,t,2), diff(q1,t), diff(q2,t), q1, q2};    
qn = {ddq1n, ddq2n, dq1n, dq2n, q1n, q2n};

% ql                    qn
%----------------------------
% diff('q1(t)',t,2) -> ddq1n
% diff('q2(t)',t,2) -> ddq2n
%   diff('q1(t)',t) ->  dq1n
%   diff('q2(t)',t) ->  dq2n
%           'q1(t)' ->   q1n
%           'q2(t)' ->   q1n 

T01zt = subs(T01zc, ql, qn);
T12zt = subs(T12zc, ql, qn);

time = 0:1:Tp;

T01t = subs(T01zt,'t',time);
T12t = subs(T12zt,'t',time);

subplot(2,1,1),plot(time,T01t),...
xlabel('t (s)'),ylabel('T01z (N m)'),grid,...

subplot(2,1,2),plot(time,T12t),...
xlabel('t (s)'),ylabel('T12z (N m)'),grid

%%

q_1t = subs(q_1n,'t',time);
q_2t = subs(q_2n,'t',time);

subplot(2,1,1),plot(time,q_1t*180/pi),...
xlabel('t (s)'),ylabel('q1 (deg)'),grid,...

subplot(2,1,2),plot(time,q_2t*180/pi),...
xlabel('t (s)'),ylabel('q2 (deg)'),grid