First order optimality increases suddenly after decreasing

Hi, im trying to run a simulation for passive walking in which, the simulation integrates a set of non-linear diffrential equations given in the onestep function.

function [z,t]=onestep(z0,GL_DIM,steps)

M = GL_DIM(1);  mt = GL_DIM(2); ms = GL_DIM(3);   
L = GL_DIM(4);  b1 = GL_DIM(5); a1 = GL_DIM(6);   
b2 = GL_DIM(7);  a2 = GL_DIM(8); gam = GL_DIM(9);   
g = GL_DIM(10);

flag = 1;
if nargin<2
    error('need more inputs to onestep');
elseif nargin<3
    flag = 0; %send only last state
    steps = 1;
end

t0 = 0; 
dt = 5;
t_ode = t0;
z_ode = z0;

for i=1:steps
    options=odeset('abstol',2.25*1e-14,'reltol',2.25*1e-14,'events',@collision);
    tspan = linspace(t0,t0+dt,1000);
    tf = 2000;
    [t_temp1, z_temp0, tfinal1] = ode113(@ranger_ss_simplest_3link,tspan,z0,options,GL_DIM);
    
    zkneeplus=kneestrike_ss_simplest(t_temp1(end),z_temp0(end,:),GL_DIM);
    
    options=odeset('abstol',2.25*1e-14,'reltol',2.25*1e-14,'events',@collision2);
    
    [t_temp2, z_temp2, tfinal2] = ode113(@ranger_ss_simplest,[t_temp1(end),tf],zkneeplus,options,GL_DIM);

    zplus=heelstrike_ss_simplest(t_temp2(end),z_temp2(end,:),GL_DIM);
    
    t_temp = [t_temp1; t_temp2];
    z_temp = [z_temp0; z_temp2];
    
    z0 = zplus;
    t0 = t_temp(end);
    
    %%% dont include the first point
    t_ode = [t_ode; t_temp(2:end); t0];
    z_ode = [z_ode; z_temp(2:end,:); z0];
    
end

z = zplus;

if flag==1
   z=z_ode;
   t=t_ode;
end
end

Now this is called by the following function which compares, the output of the integrator for one step with the inital conditions and the objective is to find fixed points for the system of equations.

function zdiff=fixedpt(z0,GL_DIM)
zdiff=onestep(z0,GL_DIM)-z0; 
end

Now this fixedpt function is called by fsolve which ultimately solves for zeroes.

options = optimoptions('fsolve','TolFun',1e-6,'TolX',1e-6,'Display','iter','OptimalityTolerance',1e-4);
[zstar,fval,exitflag,output,jacob] = fsolve(@fixedpt,z0,options,GL_DIM);
if exitflag == 1
    disp('Fixed points are');
    zstar
else
    error('Root finder not converged, change guess or change system parameters')
end

Now the problem is that the fsolve function does not converge and the first order optimality jumps suddenly to very high values after decreasing for a few iterations.

                                         Norm of      First-order   Trust-region
 Iteration  Func-count     f(x)          step         optimality    radius
     0          7         0.78532                          1.32               1
     1          8         0.78532              1           1.32               1
     2         15        0.447137           0.25           1.05            0.25
     3         22        0.353895          0.625           19.5           0.625
     4         23        0.353895          0.625           19.5           0.625
     5         30       0.0553196        0.15625           2.06           0.156
     6         37       0.0019723       0.390625          0.575           0.391
     7         44     7.67467e-08       0.100611         0.0302           0.977
     8         45     7.67467e-08    2.05296e-05         0.0302           0.977
     9         52     3.21966e-08    5.13239e-06         0.0153        5.13e-06
    10         59      6.4253e-09    5.13239e-06        0.00355        5.13e-06
    11         60      6.4253e-09     1.2831e-05        0.00355        1.28e-05
    12         61      6.4253e-09    3.20774e-06        0.00355        3.21e-06
    13         62      6.4253e-09    8.01936e-07        0.00355        8.02e-07
    14         63      6.4253e-09    2.00484e-07        0.00355           2e-07
    15         70     6.02888e-09     5.0121e-08        0.00395        5.01e-08
    16         71     6.02888e-09    1.25302e-07        0.00395        1.25e-07
    17         72     6.02888e-09    3.13256e-08        0.00395        3.13e-08
    18         79     5.95822e-09     7.8314e-09        0.00416        7.83e-09
    19         80     5.95822e-09    1.95785e-08        0.00416        1.96e-08
    20         87     5.91039e-09    4.89463e-09        0.00436        4.89e-09
    21         88     5.91039e-09    1.22366e-08        0.00436        1.22e-08
    22         95     5.87801e-09    3.05914e-09        0.00455        3.06e-09
    23        102     5.77248e-09    7.64785e-09       4.12e+04        7.65e-09
    24        103     5.77248e-09    1.91196e-08       4.12e+04        1.91e-08
    25        104     5.77248e-09    4.77991e-09       4.12e+04        4.78e-09
    26        105     5.77248e-09    1.19498e-09       4.12e+04        1.19e-09
    27        106     5.77248e-09    2.98744e-10       4.12e+04        2.99e-10
    28        107     5.77248e-09    7.46861e-11       4.12e+04        7.47e-11
    29        108     5.77248e-09    1.86715e-11       4.12e+04        1.87e-11
    30        109     5.77248e-09    4.66788e-12       4.12e+04        4.67e-12
    31        110     5.77248e-09    1.16697e-12       4.12e+04        1.17e-12

Equation solved, fsolve stalled.

fsolve stopped because the relative size of the current step is less than the
selected value of the step size tolerance squared and the vector of function values
is near zero as measured by the selected value of the function tolerance.

<stopping criteria details>

Error using kneed_waker (line 42)
Root finder not converged, change guess or change system parameters

Now when i check the output of the integration for the first step the states work fine as they should and the final value of the step is close to the intial values, which should mean that the solver will get a solution at some point. What could be causing the jump in the first order optimality values? Thank you

[Matt J]

li fulin's comment moved here:

Hi, I met same problem with this, did you solve this? It would be grateful if u can share any solve method.

Answer 1

[Matt J]

What could be causing the jump in the first order optimality values?

A bad Jacobian calculation perhaps? Maybe you landed on or near a point of non-differentiability of onestep? To investigate, you could terminate the optimization after the 22nd iteration,

options = optimoptions('fsolve','MaxIterations',22,...
                       'TolFun',1e-6,'TolX',1e-6,'Display','iter','OptimalityTolerance',1e-4);

[zstar,fval,exitflag,output,jacob] = fsolve(@fixedpt,z0,options,GL_DIM);

and see if jacob contains any bizarre values. You should also be sure to follow guidelines posted here,

https://www.mathworks.com/help/optim/ug/optimizing-a-simulation-or-ordinary-differential-equation.html