acados · sandmaennchen · Jul 24, 2024 · Jul 23, 2024 · Jul 24, 2024 · Jul 24, 2024
diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c
@@ -2772,9 +2772,9 @@ double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims,
 
 
 
-double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
-            int check_early_termination, int sqp_iter)
+            int check_early_termination, int sqp_iter, double *alpha_reference)
 {
     int i, j;
 
@@ -2796,14 +2796,10 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
         // copy out (current iterate) to work->tmp_nlp_out
         for (i = 0; i <= N; i++)
             blasfeo_dveccp(nv[i], out->ux+i, 0, work->tmp_nlp_out->ux+i, 0);
+        // NOTE: copying duals not needed, as they dont enter the merit function
 
-        // for (i = 0; i < N; i++)
-        //     blasfeo_dveccp(nx[i+1], out->pi+i, 0, work->tmp_nlp_out->pi+i, 0);
-
-        // for (i = 0; i <= N; i++)
-        //     blasfeo_dveccp(2*ni[i], out->lam+i, 0, work->tmp_nlp_out->lam+i, 0);
-
-            // linear update of algebraic variables using state and input sensitivity
+        // TODO: think about z here!
+        // linear update of algebraic variables using state and input sensitivity
     //        if (i < N)
     //        {
     //            blasfeo_dgemv_t(nu[i]+nx[i], nz[i], alpha, mem->dzduxt+i, 0, 0, mem->qp_out->ux+i, 0, 1.0, mem->z_alg+i, 0, out->z+i, 0);
@@ -2830,7 +2826,6 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
         else
         {
             // update weights
-            // printf("merit fun: update weights, sqp_iter = %d\n", sqp_iter);
             for (i = 0; i < N; i++)
             {
                 for(j=0; j<nx[i+1]; j++)
@@ -2886,15 +2881,23 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
                     printf("\npreliminary line_search: merit0 %e, merit1 %e; viol_current %e, viol_step %e\n", merit_fun0, merit_fun1, violation_current, violation_step);
                 }
 
+                if (isnan(merit_fun1) || isinf(merit_fun1))
+                {
+                    // do nothing and continue with normal line search, i.e. step reduction
+                    return ACADOS_NAN_DETECTED;
+                }
                 if (merit_fun1 < merit_fun0 && violation_step < violation_current)
                 {
                     // full step if merit and constraint violation improves
                     // TODO: check armijo in this case?
-                    return alpha;
+                    *alpha_reference = alpha;
+                    return ACADOS_SUCCESS;
                 }
-                else // this implies SOC will be done
+                else
                 {
-                    return reduction_factor * reduction_factor;
+                    // alpha < 1.0 implies SOC will be done
+                    *alpha_reference = reduction_factor * reduction_factor;
+                    return ACADOS_SUCCESS;
                 }
             }
 
@@ -2937,7 +2940,7 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
                 merit_fun1 = ocp_nlp_evaluate_merit_fun(config, dims, in, out, opts, mem, work);
                 if (opts->print_level > 1)
                 {
-                    printf("backtracking %d, merit_fun1 = %e, merit_fun0 %e\n", j, merit_fun1, merit_fun0);
+                    printf("backtracking %d alpha = %f, merit_fun1 = %e, merit_fun0 %e\n", j, alpha, merit_fun1, merit_fun0);
                 }
 
                 // if (merit_fun1 < merit_fun0 && merit_fun1 > max_next_merit_fun_val)
@@ -2946,9 +2949,10 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
                 // }
 
                 max_next_merit_fun_val = merit_fun0 + eps_sufficient_descent * dmerit_dy * alpha;
-                if (merit_fun1 < max_next_merit_fun_val)
+                if ((merit_fun1 < max_next_merit_fun_val) && !isnan(merit_fun1) && !isinf(merit_fun1))
                 {
-                    break;
+                    *alpha_reference = alpha;
+                    return ACADOS_SUCCESS;
                 }
                 else
                 {
@@ -2957,10 +2961,16 @@ double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i
             }
         }
     }
-    // if (opts->globalization != FIXED_STEP)
-    //     printf("alpha %f\n", alpha);
 
