Solving Linear Systems for Multibody Systems

Question

Tiago Carvalho 2022-7-21

0
链接

此问题的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1765110-solving-linear-systems-for-multibody-systems

评论： Tiago Carvalho 2022-7-22

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)

4 个评论
显示 2更早的评论隐藏 2更早的评论

Torsten 2022-7-21

编辑：Torsten 2022-7-21

在 MATLAB Online 中打开

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 2022-7-21

编辑：Tiago Carvalho 2022-7-21

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 2022-7-22

0
链接

此回答的直接链接

https://ww2.mathworks.cn/matlabcentral/answers/1765110-solving-linear-systems-for-multibody-systems#answer_1012840

在 MATLAB Online 中打开

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.

8 个评论
显示 6更早的评论隐藏 6更早的评论

Tiago Carvalho 2022-7-22

在 MATLAB Online 中打开

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

Tiago Carvalho 2022-7-22

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 2022-7-22

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!

请先登录，再进行评论。

Solving Linear Systems for Multibody Systems

4 个评论
显示 2更早的评论隐藏 2更早的评论

采纳的回答

8 个评论
显示 6更早的评论隐藏 6更早的评论

更多回答（0 个）

另请参阅

类别

标签

Community Treasure Hunt

Solving Linear Systems for Multibody Systems

4 个评论 显示 2更早的评论隐藏 2更早的评论

采纳的回答

8 个评论 显示 6更早的评论隐藏 6更早的评论

更多回答（0 个）

另请参阅

类别

标签

Community Treasure Hunt

4 个评论
显示 2更早的评论隐藏 2更早的评论

8 个评论
显示 6更早的评论隐藏 6更早的评论