Standard vs Augmented Lagrangian Method

Hey guys,

Can someone please explain to me what is going on in these 2 scripts with a little bit of details? Unfortunately i'm a bit time restrained :( Its about the Equations of Motion in Multi-body systems using the Standard lagrangian method and the Augmented lagrangian method (penalty method). I'm fairly new to Matlab, so excuse my "Noobness". Any help would be much appreciated :)

Standard

function analysis_s(t, u)

% Solve the constrained equations of motion at time t with the standard

% Lagrange multiplier method

include_global

% global Phi D DMiDt rhs g % for debugging purposes

Update_Position; Update_Velocity; Inertia_Array;

g = Force_array(t);

D = Jacobian; Dt = D';

rhsA = RHSAcc(t); % r-h-s of acc constraints (gamma)

% Phi = Constraints(t);

DMi = zeros(nConst,nB6);

for Bi=1:nB

ir = Bodies(Bi).ira; i3 = ir + 2; i4 = i3 + 1; i6 = i4 + 2;

DMi(:,ir:i3) = D(:,ir:i3)*Bodies(Bi).m_inv;

DMi(:,i4:i6) = D(:,i4:i6)*Bodies(Bi).J_inv;

end

% if cfriction == 1

% Dt = Coulomb(Dt); % include coulomb frictions

% end

DMiDt = DMi*Dt;

% if redund ~= 0

% DMiDt = DMiDt + ZZ; % revise for redundancies!

% end

Lambda = DMiDt\(rhsA - DMi*g);

gDtL = g + Dt*Lambda;

for Bi=1:nB

ir = Bodies(Bi).ira; i3 = ir + 2; i4 = i3 + 1; i6 = i4 + 2;

Bodies(Bi).r_dot2 = Bodies(Bi).m_inv*gDtL(ir:i3,1);

Bodies(Bi).w_dot = Bodies(Bi).J_inv*gDtL(i4:i6,1);

end

num = num + 1; % number of function evaluations

% Inform the user of progress

% Show the time once every 100 function evaluations

if mod(t10, 100) == 0

disp(t)

end

t10 = t10 + 1;

------------------------------------------------------------------

Augmented

function analysis_p(t, u)

% Solve the constrained equations of motion at time t with the augmented

% Lagrangian (or the penalty) method

include_global

% global Phi Phid D g % for debugging purposes

Update_Position;

Update_Velocity;

Inertia_Matrix;

g = Force_array(t);

D = Jacobian;

Phi = Constraints(t); % Evaluate constraints

rhsV = RHSVel(t); % r-h-s pf vel constraints

Phid = D*u(nB7+1:nB13) - rhsV; % Evaluate velocity constraints

rhsA = RHSAcc(t); % r-h-s of acc constraints (gamma)

% Augmented Lagrangian: nAug = 1 is the penalty method

acc = M\g;

Metc = M + mc*D'*D;

Detc = D'*(kc*Phi + dc*Phid - mc*rhsA);

for i = 1:nAug

acci = acc;

acc = Metc\(M*acc - Detc);

acc_norm = norm (acc - acci);

if acc_norm < 1.0e-5

% ibreak = i % test

break % Terminate execution of "for loop"

end

end

% Transfer accelerations to the body structure

for Bi=1:nB

ir = Bodies(Bi).ira; i3 = ir + 2; i4 = i3 + 1; i6 = i4 + 2;

Bodies(Bi).r_dot2 = acc(ir:i3,1);

Bodies(Bi).w_dot = acc(i4:i6,1);

end

num = num + 1; % number of function evaluations

% Inform the user of progress

% Show the time once every 100 function evaluations

if mod(t10, 100) == 0

disp(t)

end

t10 = t10 + 1;

-----------------------------------------------------------