Update of Variables during the use of ODE113

2 views (last 30 days)
Good afertnoon,
I am trying to implement a piece of code to solve Multibody Dynamics Simualtions. I pretend to use the the solver ode113, but I am having some trouble on how to manage the variables.
My question is the following, at each time step/integration of the solver, I have to update my values of position and velocity, this update should be done inside the odefun after I give yd or at the start of the function? Is this update of the variables going to affect the accuracy of the odesolver? This is, will it update with the predictor steps or only after the tolerances are met?
This is my current code for the odefun (which is not 100% finished due to my doubts):
%% Set Up of the variables needed to construct the Matrix - Dynamic Modified Jacobian
% Pre Allocation of the q0 vector, for the calc of AGL
q0 = CreateAuxiliaryBodyStructure(NBodies,Bodies);
% Calculation of the Matrixes A, G and L
Bodies = DynCalcAGL(q0,NBodies,Bodies);
% Calculus of the dofs
coord = 7;
[debugdata] = SystemDofCalc(NBodies,Joints,debugdata,[],coord);
% Calculus of the G and L matrices derivatives
[Bodies] = DynIAGdLdcalc(NBodies,Bodies);
% Change w field to allow Kinematic Functions to Run on the RHS Accel (Can
% be Optimized)
for i = 1:NBodies
Bodies(i).wl = Bodies(i).w;
end
%% Assembly of the Invariable Matrices in the Augmented Lagrangian Formula.
% The process to obtain Position Matrix will be separated from the
% Jacobian to avoid funcount conflitcs
%Position Matrix
Flags.Position = 1;
Flags.Jacobian = 0;
Flags.Velocity = 0;
Flags.Acceleration = 0;
Flags.Dynamic = 0;
Flags.AccelDyn = 0;
[fun,~,~,~] = PJmatrixfunct(Flags,Bodies,NBodies,Joints,debugdata,driverfunctions,coord);
%Jacobian Matrix
Flags.Position = 0;
Flags.Jacobian = 1;
Flags.Velocity = 0;
Flags.Acceleration = 0;
Flags.Dynamic = 0;
Flags.AccelDyn = 0;
[~,Jacobian,~,~] = PJmatrixfunct(Flags,Bodies,NBodies,Joints,debugdata,driverfunctions,coord);
%% Mass Matrix Assembly for Euler Parameters
massmatrix = zeros(7*NBodies,7*NBodies); %Pre-Allocation
for i = 1:NBodies
Mass = Bodies(i).Mass;
B = Bodies(i).L;
Inertia = Bodies(i).Inertia;
Irat = 4*B'*diag(Inertia)*B; %Nikra Article on the use of EP for 3D Dynamics
i1 = 7*(i-1)+1;
massmatrix(i1:i1+2,i1:i1+2) = Mass * eye(3);
massmatrix(i1+3:i1+6,i1+3:i1+6) = Irat;
end
%% Definition of the Velocity Vector from 6 coordinates to 7 coordinates
for i = 1:NBodies
i1 = 7*(i-1)+1;
qd(i1:i1+2,1) = Bodies(i).rd;
pd = 0.5*Bodies(i).L'*Bodies(i).w;
pd = Impose_Column(pd);
qd(i1+3:i1+6,1) = pd;
end
%% Function Responsible for the Force Vectors
[vetorg] = ForceCalculus(Forces,NBodies,Bodies,dynfunc,Grav,UnitsSystem,time,ForceFunction);
%% Solving First Iteration to start the Augmented Process
[dim1,dim2] = size(massmatrix);
wogmass = massmatrix(8:dim1,8:dim2);
[dim3,~] = size(vetorg);
wogvetor = vetorg(8:dim3,1);
qddwog = pinv(wogmass)*wogvetor;
[dim4,~] = size(qddwog);
qdd0(8:dim4+7,1) = qddwog;
%% Solving the Augmented Lagrangian Formula Iterative Formula
alf = 'alfon';
beta = 1;
qddi = qdd0;
lagit = 1; % Augmented Lagrangian Formula Iteration Numver
% Flags to retrieve gamma
Flags.Position = 0;
Flags.Jacobian = 0;
Flags.Velocity = 1;
Flags.Acceleration = 1;
Flags.Dynamic = 0;
Flags.AccelDyn = 0;
while beta > 1e-6 % Second Condition to avoid infinite loops
if lagit > 1
qddi = qddi1;
end
[~,~,niu,gamma] = PJmatrixfunct(Flags,Bodies,NBodies,Joints,debugdata,driverfunctions,coord);
lhslag = massmatrix + Jacobian'*alpha*Jacobian;
rhslag = massmatrix*qddi + Jacobian'*alpha*(gamma - 2*omega*mu*(Jacobian*qd - niu) - omega^2*fun);
qddi1 = pinv(lhslag)*rhslag;
beta = alpha*(Jacobian*qddi1 - gamma + 2*omega*mu*(Jacobian*qd - niu) + omega^2*fun);
[Bodies] = UpdateAccelerations(qddi1,NBodies,Bodies,SimType,alf);
lagit = lagit + 1; %Iteration Counter
end
yd = [qd;qddi1];
  9 Comments
Torsten
Torsten on 15 Jul 2022
As far as I can see from your code, solutions of previous time steps are never invoked.
RKCalcDyn3D works with the actual values for t and y.

Sign in to comment.

Answers (0)

Community Treasure Hunt

Find the treasures in MATLAB Central and discover how the community can help you!

Start Hunting!