acados · FreyJo · Mar 13, 2025 · Mar 12, 2025 · Mar 12, 2025 · Mar 12, 2025
diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c
@@ -3715,6 +3715,7 @@ void ocp_nlp_dump_qp_in_to_file(ocp_qp_in *qp_in, int sqp_iter, int soc)
     FILE *out_file = fopen(filename, "w");
     print_ocp_qp_in_to_file(out_file, qp_in);
     fclose(out_file);
+    printf("qp_in dumped to %s\n", filename);
 }
 
 

diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c
@@ -760,6 +760,7 @@ void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims_, void *
     }
 
     // slack update gradient
+    // grad_s (z_QP) = z_NLP + Z_NLP * slack
     blasfeo_dveccp(2*ns, &model->z, 0, &memory->grad, nu+nx);
     blasfeo_dvecmulacc(2*ns, &model->Z, 0, memory->ux, nu+nx, &memory->grad, nu+nx);
 

diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.c b/acados/ocp_nlp/ocp_nlp_reg_common.c
@@ -33,6 +33,7 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
+#include <math.h>
 
 #include "acados/utils/math.h"
 
@@ -187,20 +188,46 @@ void acados_mirror(int dim, double *A, double *V, double *d, double *e, double e
 
     acados_eigen_decomposition(dim, A, V, d, e);
 
+    // mirror
     for (i = 0; i < dim; i++)
     {
-        // project
         if (d[i] >= -epsilon && d[i] <= epsilon)
             d[i] = epsilon;
-        // mirror
         else if (d[i] < 0)
             d[i] = -d[i];
     }
 
     acados_reconstruct_A(dim, A, V, d);
 }
 
+void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block)
+{
+    int i;
+    acados_eigen_decomposition(dim, A, V, d, e);
+    double max_eig = 0.0;
+    double eps;
+
+    // compute max and min eigenvalues
+    for (i=0; i < dim; i++)
+    {
+        max_eig = MAX(max_eig, fabs(d[i]));
+    }
+    if (max_eig == 0.0)
+        eps = 1.0;
+    else
+        eps = max_eig/max_cond_block;
 
+    // mirror
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] >= -eps && d[i] <= eps)
+            d[i] = eps;
+        else if (d[i] < 0)
+            d[i] = -d[i];
+    }
+
+    acados_reconstruct_A(dim, A, V, d);
+}
 
 // projecting regularization
 void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon)
@@ -220,5 +247,29 @@ void acados_project(int dim, double *A, double *V, double *d, double *e, double
 }
 
 
+void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block)
+{
+    int i;
+    acados_eigen_decomposition(dim, A, V, d, e);
+    double max_eig = 0.0;
+    double eps;
+
+    // compute max and min eigenvalues
+    for (i=0; i < dim; i++)
+    {
+        max_eig = MAX(max_eig, d[i]);
+    }
+    if (max_eig == 0.0)
+        eps = 1.0;
+    else
+        eps = max_eig/max_cond_block;
 
+    // project
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] < eps)
+            d[i] = eps;
    }
 
+    acados_reconstruct_A(dim, A, V, d);
+}
diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.h b/acados/ocp_nlp/ocp_nlp_reg_common.h
@@ -111,8 +111,9 @@ void *ocp_nlp_reg_config_assign(void *raw_memory);
 /* regularization help functions */
 void acados_reconstruct_A(int dim, double *A, double *V, double *d);
 void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
+void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block);
 void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
-
+void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block);
 
 
 #ifdef __cplusplus

diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.c b/acados/ocp_nlp/ocp_nlp_reg_mirror.c
@@ -67,6 +67,8 @@ void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims
     ocp_nlp_reg_mirror_opts *opts = opts_;
 
     opts->epsilon = 1e-4;
+    opts->adaptive_eps = false;
+    opts->max_cond_block = 1e7;
 
     return;
 }
@@ -83,6 +85,16 @@ void ocp_nlp_reg_mirror_opts_set(void *config_, void *opts_, const char *field,
         double *d_ptr = value;
         opts->epsilon = *d_ptr;
     }
+    else if (!strcmp(field, "max_cond_block"))
+    {
+        double *d_ptr = value;
+        opts->max_cond_block = *d_ptr;
+    }
+    else if (!strcmp(field, "adaptive_eps"))
+    {
+        bool *b_ptr = value;
+        opts->adaptive_eps = *b_ptr;
+    }
     else
     {
         printf("\nerror: field %s not available in ocp_nlp_reg_mirror_opts_set\n", field);
@@ -285,7 +297,14 @@ void ocp_nlp_reg_mirror_regularize(void *config, ocp_nlp_reg_dims *dims, void *o
 
         // regularize
         blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
-        acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        if (opts->adaptive_eps)
+        {
+            acados_mirror_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block);
+        }
+        else
+        {
+            acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        }
         blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
     }
 }

diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/acados/ocp_nlp/ocp_nlp_reg_mirror.h
@@ -64,6 +64,8 @@ extern "C" {
 typedef struct
 {
     double epsilon;
+    bool adaptive_eps;
+    double max_cond_block;
 } ocp_nlp_reg_mirror_opts;
 
 //

diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.c b/acados/ocp_nlp/ocp_nlp_reg_project.c
@@ -67,6 +67,8 @@ void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims
     ocp_nlp_reg_project_opts *opts = opts_;
 
     opts->epsilon = 1e-4;
+    opts->adaptive_eps = false;
+    opts->max_cond_block = 1e7;
 
     return;
 }
@@ -83,6 +85,16 @@ void ocp_nlp_reg_project_opts_set(void *config_, void *opts_, const char *field,
         double *d_ptr = value;
         opts->epsilon = *d_ptr;
     }
+    else if (!strcmp(field, "max_cond_block"))
+    {
+        double *d_ptr = value;
+        opts->max_cond_block = *d_ptr;
+    }
+    else if (!strcmp(field, "adaptive_eps"))
+    {
+        bool *b_ptr = value;
+        opts->adaptive_eps = *b_ptr;
+    }
     else
     {
         printf("\nerror: field %s not available in ocp_nlp_reg_project_opts_set\n", field);
@@ -284,7 +296,14 @@ void ocp_nlp_reg_project_regularize(void *config, ocp_nlp_reg_dims *dims, void *
 
         // regularize
         blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
-        acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        if (opts->adaptive_eps)
+        {
+            acados_project_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block);
+        }
+        else
+        {
+            acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        }
         blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
     }
 }

diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.h b/acados/ocp_nlp/ocp_nlp_reg_project.h
@@ -64,6 +64,8 @@ extern "C" {
 typedef struct
 {
     double epsilon;
+    bool adaptive_eps;
+    double max_cond_block;
 } ocp_nlp_reg_project_opts;
 
 //

diff --git a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py
@@ -0,0 +1,162 @@
+# -*- coding: future_fstrings -*-
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+import numpy as np
+import casadi as ca
+from acados_template import AcadosOcp, AcadosModel, AcadosOcpSolver, latexify_plot
+import matplotlib.pyplot as plt
+from scipy.linalg import block_diag
+from math import isclose
+latexify_plot()
+
+def export_parametric_ocp() -> AcadosOcp:
+    nx = 4
+    nu = 2
+    model = AcadosModel()
+    ocp = AcadosOcp()
+    model.x = ca.SX.sym("x", nx)
+    model.u = ca.SX.sym("u", nu)
+
+    x_next = ca.vertcat(model.x)
+    x_next[:nu] += model.u
+    model.disc_dyn_expr = x_next
+
+    # define cost
+    p_W = ca.SX.sym("W", nx+nu, nx+nu)
+    p_W_e = ca.SX.sym("W_e", nx, nx)
+    model.p = ca.vertcat(p_W[:], p_W_e[:])
+    ocp.parameter_values = np.ones((model.p.size()[0], ))
+    ny = nx+nu
+    xu = ca.vertcat(model.x, model.u)
+    model.cost_expr_ext_cost = 0.5*xu.T @ p_W @ xu
+    model.cost_expr_ext_cost_e = 0.5*model.x.T @ p_W_e @ model.x
+    ocp.cost.cost_type = "EXTERNAL"
+    ocp.cost.cost_type_e = "EXTERNAL"
+
+    model.name = "non_ocp"
+    ocp.model = model
+
+    ocp.constraints.x0 = np.ones((nx, ))
+
+    ocp.solver_options.integrator_type = "DISCRETE"
+    ocp.solver_options.qp_solver = "FULL_CONDENSING_HPIPM"
+    ocp.solver_options.hessian_approx = "EXACT"
+    ocp.solver_options.regularize_method = "MIRROR"
+    ocp.solver_options.nlp_solver_type = "SQP"
+    ocp.solver_options.N_horizon = 1
+    ocp.solver_options.tf = 1.0
+    ocp.solver_options.print_level = 1
+    ocp.solver_options.nlp_solver_ext_qp_res = 1
+    ocp.solver_options.nlp_solver_max_iter = 2
+    ocp.solver_options.eval_residual_at_max_iter = False
+    ocp.solver_options.reg_adaptive_eps = True
+    ocp.solver_options.reg_max_cond_block = 1e3
+
+    return ocp
+
+def set_cost_matrix(solver, W_mat, W_mat_e):
+    p_val = np.concatenate([W_mat.flatten(), W_mat_e.flatten()])
+    solver.set(0, "p", p_val)
+    solver.set(1, "p", p_val)
+
+def test_reg_adaptive_eps(regularize_method='MIRROR'):
+    ocp = export_parametric_ocp()
+    ocp.solver_options.qp_solver_t0_init = 0
+    ocp.solver_options.nlp_solver_max_iter = 2 # QP should converge in one iteration
+    ocp.solver_options.regularize_method = regularize_method
+
+    ocp_solver = AcadosOcpSolver(ocp, json_file="parameter_augmented_acados_ocp.json", verbose=False)
+
+    nx = ocp.dims.nx
+    nu = ocp.dims.nu
+
+    W_mat2 = np.zeros((nx+nu, nx+nu))
+    W_mat2[0,0] = 1e6
+    W_mat2[nx+nu-1, nx+nu-1] = 1e-4
+
+    p = np.pi/2
+    A_u = np.array([[np.cos(p), -np.sin(p)], [np.sin(p), np.cos(p)]])
+    mat_u = A_u.T @ np.diag([-1, -1e-3]) @ A_u
+
+    # cf. https://stackoverflow.com/questions/65190660/orthogonality-of-a-4x4-matrix
+    A_x = np.array([[0.5000,   0.5000,   0.5000,   0.5000],
+                    [0.6533,   0.2706,  -0.2706,  -0.6533],
+                    [0.5000,  -0.5000,  -0.5000,   0.5000],
+                    [0.2706,  -0.6533,   0.6533,  -0.2706]])
+
+    mat_x = A_x.T @ np.diag([15, 4.0, -2e5, 1e-6]) @ A_x
+
+    W_mat3 = block_diag(mat_x, mat_u)
+
+    W_mats = [np.zeros((nx+nu, nx+nu)), W_mat2, W_mat3]
+    W_mat_e = np.zeros((nx, nx))
+
+    for i, W_mat in enumerate(W_mats):
+        print(f"{regularize_method} i={i}")
+        print("---------------------")
+
+        # Test zero matrix
+        set_cost_matrix(ocp_solver, W_mat, W_mat_e)
+
+        status = ocp_solver.solve()
+        ocp_solver.print_statistics()
+        nlp_iter = ocp_solver.get_stats("nlp_iter")
+
+        qp_diagnostics = ocp_solver.qp_diagnostics()
+        assert qp_diagnostics['condition_number_stage'][0] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][0]} for i = {i}"
+        assert qp_diagnostics['condition_number_stage'][1] <= ocp.solver_options.reg_max_cond_block +1e-8, f"Condition number must be <= {ocp.solver_options.reg_max_cond_block} per stage, got {qp_diagnostics['condition_number_stage'][1]} for i = {i}"
+
+        assert nlp_iter == 1, f"Number of NLP iterations should be 1, got {nlp_iter} for i = {i}"
+        assert status == 0, f"acados returned status {status} for i = {i}"
+
+        hessian_0 = ocp_solver.get_hessian_block(0)
+        if i == 0:
+            assert np.equal(hessian_0, np.eye(nx+nu)).all(), f"Zero Hessian matrix should be transformed into identity for {regularize_method} for i = {i}"
+        elif i == 1:
+            assert np.equal(hessian_0, np.diag([1e3, 1e3, 1e6, 1e3, 1e3, 1e3])).all(), f"Something in adaptive {regularize_method} went wrong for i = {i}!"
+        elif i == 2:
+            print(np.linalg.eigvals(W_mat))
+            print(hessian_0)
+            print(np.real(np.linalg.eigvals(hessian_0)))
+            if regularize_method == 'MIRROR':
+                reg_eps = 2e5/ocp.solver_options.reg_max_cond_block
+                assert np.allclose(np.linalg.eigvals(hessian_0), np.array([reg_eps, 2e5, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong for i = {i}!"
+            elif regularize_method == 'PROJECT':
+                reg_eps = 15/ocp.solver_options.reg_max_cond_block
+                assert np.allclose(np.real(np.linalg.eigvals(hessian_0)), np.array([15, 4, reg_eps, reg_eps, reg_eps, reg_eps]), rtol=1e-03, atol=1e-3), f"Something in adaptive {regularize_method} went wrong for i = {i}!"
+
+        hessian_1 = ocp_solver.get_hessian_block(1)
+        assert np.equal(hessian_1, np.eye(nx)).all(), f"Zero Hessian matrix should be transformed into identity for {regularize_method} for i = {i}"
+
+
+if __name__ == "__main__":
+    test_reg_adaptive_eps("MIRROR")
+    test_reg_adaptive_eps("PROJECT")
diff --git a/interfaces/acados_matlab_octave/AcadosOcpOptions.m b/interfaces/acados_matlab_octave/AcadosOcpOptions.m
@@ -80,6 +80,8 @@
         cost_discretization
         regularize_method
         reg_epsilon
+        reg_max_cond_block
+        reg_adaptive_eps
         exact_hess_cost
         exact_hess_dyn
         exact_hess_constr
@@ -175,6 +177,8 @@
             obj.cost_discretization = 'EULER';
             obj.regularize_method = 'NO_REGULARIZE';
             obj.reg_epsilon = 1e-4;
+            obj.reg_adaptive_eps = false;
+            obj.reg_max_cond_block = 1e-7;
             obj.shooting_nodes = [];
             obj.cost_scaling = [];
             obj.exact_hess_cost = 1;