fix bug regarding lambda00 for multi simplex model

nurkanovic · Nov 14, 2022 · 25d636a · 25d636a
1 parent 3e37010
commit 25d636a
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 3 deletions.
diff --git a/src/matlab/homotopy_solver.m b/src/matlab/homotopy_solver.m
@@ -46,8 +46,7 @@
 
 % lambda00 initialization
 if strcmp(settings.pss_mode, 'Stewart')
-    g_eval = full(model.g_Stewart_fun(x0));
-    lambda00 = g_eval - min(g_eval);
+    lambda00 = full(model.lambda00_fun(x0));
 elseif strcmp(settings.pss_mode, 'Step')
     c_x = full(model.c_fun(x0));
     lambda00 = [ max(c_x, 0); min(c_x, 0)];
@@ -59,7 +58,7 @@
     complementarity_stats = [full(comp_res(w0, p_val))];
 catch
     w0 = w0(1:length(model.w));
-    complementarity_stats = [full(comp_res(w0))];
+    complementarity_stats = [full(comp_res(w0, p_val))];
 end
 cpu_time = [];
 homotopy_iterations = [];

diff --git a/src/matlab/model_reformulation_nosnoc.m b/src/matlab/model_reformulation_nosnoc.m
@@ -809,6 +809,7 @@
 g_switching = []; % collects switching function algebraic equations 0 = g_i(x) - \lambda_i - e \mu_i
 g_convex = []; % equation for the convex multiplers 1 = e' \theta
 f_comp_residual = 0; % the orthogonality conditions diag(\theta) \lambda = 0.
+lambda00_expr =[];
 for ii = 1:n_simplex
     switch pss_mode
         case 'Stewart'
@@ -822,6 +823,7 @@
             % Gradient of Lagrange Function of indicator LP
             g_switching = [g_switching; g_Stewart{ii}-lambda_all{ii}+mu_all{ii}*e_ones_all{ii}];
             g_convex = [g_convex;e_ones_all{ii}'*theta_all{ii}-1];
+            lambda00_expr = [lambda00_expr; g_Stewart{ii}- min(g_Stewart{ii})];
             f_comp_residual = f_comp_residual + lambda_all{ii}'*theta_all{ii};
         case 'Step'
             % c_i(x) - (lambda_1_i-lambda_0_i)  = 0; for all i = 1,..., n_simplex
@@ -894,6 +896,7 @@
     g_z_all_fun = Function('g_z_all_fun',{x,z},{g_z_all}); % lp kkt conditions without bilinear complementarity term (it is treated with the other c.c. conditions)
     dot_c_fun = Function('c_fun',{x,z},{dot_c}); % total time derivative of switching functions
 end
+model.lambda00_fun = Function('lambda00_fun',{x},{lambda00_expr});
 
 J_cc_fun = Function('J_cc_fun',{z},{f_comp_residual});
 f_q_T_fun = Function('f_q_T',{x},{f_q_T});