-    return alpha;
+    *alpha_reference = alpha;
+    if (isnan(merit_fun1) || isinf(merit_fun1))
+    {
+        return ACADOS_NAN_DETECTED;
+    }
+    else
+    {
+        return ACADOS_MINSTEP;
+    }
 }
 
 /*

diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h
@@ -456,9 +456,9 @@ void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, oc
 int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
 //
-double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
-            int check_early_termination, int sqp_iter);
+            int check_early_termination, int sqp_iter, double *alpha_ref);
 //
 double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
           ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);

diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c
@@ -559,10 +559,17 @@ static bool ocp_nlp_soc_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims,
     // NOTE: the "and" is interpreted as an "or" in the current implementation
 
     // preliminary line search
-    mem->alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 1, sqp_iter);
+    int line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 1, sqp_iter, &mem->alpha);
+
+    // return bool do_line_search;
+    if (line_search_status == ACADOS_NAN_DETECTED)
+    {
+        // do line search but no SOC.
+        return true;
+    }
     if (mem->alpha >= 1.0)
     {
-        return false; // do_line_search;
+        return false;
     }
 
     // Second Order Correction (SOC): following Nocedal2006: p.557,
8000
 eq. (18.51) -- (18.56)
@@ -1533,7 +1540,13 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
 
             if (do_line_search)
             {
-                mem->alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, sqp_iter);
+                int line_search_status;
+                line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, sqp_iter, &mem->alpha);
+                if (line_search_status == ACADOS_NAN_DETECTED)
+                {
+                    mem->status = ACADOS_NAN_DETECTED;
+                    return mem->status;
+                }
             }
             mem->time_glob += acados_toc(&timer1);
             mem->stat[mem->stat_n*(sqp_iter+1)+6] = mem->alpha;

diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c
@@ -555,7 +555,7 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *
     ocp_qp_xcond_solver_config *qp_solver = config->qp_solver;
 
     int qp_iter = 0;
-    int qp_status = 0;
+    int 
6D47
qp_status, line_search_status;
     double tmp_time;
 
     // update QP rhs for SQP (step prim var, abs dual var)
@@ -665,11 +665,17 @@ static void ocp_nlp_sqp_rti_feedback_step(ocp_nlp_config *config, ocp_nlp_dims *
         return;
     }
 
+    double alpha;
     // globalization
     acados_tic(&timer1);
     // TODO: not clear if line search should be called with sqp_iter==0 in RTI;
-    double alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1);
+    line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1, &alpha);
     mem->time_glob += acados_toc(&timer1);
+    if (line_search_status == ACADOS_NAN_DETECTED)
+    {
+        mem->status = line_search_status;
+        return;
+    }
 
     // update variables
     ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, alpha);
@@ -842,7 +848,7 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
 
     // printf("AS_RTI preparation\n");
     qp_info *qp_info_;
-    int qp_iter, qp_status;
+    int qp_iter, qp_status, line_search_status;
     double alpha, tmp_time;
 
     // prepare submodules
@@ -924,8 +930,13 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
         // globalization
         acados_tic(&timer1);
         // TODO: not clear if line search should be called with sqp_iter==0 in RTI;
-        alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1);
+        line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1, &alpha);
         mem->time_glob += acados_toc(&timer1);
+        if (line_search_status == ACADOS_NAN_DETECTED)
+        {
+            mem->status = line_search_status;
+            return;
+        }
 
         // update variables
         ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, alpha);
@@ -999,8 +1010,13 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
             // globalization
             acados_tic(&timer1);
             // TODO: not clear if line search should be called with sqp_iter==0 in RTI;
-            alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1);
+            line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1, &alpha);
             mem->time_glob += acados_toc(&timer1);
+            if (line_search_status == ACADOS_NAN_DETECTED)
+            {
+                mem->status = line_search_status;
+                return;
+            }
 
             // update variables
             ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, alpha);
@@ -1077,8 +1093,13 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
             // globalization
             acados_tic(&timer1);
             // TODO: not clear if line search should be called with sqp_iter==0 in RTI;
-            alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1);
+            line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1, &alpha);
             mem->time_glob += acados_toc(&timer1);
+            if (line_search_status == ACADOS_NAN_DETECTED)
+            {
+                mem->status = line_search_status;
+                return;
+            }
 
             // update variables
             ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, alpha);
@@ -1156,8 +1177,13 @@ static void ocp_nlp_sqp_rti_preparation_advanced_step(ocp_nlp_config *config, oc
             // globalization
             acados_tic(&timer1);
             // TODO: not clear if line search should be called with sqp_iter==0 in RTI;
-            alpha = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1);
+            line_search_status = ocp_nlp_line_search(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, 0, 1, &alpha);
             mem->time_glob += acados_toc(&timer1);
+            if (line_search_status == ACADOS_NAN_DETECTED)
+            {
+                mem->status = line_search_status;
+                return;
+            }
 
             // update variables
             ocp_nlp_update_variables_sqp(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, alpha);