# Multiple Input odefun in Ode113

2 views (last 30 days)
Tiago Carvalho on 16 Jul 2022
Commented: Tiago Carvalho on 21 Jul 2022
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
Tiago Carvalho on 21 Jul 2022
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