From 35908fb14b17b3c4ea8041d0fd01578ce3708da1 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 19 Mar 2025 10:02:19 +0100 Subject: [PATCH 01/11] add gershgorin utils --- acados/utils/math.c | 45 +++++++++++++++++++++++++++++++++++++++++++++ acados/utils/math.h | 6 ++++++ 2 files changed, 51 insertions(+) diff --git a/acados/utils/math.c b/acados/utils/math.c index 0d198cf270..a18d2ae6e5 100644 --- a/acados/utils/math.c +++ b/acados/utils/math.c @@ -1110,6 +1110,51 @@ void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double } +void compute_gershgorin_max_abs_eig_estimate(int n, struct blasfeo_dmat *A, double *out) +{ + double max_abs_eig = 0.0; + double r_i, lam, rho, a; + for (int ii = 0; ii < n; ii++) + { + r_i = 0.0; + for (int jj = 0; jj < n; jj++) + { + if (jj != ii) + { + r_i += fabs(BLASFEO_DMATEL(A, ii, jj)); + } + } + a = BLASFEO_DMATEL(A, ii, ii); + lam = a - r_i; + rho = a + r_i; + max_abs_eig = fmax(max_abs_eig, fmax(fabs(lam), fabs(rho))); + } + *out = max_abs_eig; +} + +void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double *out) +{ + // returns a lower bound for the minimum eigenvalue of A + double lam = ACADOS_INFTY; + double a, r_i, lam_i; + for (int ii = 0; ii < n; ii++) + { + r_i = 0.0; + for (int jj = 0; jj < n; jj++) + { + if (jj != ii) + { + r_i += fabs(BLASFEO_DMATEL(A, ii, jj)); + } + } + a = BLASFEO_DMATEL(A, ii, ii); + lam_i = a - r_i; + lam = fmin(lam, lam_i); + } + *out = lam; +} + + double minimum_of_doubles(double *x, int n) { diff --git a/acados/utils/math.h b/acados/utils/math.h index 4bbef9cbf5..d1e13539f8 100644 --- a/acados/utils/math.h +++ b/acados/utils/math.h @@ -36,6 +36,7 @@ extern "C" { #endif #include "acados/utils/types.h" +#include "blasfeo_common.h" #if defined(__MABX2__) double fmax(double a, double b); @@ -100,6 +101,11 @@ double minimum_of_doubles(double *x, int n); void neville_algorithm(double xx, int n, double *x, double *Q, double *out); +void compute_gershgorin_max_abs_eig_estimate(int n, struct blasfeo_dmat *A, double *out); + +void compute_gershgorin_min_eig_estimate(int n, struct blasfeo_dmat *A, double *out); + + #ifdef __cplusplus } /* extern "C" */ #endif From 53baf22590cb3d18b41afd217bce65f4ef8e994e Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 19 Mar 2025 10:17:44 +0100 Subject: [PATCH 02/11] add ocp_nlp_reg_glm --- acados/ocp_nlp/ocp_nlp_reg_glm.c | 326 +++++++++++++++++++++++++++++++ acados/ocp_nlp/ocp_nlp_reg_glm.h | 120 ++++++++++++ 2 files changed, 446 insertions(+) create mode 100644 acados/ocp_nlp/ocp_nlp_reg_glm.c create mode 100644 acados/ocp_nlp/ocp_nlp_reg_glm.h diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.c b/acados/ocp_nlp/ocp_nlp_reg_glm.c new file mode 100644 index 0000000000..2b11f67925 --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.c @@ -0,0 +1,326 @@ +/* + * 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.; + */ + + +#include "acados/ocp_nlp/ocp_nlp_reg_glm.h" + +#include +#include +#include +#include + +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/utils/math.h" + +#include "blasfeo_d_aux.h" +#include "blasfeo_d_blas.h" + + + +/************************************************ + * opts + ************************************************/ + +acados_size_t ocp_nlp_reg_glm_opts_calculate_size(void) +{ + return sizeof(ocp_nlp_reg_glm_opts); +} + + + +void *ocp_nlp_reg_glm_opts_assign(void *raw_memory) +{ + return raw_memory; +} + + + +void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + // ocp_nlp_reg_glm_opts *opts = opts_; + return; +} + + + +void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, void* value) +{ + + // ocp_nlp_reg_glm_opts *opts = opts_; + printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); + exit(1); + // if (!strcmp(field, "epsilon")) + // { + // double *d_ptr = value; + // opts->epsilon = *d_ptr; + // } + // else + // { + // printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); + // exit(1); + // } + + return; +} + + + +/************************************************ + * memory + ************************************************/ + +acados_size_t ocp_nlp_reg_glm_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + } + + acados_size_t size = 0; + + size += sizeof(ocp_nlp_reg_glm_memory); + + size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + + return size; +} + + + +void *ocp_nlp_reg_glm_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + } + + char *c_ptr = (char *) raw_memory; + + ocp_nlp_reg_glm_memory *mem = (ocp_nlp_reg_glm_memory *) c_ptr; + c_ptr += sizeof(ocp_nlp_reg_glm_memory); + + mem->RSQrq = (struct blasfeo_dmat **) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + + assert((char *) mem + ocp_nlp_reg_glm_memory_calculate_size(config_, dims, opts_) >= c_ptr); + + return mem; +} + + + +void ocp_nlp_reg_glm_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_) +{ + ocp_nlp_reg_glm_memory *memory = memory_; + + int ii; + + int N = dims->N; + // int *nx = dims->nx; + // int *nu = dims->nu; + + for(ii=0; ii<=N; ii++) + { + memory->RSQrq[ii] = RSQrq+ii; +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0); + } + + return; +} + + + +void ocp_nlp_reg_glm_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_glm_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) +{ + + if(!strcmp(field, "RSQrq_ptr")) + { + struct blasfeo_dmat *RSQrq = value; + ocp_nlp_reg_glm_memory_set_RSQrq_ptr(dims, RSQrq, memory_); + } + else + { + printf("\nerror: field %s not available in ocp_nlp_reg_glm_set\n", field); + exit(1); + } + + return; +} + + + +/************************************************ + * functions + ************************************************/ + +void ocp_nlp_reg_glm_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_glm_memory *mem = (ocp_nlp_reg_glm_memory *) mem_; + // ocp_nlp_reg_glm_opts *opts = opts_; + + int ii; + + int *nx = dims->nx; + int *nu = dims->nu; + double tmp; + + for(ii=0; ii<=dims->N; ii++) + { + // make symmetric + blasfeo_dtrtr_l(nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + + // regularize + compute_gershgorin_min_eig_estimate(nu[ii]+nx[ii], mem->RSQrq[ii], &tmp); + + blasfeo_ddiare(nu[ii]+nx[ii], tmp, mem->RSQrq[ii], 0, 0); + } +} + + +void ocp_nlp_reg_glm_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_glm_regularize(config, dims, opts_, mem_); +} + + +void ocp_nlp_reg_glm_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + +void ocp_nlp_reg_glm_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + + +void ocp_nlp_reg_glm_config_initialize_default(ocp_nlp_reg_config *config) +{ + // dims + config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size; + config->dims_assign = &ocp_nlp_reg_dims_assign; + config->dims_set = &ocp_nlp_reg_dims_set; + // opts + config->opts_calculate_size = &ocp_nlp_reg_glm_opts_calculate_size; + config->opts_assign = &ocp_nlp_reg_glm_opts_assign; + config->opts_initialize_default = &ocp_nlp_reg_glm_opts_initialize_default; + config->opts_set = &ocp_nlp_reg_glm_opts_set; + // memory + config->memory_calculate_size = &ocp_nlp_reg_glm_memory_calculate_size; + config->memory_assign = &ocp_nlp_reg_glm_memory_assign; + config->memory_set = &ocp_nlp_reg_glm_memory_set; + config->memory_set_RSQrq_ptr = &ocp_nlp_reg_glm_memory_set_RSQrq_ptr; + config->memory_set_rq_ptr = &ocp_nlp_reg_glm_memory_set_rq_ptr; + config->memory_set_BAbt_ptr = &ocp_nlp_reg_glm_memory_set_BAbt_ptr; + config->memory_set_b_ptr = &ocp_nlp_reg_glm_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_glm_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_glm_memory_set_DCt_ptr; + config->memory_set_ux_ptr = &ocp_nlp_reg_glm_memory_set_ux_ptr; + config->memory_set_pi_ptr = &ocp_nlp_reg_glm_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_glm_memory_set_lam_ptr; + // functions + config->regularize = &ocp_nlp_reg_glm_regularize; + config->regularize_rhs = &ocp_nlp_reg_glm_regularize_rhs; + config->regularize_lhs = &ocp_nlp_reg_glm_regularize_lhs; + config->correct_dual_sol = &ocp_nlp_reg_glm_correct_dual_sol; +} + diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.h b/acados/ocp_nlp/ocp_nlp_reg_glm.h new file mode 100644 index 0000000000..bf06fb0d01 --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.h @@ -0,0 +1,120 @@ +/* + * 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.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int dummy; +} ocp_nlp_reg_glm_opts; + +// +acados_size_t ocp_nlp_reg_glm_opts_calculate_size(void); +// +void *ocp_nlp_reg_glm_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + double *reg_hess; // TODO move to workspace + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_glm_memory; + +// +acados_size_t ocp_nlp_reg_glm_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_glm_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_glm_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +/// @} +/// @} From 005826d4cd3036b141de9a729a4dc5d1f1ba006c Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Wed, 19 Mar 2025 10:18:13 +0100 Subject: [PATCH 03/11] interface new option --- interfaces/acados_c/ocp_nlp_interface.c | 4 ++++ interfaces/acados_c/ocp_nlp_interface.h | 1 + .../acados_template/acados_template/acados_ocp_options.py | 2 +- 3 files changed, 6 insertions(+), 1 deletion(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 1d07d15f36..524e0961cc 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -48,6 +48,7 @@ #include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" #include "acados/ocp_nlp/ocp_nlp_constraints_bgp.h" #include "acados/ocp_nlp/ocp_nlp_reg_convexify.h" +#include "acados/ocp_nlp/ocp_nlp_reg_glm.h" #include "acados/ocp_nlp/ocp_nlp_reg_mirror.h" #include "acados/ocp_nlp/ocp_nlp_reg_project.h" #include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h" @@ -237,6 +238,9 @@ ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan) case CONVEXIFY: ocp_nlp_reg_convexify_config_initialize_default(config->regularize); break; + case GERSHGORIN_LEVENBERG_MARQUARDT: + ocp_nlp_reg_glm_config_initialize_default(config->regularize); + break; default: printf("\nerror: ocp_nlp_config_create: unsupported plan->regularization\n"); exit(1); diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 69011dd1a1..fc3cb0cb7c 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -92,6 +92,7 @@ typedef enum PROJECT, PROJECT_REDUC_HESS, CONVEXIFY, + GERSHGORIN_LEVENBERG_MARQUARDT, INVALID_REGULARIZE, } ocp_nlp_reg_t; diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 3a40539bdf..2527f0a284 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -324,7 +324,7 @@ def collocation_type(self): @property def regularize_method(self): """Regularization method for the Hessian. - String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY') or :code:`None`. + String in ('NO_REGULARIZE', 'MIRROR', 'PROJECT', 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'). - MIRROR: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, abs(D_ii)) - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) From 890fc1c8735a9c9bb60715c97d2cfb39094a8844 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:08:55 +0100 Subject: [PATCH 04/11] finalize glm module --- acados/ocp_nlp/ocp_nlp_reg_glm.c | 42 ++++++++++++++++++-------------- acados/ocp_nlp/ocp_nlp_reg_glm.h | 13 +--------- 2 files changed, 25 insertions(+), 30 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.c b/acados/ocp_nlp/ocp_nlp_reg_glm.c index 2b11f67925..c5c7775476 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_glm.c +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.c @@ -35,6 +35,7 @@ #include #include #include +#include #include "acados/ocp_nlp/ocp_nlp_reg_common.h" #include "acados/utils/math.h" @@ -64,7 +65,8 @@ void *ocp_nlp_reg_glm_opts_assign(void *raw_memory) void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_) { - // ocp_nlp_reg_glm_opts *opts = opts_; + ocp_nlp_reg_glm_opts *opts = opts_; + opts->epsilon = 1e-6; return; } @@ -73,19 +75,17 @@ void ocp_nlp_reg_glm_opts_initialize_default(void *config_, ocp_nlp_reg_dims *di void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, void* value) { - // ocp_nlp_reg_glm_opts *opts = opts_; - printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); - exit(1); - // if (!strcmp(field, "epsilon")) - // { - // double *d_ptr = value; - // opts->epsilon = *d_ptr; - // } - // else - // { - // printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); - // exit(1); - // } + ocp_nlp_reg_glm_opts *opts = opts_; + if (!strcmp(field, "epsilon")) + { + double *d_ptr = value; + opts->epsilon = *d_ptr; + } + else + { + printf("\nerror: field %s not available in ocp_nlp_reg_glm_opts_set\n", field); + exit(1); + } return; } @@ -253,13 +253,13 @@ void ocp_nlp_reg_glm_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *mem void ocp_nlp_reg_glm_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) { ocp_nlp_reg_glm_memory *mem = (ocp_nlp_reg_glm_memory *) mem_; - // ocp_nlp_reg_glm_opts *opts = opts_; + ocp_nlp_reg_glm_opts *opts = opts_; int ii; int *nx = dims->nx; int *nu = dims->nu; - double tmp; + double tmp, alpha; for(ii=0; ii<=dims->N; ii++) { @@ -268,8 +268,14 @@ void ocp_nlp_reg_glm_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts // regularize compute_gershgorin_min_eig_estimate(nu[ii]+nx[ii], mem->RSQrq[ii], &tmp); - - blasfeo_ddiare(nu[ii]+nx[ii], tmp, mem->RSQrq[ii], 0, 0); + if (tmp < opts->epsilon) + { + if (tmp < 0) + alpha = fabs(tmp)+opts->epsilon; + else + alpha = opts->epsilon; + blasfeo_ddiare(nu[ii]+nx[ii], alpha, mem->RSQrq[ii], 0, 0); + } } } diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.h b/acados/ocp_nlp/ocp_nlp_reg_glm.h index bf06fb0d01..8b711e26bd 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_glm.h +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.h @@ -63,7 +63,7 @@ extern "C" { typedef struct { - int dummy; + double epsilon; } ocp_nlp_reg_glm_opts; // @@ -83,11 +83,6 @@ void ocp_nlp_reg_glm_opts_set(void *config_, void *opts_, const char *field, voi typedef struct { - double *reg_hess; // TODO move to workspace - double *V; // TODO move to workspace - double *d; // TODO move to workspace - double *e; // TODO move to workspace - struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in } ocp_nlp_reg_glm_memory; @@ -96,12 +91,6 @@ acados_size_t ocp_nlp_reg_glm_memory_calculate_size(void *config, ocp_nlp_reg_di // void *ocp_nlp_reg_glm_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); -/************************************************ - * workspace - ************************************************/ - - // TODO - /************************************************ * functions ************************************************/ From 764ff20a5969be1833381749a70a08505b8a1658 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:10:07 +0100 Subject: [PATCH 05/11] interface new option --- .../acados_template/acados_template/acados_ocp_options.py | 4 ++-- .../acados_template/c_templates_tera/acados_multi_solver.in.c | 2 +- .../acados_template/c_templates_tera/acados_solver.in.c | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 2527f0a284..162f4ee01b 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -766,7 +766,7 @@ def alpha_min(self): @property def reg_epsilon(self): - """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY']""" + """Epsilon for regularization, used if regularize_method in ['PROJECT', 'MIRROR', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT'].""" return self.__reg_epsilon @property @@ -1239,7 +1239,7 @@ def qp_solver(self, qp_solver): @regularize_method.setter def regularize_method(self, regularize_method): regularize_methods = ('NO_REGULARIZE', 'MIRROR', 'PROJECT', \ - 'PROJECT_REDUC_HESS', 'CONVEXIFY') + 'PROJECT_REDUC_HESS', 'CONVEXIFY', 'GERSHGORIN_LEVENBERG_MARQUARDT') if regularize_method in regularize_methods: self.__regularize_method = regularize_method else: diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c index 04fb7662cf..2db025a1fc 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_multi_solver.in.c @@ -2271,7 +2271,7 @@ void {{ name }}_acados_create_set_opts({{ name }}_solver_capsule* capsule) {%- endif %} {%- endif %} -{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" %} +{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" or solver_options.regularize_method == "GERSHGORIN_LEVENBERG_MARQUARDT"%} double reg_epsilon = {{ solver_options.reg_epsilon }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_epsilon", ®_epsilon); {%- endif %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index a32a886ebd..448ea099e4 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -2386,7 +2386,7 @@ static void {{ model.name }}_acados_create_set_opts({{ model.name }}_solver_caps {%- endif %} {%- endif %} -{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" %} +{%- if solver_options.regularize_method == "PROJECT" or solver_options.regularize_method == "MIRROR" or solver_options.regularize_method == "CONVEXIFY" or solver_options.regularize_method == "GERSHGORIN_LEVENBERG_MARQUARDT"%} double reg_epsilon = {{ solver_options.reg_epsilon }}; ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "reg_epsilon", ®_epsilon); {%- endif %} From fe1e31ab14858262d398e8bf5d04745ec00e3af8 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:10:23 +0100 Subject: [PATCH 06/11] remove print in interface --- interfaces/acados_template/acados_template/acados_ocp_solver.py | 1 - 1 file changed, 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_solver.py b/interfaces/acados_template/acados_template/acados_ocp_solver.py index ca9b912ed0..56a642dde8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_solver.py +++ b/interfaces/acados_template/acados_template/acados_ocp_solver.py @@ -1568,7 +1568,6 @@ def get_stats(self, field_: str) -> Union[int, float, np.ndarray]: nlp_iter = self.get_stats("nlp_iter") stat_m = self.get_stats("stat_m") stat_n = self.get_stats("stat_n") - print("stat_n: ", stat_n) min_size = min([stat_m, nlp_iter+1]) out = np.zeros((stat_n+1, min_size), dtype=np.float64, order="C") out_data = cast(out.ctypes.data, POINTER(c_double)) From 015879ba5cac194198c7b1a4f07d1ea8462b279b Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:10:53 +0100 Subject: [PATCH 07/11] test new regularization module --- .../non_ocp_nlp/adaptive_eps_reg_test.py | 54 ++++++++++--------- 1 file changed, 30 insertions(+), 24 deletions(-) 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 index 2613314016..21c117b7fe 100644 --- a/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py +++ b/examples/acados_python/non_ocp_nlp/adaptive_eps_reg_test.py @@ -73,7 +73,7 @@ def export_parametric_ocp() -> AcadosOcp: 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.print_level = 0 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 @@ -94,12 +94,12 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): 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) + ocp_solver = AcadosOcpSolver(ocp, verbose=False) nx = ocp.dims.nx nu = ocp.dims.nu - eps_min = ocp.solver_options.reg_min_epsilon + eps_min = ocp.solver_options.reg_min_epsilon if regularize_method != "GERSHGORIN_LEVENBERG_MARQUARDT" else ocp.solver_options.reg_epsilon W_mat2 = np.zeros((nx+nu, nx+nu)) W_mat2[0,0] = 1e6 @@ -134,31 +134,36 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): hess_0 = ocp_solver.get_hessian_block(0) hess_1 = ocp_solver.get_hessian_block(1) - # check condition numbers - 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]}" - 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]}" - # check solver stats assert status == 0, f"acados returned status {status}" + # check eigenvalues + eigvals_0 = np.real(np.linalg.eigvals(hess_0)) + assert np.min(eigvals_0) >= eps_min*0.99, f"Eigenvalues of Hessian must be >= {eps_min} = eps_min, got {eigvals_0}" # check hessian - if i == 0: - assert np.equal(hess_0, eps_min*np.eye(nx+nu)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" - elif i == 1: - assert np.equal(hess_0, np.diag([1e3, 1e3, 1e6, 1e3, 1e3, 1e3])).all(), f"Something in adaptive {regularize_method} went wrong!" - elif i == 2: - # print(np.linalg.eigvals(W_mat)) - print(hess_0) - print(np.real(np.linalg.eigvals(hess_0))) - if regularize_method == 'MIRROR': - max_abs_eig = np.max(np.abs(W3_eig)) - reg_eps = max(max_abs_eig/ocp.solver_options.reg_max_cond_block, eps_min) - assert np.allclose(np.linalg.eigvals(hess_0), np.array([reg_eps, max_abs_eig, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong!" - elif regularize_method == 'PROJECT': - max_pos_eig = np.max(W3_eig) - reg_eps = max(max_pos_eig/ocp.solver_options.reg_max_cond_block, eps_min) - assert np.allclose(np.real(np.linalg.eigvals(hess_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!" + if regularize_method in ['MIRROR', 'PROJECT']: + # check condition numbers + 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]}" + 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]}" + if i == 0: + print(hess_0) + assert np.equal(hess_0, eps_min*np.eye(nx+nu)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" + elif i == 1: + min_eig = np.max(eigvals_0) / ocp.solver_options.reg_max_cond_block + assert np.equal(hess_0, np.diag([min_eig, min_eig, 1e6, min_eig, min_eig, min_eig])).all(), f"Something in adaptive {regularize_method} went wrong!" + elif i == 2: + # print(np.linalg.eigvals(W_mat)) + # print(hess_0) + # print(eigvals_0) + if regularize_method == 'MIRROR': + max_abs_eig = np.max(np.abs(W3_eig)) + reg_eps = max(max_abs_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(eigvals_0, np.array([reg_eps, max_abs_eig, reg_eps, reg_eps, reg_eps, reg_eps])), f"Something in adaptive {regularize_method} went wrong!" + elif regularize_method == 'PROJECT': + max_pos_eig = np.max(W3_eig) + reg_eps = max(max_pos_eig/ocp.solver_options.reg_max_cond_block, eps_min) + assert np.allclose(eigvals_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!" assert np.equal(hess_1, eps_min*np.eye(nx)).all(), f"Zero matrix should be regularized to eps_min * eye for {regularize_method}" @@ -166,3 +171,4 @@ def test_reg_adaptive_eps(regularize_method='MIRROR'): if __name__ == "__main__": test_reg_adaptive_eps("MIRROR") test_reg_adaptive_eps("PROJECT") + test_reg_adaptive_eps("GERSHGORIN_LEVENBERG_MARQUARDT") From 4176b291600f7c2ff1ebabde53b99122e63a04e0 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:23:05 +0100 Subject: [PATCH 08/11] docstring --- .../acados_template/acados_template/acados_ocp_options.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index 162f4ee01b..f1dd9823e5 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -330,8 +330,7 @@ def regularize_method(self): - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf, does not support nonlinear constraints - PROJECT_REDUC_HESS: experimental - - Note: default eps = 1e-4 + - GERSHGORIN_LEVENBERG_MARQUARDT: estimates the smallest eigenvalue block-wise using Gershgorin circles and adds multiple of identity to the block, such that smallest eigenvalue after regularization is at least reg_epsilon Default: 'NO_REGULARIZE'. """ From f353d555691e3dd683832c592e32f2aac65dc77c Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 11:36:43 +0100 Subject: [PATCH 09/11] fix include guard --- acados/ocp_nlp/ocp_nlp_reg_glm.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.h b/acados/ocp_nlp/ocp_nlp_reg_glm.h index 8b711e26bd..e880715320 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_glm.h +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.h @@ -34,8 +34,8 @@ /// \addtogroup ocp_nlp_reg /// @{ -#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ -#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ #ifdef __cplusplus extern "C" { @@ -104,6 +104,6 @@ void ocp_nlp_reg_glm_config_initialize_default(ocp_nlp_reg_config *config); } #endif -#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_GLM_H_ /// @} /// @} From db1b3515b7633b05677534c59f213f43c6f30a69 Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 14:33:36 +0100 Subject: [PATCH 10/11] remove commented code, add todo --- acados/ocp_nlp/ocp_nlp_reg_glm.c | 17 +++-------------- 1 file changed, 3 insertions(+), 14 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_glm.c b/acados/ocp_nlp/ocp_nlp_reg_glm.c index c5c7775476..121086c94d 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_glm.c +++ b/acados/ocp_nlp/ocp_nlp_reg_glm.c @@ -157,13 +157,10 @@ void ocp_nlp_reg_glm_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo int ii; int N = dims->N; - // int *nx = dims->nx; - // int *nu = dims->nu; for(ii=0; ii<=N; ii++) { memory->RSQrq[ii] = RSQrq+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0); } return; @@ -229,17 +226,9 @@ void ocp_nlp_reg_glm_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_d void ocp_nlp_reg_glm_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) { - - if(!strcmp(field, "RSQrq_ptr")) - { - struct blasfeo_dmat *RSQrq = value; - ocp_nlp_reg_glm_memory_set_RSQrq_ptr(dims, RSQrq, memory_); - } - else - { - printf("\nerror: field %s not available in ocp_nlp_reg_glm_set\n", field); - exit(1); - } + // TODO: remove this function in all regularizaiton modules + printf("\nerror: field %s not available in ocp_nlp_reg_glm_set\n", field); + exit(1); return; } From bf047912a70a39fb31d3a3bc02b2a5c4192f756d Mon Sep 17 00:00:00 2001 From: Jonathan Frey Date: Thu, 20 Mar 2025 14:33:50 +0100 Subject: [PATCH 11/11] adjust docstring --- .../acados_template/acados_template/acados_ocp_options.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_options.py b/interfaces/acados_template/acados_template/acados_ocp_options.py index f1dd9823e5..68d3e67eca 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_options.py +++ b/interfaces/acados_template/acados_template/acados_ocp_options.py @@ -330,7 +330,7 @@ def regularize_method(self): - PROJECT: performs eigenvalue decomposition H = V^T D V and sets D_ii = max(eps, D_ii) - CONVEXIFY: Algorithm 6 from Verschueren2017, https://cdn.syscop.de/publications/Verschueren2017.pdf, does not support nonlinear constraints - PROJECT_REDUC_HESS: experimental - - GERSHGORIN_LEVENBERG_MARQUARDT: estimates the smallest eigenvalue block-wise using Gershgorin circles and adds multiple of identity to the block, such that smallest eigenvalue after regularization is at least reg_epsilon + - GERSHGORIN_LEVENBERG_MARQUARDT: estimates the smallest eigenvalue of each Hessian block using Gershgorin circles and adds multiple of identity to each block, such that smallest eigenvalue after regularization is at least reg_epsilon Default: 'NO_REGULARIZE'. """