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

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

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

U1_aux = subs(U1,{'ang1(t)','ang2(t)'},{'ang1','ang2'}); 
U2_aux = subs(U2,{'ang1(t)','ang2(t)'},{'ang1','ang2'}); 
U_aux = U1_aux+U2_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');

%-------------------------------------------------------------------------------------
%-- 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);

%-------------------------------------------------------------------------------------
%-- 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('###############################################################')
