% Program 6.4
% RRT robot arm
% Inverse Dynamics 
% 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)'); q3 = sym('q3(t)');
c1 = cos(q1); s1 = sin(q1); c2 = cos(q2); s2 = sin(q2);
R10 = [[1 0 0]; [0 c1 s1]; [0 -s1 c1]];
R21 = [[c2 0 -s2]; [0 1 0]; [s2 0 c2]];
w10 = [diff(q1,t) 0 0 ];
w201 = [diff(q1,t) diff(q2,t) 0];
w20 = w201 * transpose(R21); 
rC1 = [0 0 L1];
vC1 = diff(rC1,t) + cross(w10, rC1);
rC2 = [0 0 2*L1]*transpose(R21) + [0 0 L2];
vC2 = simple(diff(rC2,t) + cross(w20,rC2));
rC3 = rC2 + [0 0 q3];
vC3 = simple(diff(rC3,t) + cross(w20,rC3));
vC32 = simple(vC2 + cross(w20,[0 0 q3]));
G1 = [-m1*g 0 0]; 	
G2 = [-m2*g 0 0]*transpose(R21);
G3 = [-m3*g 0 0]*transpose(R21);

syms T01x T01y T01z T12x T12y T12z F23x F23y F23z real
T01 = [T01x T01y T01z];
T12 = [T12x T12y T12z];
F23 = [F23x F23y F23z];

% deriv(f, g(t)) differentiates f with respect to g(t)
w1_1 = deriv(w10, diff(q1,t)); w2_1 = deriv(w20, diff(q1,t)); 
w1_2 = deriv(w10, diff(q2,t)); w2_2 = deriv(w20, diff(q2,t)); 
w1_3 = deriv(w10, diff(q3,t)); w2_3 = deriv(w20, diff(q3,t)); 

vC1_1 = deriv(vC1, diff(q1,t)); vC2_1 = deriv(vC2, diff(q1,t));
vC1_2 = deriv(vC1, diff(q2,t)); vC2_2 = deriv(vC2, diff(q2,t));
vC1_3 = deriv(vC1, diff(q3,t)); vC2_3 = deriv(vC2, diff(q3,t));

vC32_1 = deriv(vC32, diff(q1,t)); vC3_1 = deriv(vC3, diff(q1,t));
vC32_2 = deriv(vC32, diff(q2,t)); vC3_2 = deriv(vC3, diff(q2,t));
vC32_3 = deriv(vC32, diff(q3,t)); vC3_3 = deriv(vC3, diff(q3,t));

% generalized active forces
Q1 = w1_1*T01.' + vC1_1*G1.' + w1_1*transpose(R21)*(-T12.') + ...
     w2_1*T12.' + vC2_1*G2.' + vC32_1*(-F23.') + ...
     vC3_1*F23.' + vC3_1*G3.';

Q2 = w1_2*T01.' + vC1_2*G1.' + w1_2*transpose(R21)*(-T12.') + ...
     w2_2*T12.' + vC2_2*G2.' + vC32_2*(-F23.') + ...
     vC3_2*F23.' + vC3_2*G3.';

Q3 = w1_3*T01.' + vC1_3*G1.' + w1_3*transpose(R21)*(-T12.') + ...
     w2_3*T12.' + vC2_3*G2.' + vC32_3*(-F23.') + ...
     vC3_3*F23.' + vC3_3*G3.';
 
I1 = [m1*(2*L1)^2/12 0 0; 0 m1*(2*L1)^2/12 0; 0 0 0];
I2 = [m2*(2*L2)^2/12 0 0; 0 m2*(2*L2)^2/12 0; 0 0 0];
syms I3x I3y I3z real
I3 = [I3x 0 0; 0 I3y 0; 0 0 I3z];

T1 = (1/2)*m1*vC1*vC1.' + (1/2)*w10*I1*w10.';
T2 = (1/2)*m2*vC2*vC2.' + (1/2)*w20*I2*w20.';
T3 = (1/2)*m3*vC3*vC3.' + (1/2)*w20*I3*w20.';
T = expand(T1 + T2 + T3);

Tdq1 = deriv(T, diff(q1,t)); 
Tdq2 = deriv(T, diff(q2,t)); 
Tdq3 = deriv(T, diff(q3,t));

Tt1=diff(Tdq1, t); Tt2=diff(Tdq2, t); Tt3=diff(Tdq3, t);

Tq1=deriv(T, q1); Tq2=deriv(T, q2); Tq3=deriv(T, q3);

LHS1=Tt1 - Tq1; LHS2=Tt2 - Tq2; LHS3=Tt3 - Tq3;
 
Lagrange1=LHS1-Q1; Lagrange2=LHS2-Q2; Lagrange3=LHS3-Q3;

data = {L1, L2, I3x, I3y, I3z, m1, m2, m3, g};
datn = {0.4, 0.4, 5, 4, 1, 90, 60, 40, 9.81};

Lagra1 = subs(Lagrange1, data, datn);
Lagra2 = subs(Lagrange2, data, datn);
Lagra3 = subs(Lagrange3, data, datn);

sol = solve(Lagra1,Lagra2,Lagra3,'T01x, T12y, F23z');
T01xc = simple(sol.T01x);
T12yc = simple(sol.T12y);
F23zc = simple(sol.F23z);

q1s = pi/18; q2s = pi/6; q3s = 0.25;
q1f = pi/3 ; q2f = pi/3; q3f = 0.3;

Tp=15.;

q1t = q1s + (q1f-q1s)/Tp*(t - Tp/(2*pi)*sin(2*pi/Tp*t));
q2t = q2s + (q2f-q2s)/Tp*(t - Tp/(2*pi)*sin(2*pi/Tp*t));
q3t = q3s + (q3f-q3s)/Tp*(t - Tp/(2*pi)*sin(2*pi/Tp*t));

dq1t = diff(q1t,t);
dq2t = diff(q2t,t);
dq3t = diff(q3t,t);

ddq1t = diff(dq1t,t);
ddq2t = diff(dq2t,t);
ddq3t = diff(dq3t,t);

ql = {diff(q1,t,2), diff(q2,t,2), diff(q3,t,2), ...
      diff(q1,t), diff(q2,t), diff(q3,t), q1, q2, q3};    

qn = {ddq1t, ddq2t, ddq3t, dq1t, dq2t, dq3t, q1t, q2t, q3t};

T01xt = subs(T01xc, ql, qn);
T12yt = subs(T12yc, ql, qn);
F23zt = subs(F23zc, ql, qn);

time = 0:1:Tp;

T01t = subs(T01xt,'t',time);
T12t = subs(T12yt,'t',time);
F23t = subs(F23zt,'t',time);

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

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

subplot(3,1,3),plot(time,F23t),...
xlabel('t (s)'),ylabel('F23z (N)'),grid

fprintf('Results \n'); fprintf('\n');

fprintf('t(s) T01x(Nm) T12y(Nm) F23z(N) \n'); 

[time' T01t' T12t' F23t']

%%

q1g = subs(q1t,'t',time);
q2g = subs(q2t,'t',time);
q3g = subs(q3t,'t',time);

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

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

subplot(3,1,3),plot(time,q3g),...
xlabel('t (s)'),ylabel('q3 (m)'),grid