Fixes for spontaneous switch via initialization (#54)

* fixing print_iterates

* don't always print iterates

* main switching cases with init

* update example

* update instructions to only add src

---------

Co-authored-by: Anton Edvinovich Pozharskiy <apozharski@gmail.com>

README.md

@@ -42,9 +42,9 @@ For `CasADi` installation instructions follow visit: https://web.casadi.org/get/
 git clone https://github.com/nurkanovic/nosnoc.git
 ```
 
-3. Open the `NOSNOC` folder in Matlab and add it with subdirectories to the path by running:
+3. Open the `NOSNOC` folder in Matlab and add `src` with subdirectories to the path by running:
 ```
->> addpath(genpath(pwd))
+>> addpath(genpath(fullfile(pwd, 'src')))
 ```
 
 Note that `IPOPT` is shipped with `CasADi`. More information including detailed documentation can be found on its [homepage](https://coin-or.github.io/Ipopt/ )

examples/different_switching_cases/main_switching_cases.m

@@ -35,7 +35,7 @@
 % 2) sliding mode
 % 3) sliding on a surfce of disconinuity where a spontaneous switch can happen (nonuqnie solutions)
 % 4) unique leaving of a sliding mode
-switching_case = 'crossing';
+switching_case = 'spontaneous_switch';
 %  Options: 'crossing' 'sliding_mode', 'spontaneous_switch' , 'leave_sliding_mode', 
 %% NOSNOC settings
 problem_options = NosnocProblemOptions();
@@ -82,7 +82,7 @@
         model.F = [f_1 f_2];
         % implicit methods more accurate, explicit Euler enables "random"
         % leaving
-        problem_options.irk_scheme = 'EXPLICIT_RK';
+        problem_options.irk_scheme = IRKSchemes.RADAU_IIA;
         problem_options.n_s = 1;
         problem_options.N_finite_elements = 3; % set 4, 5 for different outcomes
     case 'leave_sliding_mode'
@@ -104,8 +104,15 @@
 solver_options.store_integrator_step_results = 1;
 
 integrator = NosnocIntegrator(model, problem_options, solver_options, [], []);
+
+if strcmp('spontaneous_switch', switching_case)
+    % initialize solver
+    theta_init = {[1, 0]};
+    integrator.solver.set('theta', theta_init);
+end
 [results,stats] = integrator.solve();
 
+
 figure
 latexify_plot()
 plot(results.t_grid,results.x(1,:))

src/NosnocIntegrator.m

@@ -174,7 +174,7 @@
                 %% handle failure -> try second initialization
                 if solver_stats.converged == 0
                     disp(['integrator_fesd: did not converge in step ', num2str(ii), 'constraint violation: ', num2str(solver_stats.constraint_violation, '%.2e')])
-                    % solver.print_iterate(sol.W(:,end))
+                    solver.print_iterate(res.W(:,end))
                     if problem_options.dcs_mode == "CLS"
                         disp('provided initial guess in integrator step did not converge, trying anther inital guess.');
                         solver.set('Lambda_normal', {7});

src/NosnocSolver.m

@@ -155,12 +155,10 @@ function set(obj, type, val)
         end
 
         function print_infeasibility(obj, results)
-            % warning('nosnoc:homotopy_solver:NLP_infeasible', 'NLP infeasible: try different mpcc_mode or check problem functions.');
             if obj.solver_options.print_details_if_infeasible
                 print_problem_details(results,obj.model,obj.problem, []);
             end
             if obj.solver_options.pause_homotopy_solver_if_infeasible
-                %             error('nosnoc: infeasible problem encounterd - stopping for debugging.')
                 keyboard
             end
         end
@@ -257,24 +255,24 @@ function print_iterate(obj, iterate, only_violations, filename)
             end
             fileID = 1;
             fprintf(fileID, "\nw\t\t\tlbw\t\tubw\titerate\n");
-            for i = 1:length(obj.problem.lbw)
-                if ~only_violations || (iterate(i) < obj.problem.lbw(i) || iterate(i) > obj.problem.ubw(i))
-                    expr_str = pad(formattedDisplayText(obj.problem.w(i)), 20);
-                    lb_str = pad(sprintf('%.2e', obj.problem.lbw(i)), 10);
-                    ub_str = pad(sprintf('%.2e', obj.problem.ubw(i)), 10);
+            for i = 1:length(obj.nlp.lbw)
+                if ~only_violations || (iterate(i) < obj.nlp.lbw(i) || iterate(i) > obj.nlp.ubw(i))
+                    expr_str = pad(formattedDisplayText(obj.nlp.w(i)), 20);
+                    lb_str = pad(sprintf('%.2e', obj.nlp.lbw(i)), 10);
+                    ub_str = pad(sprintf('%.2e', obj.nlp.ubw(i)), 10);
                     iterate_str = pad(sprintf('%.2e', iterate(i)), 10);
                     fprintf(fileID, "%s\t%s\t%s\t%s\n", expr_str, lb_str, ub_str, iterate_str);
                 end
             end
 
             % constraints
-            g_val = full(obj.problem.g_fun(iterate, obj.p_val));
+            g_val = full(obj.nlp.g_fun(iterate, obj.p_val));
             fprintf(fileID, "\ni\tlbg\t\t ubg\t\t g_val\t\tg_expr\n");
-            for i = 1:length(obj.problem.lbg)
-                if ~only_violations || (g_val(i) < obj.problem.lbg(i) || g_val(i) > obj.problem.ubg(i))
-                    expr_str = formattedDisplayText(obj.problem.g(i));
-                    lb_str = pad(sprintf('%.2e', obj.problem.lbg(i)), 12);
-                    ub_str = pad(sprintf('%.2e', obj.problem.ubg(i)), 12);
+            for i = 1:length(obj.nlp.lbg)
+                if ~only_violations || (g_val(i) < obj.nlp.lbg(i) || g_val(i) > obj.nlp.ubg(i))
+                    expr_str = formattedDisplayText(obj.nlp.g(i));
+                    lb_str = pad(sprintf('%.2e', obj.nlp.lbg(i)), 12);
+                    ub_str = pad(sprintf('%.2e', obj.nlp.ubg(i)), 12);
                     fprintf(fileID, "%d\t%s\t%s\t%.2e\t%s\n", i, lb_str, ub_str, g_val(i), expr_str);
                 end
             end