% L1 = 1;
% L2 = 1;
% L1m = .6;
% L2m = 1;
% m1 = 1;
% m2 = 2;

syms theta1 theta2 dtheta1 dtheta2 L1 L2 L1m L2m m1 m2 g tau

% Variable theta1 is the absolute angle to the first link, measured
% ccw wrt the x axis. Theta2 is the relative angle of the second link,
% measure ccw wrt to first link.
%
% tau is the torque applied at the "elbox" from link 1 to link 2.

x1 = L1m*cos(theta1); % define geometry
y1 = L1m*sin(theta1);
dx1 = fulldiff(x1,theta1)  % full derivative (wrt time)
dy1 = fulldiff(y1,theta1)

x2 = L1*cos(theta1) + L2m*cos(theta1+theta2);
y2 = L1*sin(theta1) + L2m*sin(theta1+theta2);
dx2 = fulldiff(x2,theta2)
dy2 = fulldiff(y2,theta2)

% Now, define the KINETIC CO-ENERGY, which is equal to the 
% kinetic energy:
Tstar = (1/2)*m1*(dx1^2 + dy1^2) + (1/2)*m2*(dx2^2 + dy2^2)
% ...and the POTENTIAL ENERGY, due to gravity:
V = m1*g*y1 + m2*g*y2
% The Lagrangian is "Tstar - V"
L = Tstar - V

vars = {theta1, theta2}
Eqn1_LHS = fulldiff(diff(L,dtheta1),vars) - diff(L,theta1)
Eqn2_LHS = fulldiff(diff(L,dtheta2),vars) - diff(L,theta2)

Eqn1_RHS = 0; % no forces!
Eqn2_RHS = tau; % torque to link 2, "relative to" link 1
