%-------------------------------------------------------------------------------------
% File:	   Lagrange.m
%
% Author: 	Filipe Silva
% Date:		01-Abril,2004
% Purpose:	Compute the general equation of motion for a 3-dof robot
%-------------------------------------------------------------------------------------

%-------------------------------------------------------------------------------------
%-- Redefinition of variables
%-------------------------------------------------------------------------------------
w1 = diff(ang1); 
w2 = diff(ang2);
w3 = diff(ang3);

%-------------------------------------------------------------------------------------
%-- Symbolic substitution 
%-------------------------------------------------------------------------------------
K1_aux = subs(K1,{diff('ang1(t)', t),'ang1(t)',diff('ang2(t)', t),'ang2(t)',diff('ang3(t)', t),'ang3(t)'},{'w1','ang1','w2','ang2','w3','ang3'});
K2_aux = subs(K2,{diff('ang1(t)', t),'ang1(t)',diff('ang2(t)', t),'ang2(t)',diff('ang3(t)', t),'ang3(t)'},{'w1','ang1','w2','ang2','w3','ang3'});
K3_aux = subs(K3,{diff('ang1(t)', t),'ang1(t)',diff('ang2(t)', t),'ang2(t)',diff('ang3(t)', t),'ang3(t)'},{'w1','ang1','w2','ang2','w3','ang3'});
K_aux = K1_aux+K2_aux+K3_aux;

U1_aux = subs(U1,{'ang1(t)','ang2(t)','ang3(t)'},{'ang1','ang2','ang3'}); 
U2_aux = subs(U2,{'ang1(t)','ang2(t)','ang3(t)'},{'ang1','ang2','ang3'}); 
U3_aux = subs(U3,{'ang1(t)','ang2(t)','ang3(t)'},{'ang1','ang2','ang3'}); 
U_aux = U1_aux+U2_aux+U3_aux;

%-------------------------------------------------------------------------------------
%-- Three terms: dKt/dqi + dKt/qi + dUt/qi
%-------------------------------------------------------------------------------------
J1 = diff(K_aux,'w1'); C1 = diff(K_aux,'ang1'); G1 = diff(U_aux,'ang1');
J2 = diff(K_aux,'w2'); C2 = diff(K_aux,'ang2'); G2 = diff(U_aux,'ang2');
J3 = diff(K_aux,'w3'); C3 = diff(K_aux,'ang3'); G3 = diff(U_aux,'ang3');

%-------------------------------------------------------------------------------------
%-- Defining the kinetic and potential energy in a symbolic way
%-------------------------------------------------------------------------------------
T1 = diff( subs(J1) )-subs(C1)+subs(G1);
T2 = diff( subs(J2) )-subs(C2)+subs(G2);
T3 = diff( subs(J3) )-subs(C3)+subs(G3);

T1 = subs(T1, {diff(ang1, 2), diff(ang2, 2), diff(ang3, 2), diff(ang1, t), diff(ang2, t), diff(ang3, t), 'ang1(t)', 'ang2(t)', 'ang3(t)'}, {'alfa1', 'alfa2', 'alfa3', 'omega1', 'omega2', 'omega3', 'teta1', 'teta2', 'teta3'})
T2 = subs(T2, {diff(ang1, 2), diff(ang2, 2), diff(ang3, 2), diff(ang1, t), diff(ang2, t), diff(ang3, t), 'ang1(t)', 'ang2(t)', 'ang3(t)'}, {'alfa1', 'alfa2', 'alfa3', 'omega1', 'omega2', 'omega3', 'teta1', 'teta2', 'teta3'})
T3 = subs(T3, {diff(ang1, 2), diff(ang2, 2), diff(ang3, 2), diff(ang1, t), diff(ang2, t), diff(ang3, t), 'ang1(t)', 'ang2(t)', 'ang3(t)'}, {'alfa1', 'alfa2', 'alfa3', 'omega1', 'omega2', 'omega3', 'teta1', 'teta2', 'teta3'})

%-------------------------------------------------------------------------------------
%-- Simplifying the computed torques
%-------------------------------------------------------------------------------------
disp('###############################################################')
disp('# BINARIO T1:                                                 #')
T1 = simple( simplify(T1) ); pretty(T1)
disp('############################################### Press any key #')
%pause
disp('###############################################################')
disp('# BINARIO T2:                                                 #')
T2 = simple( simplify(T2) ); pretty(T2)
disp('###############################################################')
%pause
disp('###############################################################')
disp('# BINARIO T3:                                                 #')
T3 = simple( simplify(T3) ); pretty(T3)
disp('###############################################################')
