%-------------------------------------------------------------------------------------
% 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 
%-------------------------------------------------------------------------------------
L_aux = subs(L,{diff('ang1(t)'),'ang1(t)',diff('ang2(t)'),'ang2(t)'},{'w1','ang1','w2','ang2'});
x1_aux= subs(x1,{'ang1(t)','ang2(t)'},{'ang1','ang2'});
y1_aux= subs(y1,{'ang1(t)','ang2(t)'},{'ang1','ang2'});
x2_aux= subs(x2,{'ang1(t)','ang2(t)'},{'ang1','ang2'});
y2_aux= subs(y2,{'ang1(t)','ang2(t)'},{'ang1','ang2'});

%-------------------------------------------------------------------------------------
%-- Torque applied (by an external force) -> Given by the Jacobian Matrix
%-------------------------------------------------------------------------------------
T1_app = diff([x1_aux y1_aux],'ang1')*[Fx1; Fy1] + diff([x2_aux y2_aux],'ang1')*[Fx2; Fy2];
T2_app = diff([x1_aux y1_aux],'ang2')*[Fx1; Fy1] + diff([x2_aux y2_aux],'ang2')*[Fx2; Fy2];

%-------------------------------------------------------------------------------------
%-- Three terms: dKt/dqi + dKt/qi + dUt/qi
%-------------------------------------------------------------------------------------
J1 = diff(L_aux,'w1'); C1 = diff(L_aux,'ang1'); atrito1 = c1*w1+k1*ang1;
J2 = diff(L_aux,'w2'); C2 = diff(L_aux,'ang2'); atrito2 = c2*w2+k2*ang2;

%-------------------------------------------------------------------------------------
%-- Defining the kinetic and potential energy in a symbolic way
%-------------------------------------------------------------------------------------
T1 = diff( subs(J1) )-subs(C1)+subs(atrito1)-subs(T1_app);
T2 = diff( subs(J2) )-subs(C2)+subs(atrito2)-subs(T2_app);

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