Solving Linear Systems for Multibody Systems

Good afternoon,

I'm currently coding a Multibody Foward Dynamcis Simulator in MATLAB, for my master thesis, and I am having issues with solving linear systems due to low RCOND values, ill-condition of the matrices.

This problem is expected in this type of problems since each column entry in a line corresponds to the elements of a joint equation for two bodies resulting in a highly sparse matrix and consequently ill-conditioned matrices.

What my algorithm does is it calculates the initial accelerations of bodies in order for them to be integrated through an ode solver for position and velocity. My problems arise in solving the linear system for the initial accelerations that will be integrated. The system I have to solve is the following:

Where: M is a mass matrix, Phiq is a Jacobian, alpha/omega/mu are scalars, Phi, gamma and upsilon are vectors.

For my algorithm I tried using pinv(A)*B or lsqr(A,B), but I am not able to get the correct results (only get constant values from initial time until the last integration). On the other hand I get good results by using mldivide or \, but i get the following message:

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 1.394430e-19.

I wanted to know if there is anyway to avoid this issue, since I know it can cause significant noise in my results, I usually use pinv for this but it seems to not be working this time.

Thank you for your time and attention.

Tiago

(Attached files are the left and right hand side of the linear problem)

[Sam Chak]

@Tiago Carvalho, unsure how to deal with this problem. Have you tried performing some kind of matrix transformation so that it is no longer ill-conditioned?

[Tiago Carvalho]

Hello, I tried to use svd but it really didn't work, also tried lsqminnorm, but I think the solution I am looking for is not the one with the minnorm just like pinv does. I am starting to think that traditional algebra tools wont do it and I'm trying to research in papers or thesis. Nevertheless, I posted the question to see if someone had any other idea.

Regards,

Tiago

[Torsten]

MATLAB's ODE solvers are designed to solve systems of the form

M(t,y)*y' = f(t,y)

Why do you invert your matrix in the ODE function routine and supply M^(-1)*f and don't let the ODE solver do the job by defining two functions in which you separately define the mass matrix M(t,y) and the right-hand side vector f(t,y) ?

[Tiago Carvalho]

Hello Torsten,

I don't think I can do that in this case in particular, since my algorithm follows the flowchart in the Picture. The process of this function it self is iterative inside the ode, in order to ensure that the constraints of my specific problem are solved, I don't see how I could integrate that outside the ode to allow him to do the inversion.

(EDIT) What the ode integrates is the product of the equation that I previously shared and the velocity that is an input of the odefun.

Answer 1

[Bjorn Gustavsson]

As best I can interpret your flow-chart it seems that it ought to be "reasonably straightforward" to follow Torsten's advice by converting the equation for into one for both and . If I get it right it should be something like:

function Mout = modified_massmatrix(M)
  Mout = [eye(size(M,1)),zeros(size(M));
          zeros(size(M)),M];
end

or if the mass-matrix is a function of t, q and :

function Mout = modified_massmatrix(t,qqdot,M)
  Mnow = M(t,qqdot);
  Mout = [eye(size(Mnow,1)),zeros(size(Mnow));
          zeros(size(Mnow)),Mnow];
end

Then you should be good to go with the ODE-integrating functions.

[Tiago Carvalho]

Hello,

I will try this way, because honestly I am not having any other idea. I was currently reading the Reg Tools matlab manual to see if I could apply some type of regularization to the system before the integration.

The only thing, is that I don't know if it will be possible to just feed the ode solver those matrices as you and trosten suggested. Since inside the the ode solver I have a while loop that is reponsible to solve the equation given in the first cooment, this is because the process in ALF is iterative for each ode solver iteration.

Here is my code: (I am sorry if I wasn't 100% clear, the problem is still a bit comples)

Anyway thank you for all your help and suggestions!

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;
%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,coord);

