fix cross comp

nurkanovic · Jul 4, 2024 · 66ba551 · 66ba551
1 parent e0f0bec
commit 66ba551
Show file tree

Hide file tree

Showing 3 changed files with 32 additions and 10 deletions.
diff --git 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
@@ -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
@@ -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;