%% Example of the Lagrange equations of motion, fully automagically!
% Homework Assigment 5.
% wb1413, Multibody Dynamics B, Spring 2006.

%	Arend L. Schwab	23-March-2006
%	Copyright (c) 2006 by TU-Delft, the Netherlands.
clear

syms m g l 
syms u v 
syms ud vd 
syms udd vdd
syms fu fv

q = [u; v];
qd = [ud; vd];
qdd = [udd; vdd];
Q = [fu; fv];

I = 1/12*m*l^2;

% position centre of mass body 1
p1 = [l/2*cos(u);l/2*sin(u)]
% position centre of mass body 2
pB = [l*cos(u);l*sin(u)];
p2 = pB+[l/2*cos(u+v);l/2*sin(u+v)]

% Velocities of the bodies, just partial derivatives by definition.
p1d = jacobian(p1,q)*qd
p2d = jacobian(p2,q)*qd

% Kinetic energy, body 1 and 2
T1 = 1/2*m*p1d.'*p1d+1/2*I*ud^2;
T2 = 1/2*m*p2d.'*p2d+1/2*I*(ud+vd)^2;
% Totale Kinetische energie van het systeem
T = T1+T2

% Potential energy, only gravity
V = -m*g*p1(1)-m*g*p2(1)

T = simple(T)
pretty(T);

%% Lagrange equations of motion, allmost a one-liner!
dT = jacobian(T,qd);
eqn = jacobian(dT,qd)*qdd+jacobian(dT,q)*qd-jacobian(T,q).'+jacobian(V,q).'-Q
eqn = simple(eqn);
pretty(eqn);

%% Find the massmatrix by partial derivatives, there are other ways.
M = jacobian(eqn,qdd)
M = simple(M);
pretty(M);
%The righthand side is M*qdd minus the equations
rhs = M*qdd-eqn
rhs = simple(rhs);
pretty(rhs);
% Solve for the accelerations, not the most elegant way!
% You should never do this like this, when you have 
% a somewhat larger system this will result in WALLPAPER!
lhs = inv(M)*rhs;
lhs = simple(lhs);
pretty(lhs);

%% parameters for homework assignment 1
l0 = 0.50;
w0 = 0.05;
h0 = 0.005;
g0 = 9.81;
% specific mass 
rho0 = 1200;
% dependent values
A0 = w0*h0;
m0 = rho0*A0*l0;
% substitute the initial conditions and the parameter values
icvar = {u,v,ud,vd,fu,fv,m,g,l};
icval = {pi/2,0,0,0,0,0,m0,g0,l0};
qdd0 = subs(lhs,icvar,icval);
double(qdd0)

%% A far better approach is not to solve symbolically
% but first evaluate M and the rhs numerically and then
% solve for the accelerations numerically
M0 = double(subs(M,icvar,icval));
rhs0 = double(subs(rhs,icvar,icval));
lhs0 = M0\rhs0
% Do you see the difference in the last digits of the solution?
% Any idea why this is?