%% 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';
    deltamax = 1;
    qddi = qdd0;
    lagit = 1; % Augmented Lagrangian Formula Iteration Number
    lhslag = massmatrix + Jacobian'*alpha*Jacobian;
    % Flags to retrieve gamma
    Flags.Position = 0;
    Flags.Jacobian = 0;
    Flags.Velocity = 1;
    Flags.Acceleration = 1;
    Flags.Dynamic = 0;
    Flags.AccelDyn = 0;
    while deltamax > 1e-3 % Second Condition to avoid infinite loops
        if lagit > 1
           qddi = qddi1; 
        end
        [~,~,niu,gamma] = PJmatrixfunct(Flags,Bodies,NBodies,Joints,debugdata,driverfunctions,coord);
        rhslag = massmatrix*qddi + Jacobian'*alpha*(gamma - 2*omega*mu*(Jacobian*qd - niu) - omega^2*fun);
        qddi1 = gaussianelim(lhslag,rhslag);
        beta = alpha*(Jacobian*qddi1 - gamma + 2*omega*mu*(Jacobian*qd - niu) + omega^2*fun);
        %betamax = abs(max(beta));
        qdd = qddi1;
        deltaqdd = qddi1 - qddi;
        deltamax = abs(max(deltaqdd));
        [Bodies] = UpdateAccelerations(qddi1,NBodies,Bodies,SimType,alf);
        lagit = lagit + 1; %Iteration Counter
    end

    yd = [qd;qdd];

end

[Torsten]

Since inside the the ode solver I have a while loop that is reponsible to solve the equation given in the first cooment, this is because the process in ALF is iterative for each ode solver iteration.

ODE solvers in MATLAB can cope with differential-algebraic systems of equations. Thus your iterative process to update accelerations shouldn't be necessary because they can be solved within the system itself. Just define the corresponding rows of M to be zero and define the algebraic equations at the corresponding positions in the f-vector of the right-hand side of your system.

[Sam Chak]

@Tiago Carvalho, is this the source of the ill-conditioned thing?

[Bjorn Gustavsson]

The only place I've encountered this type of problem is where I've tried to integrate equations of motion for chains falling and behaving peculiarly using cartesian coordinates - then I had to try to do a lot of extra stuff (which I failed to complete). Instead I re-parameterized the problem using the Lagrangian formalism to express the equations of motion in more convenient sets of coordinates (mostly angles). If that is similar to your problem that might be what you could do if this is not what you already have.

In situations like these I've also been tremendously helped by reducing the problem complexity to a small enough similar problem that I managed to handle - just to get used with the problem-type and get some understanding of what is going on.

[Tiago Carvalho]

Hello @Sam Chak,

I am not sure if that would be the issue of ill-condition, I used the pinv because the \ was already giving me a RCOND warning, I'll look into it!

Thank you!

[Sam Chak]

@Tiago Carvalho Did you derive this multibody system via Kane's method?

[Tiago Carvalho]

Hello @Bjorn Gustavsson,

Yes my issue is very similar with what you have described! What I am trying to solve is the equations of motion for constrained multibody systems in relation to the bodies accelerations. Where i use the Lagragian method:

However, these equations of motion are only dependent on the Lagrangian Mutipliers and in the Acceleration vector, and do not explicitly use the position equations, written using kinematic constraints. This leads to an accumulation of a quadratic error in the position solution (the integration of velocity qd).

The only way I can avoid this is if I implement a "stabilization constraints algorithm" (like a feedback loop in control theory), in this case I use the Augmented Lagrange Formula, a penalty method, to stabilize the constraints in each iteration ( the while loop).

Nevertheless, I will try to implement th suggestions that you gave me to see if I can solve my issues. I have already follow your advise in simplifying the problem and I have reduce it to a basic one (a body which actuated only by gravity) and in this case I don't have any issues, so I think maybe the problem comes when I introduce other variables to the problem (like springs and dampers, which are part of the g vector).

Anyway, thank you for your help. I understand that probably I haven't been 100% explicit in my questions, but I think you can understand that my problem is a bit complex and is difficult to me to explain these in simpler terms when I have been working on this code for months already. For this I want to apologize.

Thank you again for your help, it is much appreciated.

Tiago Carvalho

[Tiago Carvalho]

Hello @Sam Chak, no I use the standard Newton-Euler's equation and Lagrange Method, not Kane's.

My main source of information was the P.E Nikravesh book titles Computer Aided Analysis of Mechanical Systems, which suggested the use of the Newton-Euler's, to be 100% honest I am not familiriazed with Kane's method but I will research it to see if it can simplify my issues!