Update of Variables during the use of ODE113

2 次查看（过去 30 天）

Tiago Carvalho 2022-7-15

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1760875-update-of-variables-during-the-use-of-ode113

评论： Torsten 2022-7-15

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];
    
    