From 66ba55198f4078ec115afb1308dbeaa4f9e5704b Mon Sep 17 00:00:00 2001 From: Anton Edvinovich Pozharskiy Date: Thu, 20 Jun 2024 17:00:50 +0200 Subject: [PATCH] fix cross comp --- +nosnoc/+discrete_time_problem/Cls.m | 32 ++++++++++++++++--- .../bouncing_ball_2d_sim.m | 2 +- .../new_design_examples/new_design_cls2.m | 8 ++--- 3 files changed, 32 insertions(+), 10 deletions(-) diff --git a/+nosnoc/+discrete_time_problem/Cls.m b/+nosnoc/+discrete_time_problem/Cls.m index b205f9595..ade729563 100644 --- a/+nosnoc/+discrete_time_problem/Cls.m +++ b/+nosnoc/+discrete_time_problem/Cls.m @@ -885,7 +885,7 @@ function generate_complementarity_constraints(obj) else n_vt_lb = 0; end - sum_n_vt = sum2(obj.w.n_vt(ii,jj,:)); + sum_n_vt = n_vt_lb + sum2(obj.w.n_vt(ii,jj,:)); if jj ~= 1 || ~opts.no_initial_impacts Gij = vertcat(Gij, {obj.w.P_vt(ii,jj)}); Hij = vertcat(Hij, {sum_n_vt}); @@ -897,8 +897,19 @@ function generate_complementarity_constraints(obj) Hij = vertcat(Hij, {n_vt_lb + sum_n_vt}); end case 'Lp' - sum_p_vt = obj.w.P_vt(ii,jj) + sum2(obj.w.p_vt(ii,jj,:)); - sum_n_vt = obj.w.N_vt(ii,jj) + sum2(obj.w.n_vt(ii,jj,:)); + if jj ~= 1 || ~opts.no_initial_impacts + p_vt_lb = obj.w.P_vt(ii,jj); + else + p_vt_lb = 0; + end + sum_p_vt = sum2(obj.w.p_vt(ii,jj,:)); + if jj ~= 1 || ~opts.no_initial_impacts + n_vt_lb = obj.w.N_vt(ii,jj); + else + n_vt_lb = 0; + end + sum_p_vt = p_vt_lb + sum2(obj.w.p_vt(ii,jj,:)); + sum_n_vt = n_vt_lb + sum2(obj.w.n_vt(ii,jj,:)); for kk=1:opts.n_s alpha_vt_ijk = obj.w.alpha_vt(ii,jj,kk); @@ -980,8 +991,19 @@ function generate_complementarity_constraints(obj) Gij = vertcat(Gij, {p_vt_lb + sum_p_vt}); Hij = vertcat(Hij, {n_vt_lb + sum_n_vt}); case 'Lp' - sum_p_vt = obj.w.P_vt(ii,jj) + sum2(obj.w.p_vt(ii,jj,:)); - sum_n_vt = obj.w.N_vt(ii,jj) + sum2(obj.w.n_vt(ii,jj,:)); + if jj ~= 1 || ~opts.no_initial_impacts + p_vt_lb = obj.w.P_vt(ii,jj); + else + p_vt_lb = 0; + end + sum_p_vt = sum2(obj.w.p_vt(ii,jj,:)); + if jj ~= 1 || ~opts.no_initial_impacts + n_vt_lb = obj.w.N_vt(ii,jj); + else + n_vt_lb = 0; + end + sum_p_vt = p_vt_lb + sum2(obj.w.p_vt(ii,jj,:)); + sum_n_vt = n_vt_lb + sum2(obj.w.n_vt(ii,jj,:)); sum_alpha_vt = sum2(obj.w.alpha_vt(ii,jj,:)); sum_alpha_vt_minus = sum2(1-obj.w.alpha_vt(ii,jj,:)); diff --git a/examples/cls_minimal_examples/bouncing_ball_2d_sim.m b/examples/cls_minimal_examples/bouncing_ball_2d_sim.m index f04576560..12641902d 100644 --- a/examples/cls_minimal_examples/bouncing_ball_2d_sim.m +++ b/examples/cls_minimal_examples/bouncing_ball_2d_sim.m @@ -17,7 +17,7 @@ solver_options.complementarity_tol = 1e-6; %problem_options.friction_model = "Polyhedral"; problem_options.friction_model = "Conic"; % "Conic" -problem_options.conic_model_switch_handling = "Abs"; +problem_options.conic_model_switch_handling = "Lp"; % settings.mpcc_mode = MpccMode.elastic_ineq; % solver_options.nlpsol = 'snopt'; solver_options.use_previous_solution_as_initial_guess = 1; diff --git a/examples/new_design_examples/new_design_cls2.m b/examples/new_design_examples/new_design_cls2.m index e42717c91..85dad7b31 100644 --- a/examples/new_design_examples/new_design_cls2.m +++ b/examples/new_design_examples/new_design_cls2.m @@ -18,14 +18,14 @@ problem_options.T_sim = T_sim; problem_options.no_initial_impacts = true; problem_options.cross_comp_mode = CrossCompMode.FE_FE; -%problem_options.friction_model = "Conic"; -problem_options.friction_model = "Polyhedral"; +problem_options.friction_model = "Conic"; +%problem_options.friction_model = "Polyhedral"; problem_options.conic_model_switch_handling = "Abs"; %problem_options.eps_cls = 0.5; solver_options.ipopt_callback = @cls_callback; solver_options.sigma_0 = 10; -solver_options.homotopy_steering_strategy = 'ELL_INF'; -solver_options.decreasing_s_elastic_upper_bound = true; +%solver_options.homotopy_steering_strategy = 'ELL_INF'; +%solver_options.decreasing_s_elastic_upper_bound = true; solver_options.complementarity_tol = 1e-6; solver_options.N_homotopy = 7;