Multiple Input odefun in Ode113

Good morning,

I am trying to write a code that is able to solve a Multibody Simulation in Foward Dynamics. For this I selected to use the ODE113 for being a predictor-corrector algorithm (Adams-Bashforth-Moulton). However I am having issues in implementing the proper code due to the syntax or the incorrect usage of ode solvers functions which I am not familiar. My odesolver inputs and outputs are Position and Velocity of each body during the simulation.

In theory for each ode solver iteration, my anonymous function should:

• Calculate that iteration acceleration vector, using the position and velocity updated with the previous step integration. For this to happen it is necessary to use an algorithm that is inside the anonymous function of the odefun, and is in annexe to this question.

However, when I try to implement this, what is happening is that, my of t and y do not change, and the calculus is always the same during 0 to final time. My code is the following:

Sub function that calls the ode solver:

function [Bodies,Points,CoM,DynAcc,it,debugdata] = MBS_DynAnalysis(NBodies,Bodies,dynfunc,Joints,Forces,Points,CoM,TimeStep,Grav,SimType,UnitsSystem,it,driverfunctions,debugdata,ForceFunction,tini,RunTime)
%Calls the functions needed to solve the Foward Dynamic Problem
    % Prepares the initial conditions for the ode solver and calculates the
    % first acceleration for t=0, to store.
    [y0,t0,tf,DynAcc,Bodies] = InitialOdeSetup(NBodies,Bodies,tini,TimeStep,RunTime,dynfunc,Forces,Grav,UnitsSystem,ForceFunction,SimType);
    
    % Update of the variables (Stores t - Timestep)
    [Points,CoM,it] = DynDataStorage(Points,CoM,NBodies,Bodies,Joints,DynAcc,it);
    
    
    opts = odeset('RelTol',1e-6,'AbsTol',1e-6);
    [timevector,y] = ode113(@(t,y)DynOdefunction(t,y,NBodies,Bodies,dynfunc,Joints,Forces,Grav,SimType,UnitsSystem,driverfunctions,debugdata,ForceFunction),[t0:TimeStep:tf],y0,opts);
    [a,~] = size(timevector);
    
end

Ode Solver Anonymous function:

function [yd] = DynOdefunction(t,y,NBodies,Bodies,dynfunc,Joints,Forces,Grav,SimType,UnitsSystem,driverfunctions,debugdata,ForceFunction)
%This function uses the inputs of initial position, initial velocities and
%forces to calculate the initial acceleration that will be fed to the
%Integrator.

%% Update the Structs with the variables that are given by the odesolver.
    time = t;
% Update Position
    Bodies = UpdateBodyPostures(y(1:7*NBodies), NBodies, Bodies);


%% 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);
%% Velocity Update

    for i = 1:NBodies
       i2 = 7*(i-1) + 1 + 7*NBodies;
       Bodies(i).rd = y(i2:i2+2,1);
       pd = y(i2+3:i2+6,1);
       Bodies(i).w = 2*Bodies(i).L*pd;
    end
    
% 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; 
    
%% Define Augmented Lagrangian Penalty Parameters
    %Values taken from Paulo Flores Art on Constraints
    alpha = 1*10^7;
    omega = 10;
    mu = 1;

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

end

I am not entirely sure if I was able to expose my problem, if not I apolagize in advance. The problem itself is complex and hard to explain.

Thank you for your time and attention.

Tiago Carvalho

[Torsten]

So "yd" as output from "DynOdefunction" is always a vector of 7*NBodies zeros ?

[Tiago Carvalho]

Yes, I am working with quarternions só each body has 7 coordinates for position,velocity and acceleration. The struct of the results that I am obtaining is in annexe.

Thank you for your help.

Edit: The issue may also be from the code itslef, however, my objective here is to try and confirm that the error does not come from my ode solver problem syntax.

[Torsten]

I mean: is yd is a 7*NBodies vector of zeros ? Because your y-vector does not change in time during the integration ?

[Tiago Carvalho]

The initial conditions of yd are 0 yes. However, yd should change with time since the yd from the next step is the integration of the acceleration that was calculated.

Meaning that, yd0 is 0 but ydi+1 comes from the integration and the system itslef is subjected to gravity, so there is a force applied to it and the acceleration is non 0.

[Torsten]

If yd is not 0, y cannot remain at its initial value as you stated.

Did you check "timevector" after the call

[timevector,y] = ode113(@(t,y)DynOdefunction(t,y,NBodies,Bodies,dynfunc,Joints,Forces,Grav,SimType,UnitsSystem,driverfunctions,debugdata,ForceFunction),[t0:TimeStep:tf],y0,opts);

if ODE113 really finished integration ?

[Tiago Carvalho]

Yes I did check at the end with the debug I receive 3334 values for time, since i asked for those values in the t0:TimeStep:tf.

The syntax for the multiple inputs is correct? I used the syntax of Matlab documentation.

[Torsten]

The call to ODE113 looks correct.

And if you plot selected y components using

plot(timevector,y(:,1))

e.g. after

[timevector,y] = ode113(@(t,y)DynOdefunction(t,y,NBodies,Bodies,dynfunc,Joints,Forces,Grav,SimType,UnitsSystem,driverfunctions,debugdata,ForceFunction),[t0:TimeStep:tf],y0,opts); 

you get a constant graph ?

[Tiago Carvalho]

Hello Torsten,

Sorry for my late reply. I had to change some of the code in order to be able to produce the graphics that I wanted and it took me more time than what I expected since I had another deadline coming.

The graphics are all constant, I will leave them in the answer.

This is the Position of the body 3, and It should not be constant.

Here is the model I am trying to simulate. Body 3 represents the base of the damper and support that is free to move up and down. The model does not have the angular velocity, only dampers and gravity, I used another software to check what results should I have (MSC Adams) and that body should go progressively lower in y.

Here are the results I obtained with another algorithm ( which however, is not the best implementation and could get me trouble in my Thesis defense - due to the bad application of ode)

[Tiago Carvalho]

Hello Trosten,

Just want to tell you that I was able to fix the problem, it was on the linear solver. I tried to use a pinv to go around the lower RCOND numbers and that was causing the problem in the calculus for some reason. I am now able to gather results but with badly scaled warnings which I am trying to fix.

Anyway, I wanted to thank for the help you gave me in the last 2 answers I posted it helped me a lot!

Regards,

Tiago