From 8259a66d643d5b24763a9ee1d573aff40c02326f Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 7 May 2024 17:25:36 +0200 Subject: [PATCH 1/4] update hpipm --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index 2b18b0f419..f7c4f50217 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 2b18b0f419046cb00806520e2cf4bd691d149277 +Subproject commit f7c4f502172b8ea5279c8f4d34afe882407526c3 From faa53d02aa7e0814e066a2c0096c60d4caf8329a Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 7 May 2024 17:29:50 +0200 Subject: [PATCH 2/4] fix soft constraints mass spring qp example c --- .../mass_spring_example.c | 14 +- .../mass_spring_model/mass_spring_qp.c | 368 ++++++------------ 2 files changed, 118 insertions(+), 264 deletions(-) diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index ca9acf633f..860a8c3039 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -49,17 +49,12 @@ ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring(ocp_qp_xcond_solver_config *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); ocp_qp_in *create_ocp_qp_in_mass_spring(ocp_qp_dims *dims); // soft constraints -// TODO -ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, int nb_, int ng_, int ngN); +ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring_soft_constr(ocp_qp_xcond_solver_config *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims); -// #ifndef ACADOS_WITH_QPDUNES -#define ELIMINATE_X0 -// #endif - #define GENERAL_CONSTRAINT_AT_TERMINAL_STAGE -// #define SOFT_CONSTRAINTS +//#define SOFT_CONSTRAINTS #define NREP 1 @@ -100,7 +95,7 @@ int main() { int num_N2_values = 3; int N2_values[3] = {15,10,5}; - int ii_max = 8; + int ii_max = 9; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -171,8 +166,7 @@ int main() { // dims #ifdef SOFT_CONSTRAINTS - // TODO - ocp_qp_dims *qp_dims = create_ocp_qp_dims_mass_spring_soft_constr(N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_xcond_solver_dims *qp_dims = create_ocp_qp_dims_mass_spring_soft_constr(config, N, nx_, nu_, nb_, ng_, ngN); #else ocp_qp_xcond_solver_dims *qp_dims = create_ocp_qp_dims_mass_spring(config, N, nx_, nu_, nb_, ng_, ngN); #endif diff --git a/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c b/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c index 2bd0185b00..ab56806b31 100644 --- a/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c +++ b/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c @@ -137,11 +137,7 @@ ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring(ocp_qp_xcond_solver_con int nbx_ = nb_ - nu_ > 0 ? nb_ - nu_ : 0; int nx[N+1]; -#if defined(ELIMINATE_X0) - nx[0] = 0; -#else nx[0] = nx_; -#endif for (int ii = 1; ii <= N; ii++) { nx[ii] = nx_; } @@ -160,21 +156,12 @@ ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring(ocp_qp_xcond_solver_con nbu[N] = 0; int nbx[N+1]; -#if defined(ELIMINATE_X0) - nbx[0] = 0; -#else nbx[0] = nx_; -#endif for (int ii = 1; ii <= N; ii++) { nbx[ii] = nbx_; } - // int nb[N+1]; - // for (int ii = 0; ii <= N; ii++) { - // nb[ii] = nbu[ii]+nbx[ii]; - // } - int ng[N+1]; for (int ii = 0; ii < N; ii++) { ng[ii] = ng_; @@ -187,11 +174,7 @@ ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring(ocp_qp_xcond_solver_con } int nbxe[N+1]; -#if defined(ELIMINATE_X0) - nbxe[0] = 0; -#else nbxe[0] = nx_; -#endif for (int ii = 1; ii <= N; ii++) { nbxe[ii] = 0; @@ -205,14 +188,14 @@ ocp_qp_xcond_solver_dims *create_ocp_qp_dims_mass_spring(ocp_qp_xcond_solver_con for (int ii=0; ii<=N; ii++) { - config->dims_set(config, dims, ii, "nx", &nx[ii]); - config->dims_set(config, dims, ii, "nu", &nu[ii]); - config->dims_set(config, dims, ii, "nbx", &nbx[ii]); - config->dims_set(config, dims, ii, "nbu", &nbu[ii]); - config->dims_set(config, dims, ii, "ng", &ng[ii]); - config->dims_set(config, dims, ii, "ns", &ns[ii]); + config->dims_set(config, dims, ii, "nx", &nx[ii]); + config->dims_set(config, dims, ii, "nu", &nu[ii]); + config->dims_set(config, dims, ii, "nbx", &nbx[ii]); + config->dims_set(config, dims, ii, "nbu", &nbu[ii]); + config->dims_set(config, dims, ii, "ng", &ng[ii]); + config->dims_set(config, dims, ii, "ns", &ns[ii]); } - config->dims_set(config, dims, 0, "nbxe", &nbxe[0]); + config->dims_set(config, dims, 0, "nbxe", &nbxe[0]); //d_ocp_qp_dim_print(dims->orig_dims); @@ -272,20 +255,6 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(ocp_qp_dims *dims) // d_print_mat(nx_, 1, b, nx_); // d_print_mat(nx_, 1, x0, nx_); -#if defined(ELIMINATE_X0) - // compute b0 = b + A*x0 - double *b0; - d_zeros(&b0, nx_, 1); - dcopy_3l(nx_, b, 1, b0, 1); - dgemv_n_3l(nx_, nx_, A, nx_, x0, b0); - // d_print_mat(nx_, 1, b, nx_); - // d_print_mat(nx_, 1, b0, nx_); - - // then A0 is a matrix of size 0x0 - double *A0; - d_zeros(&A0, 0, 0); -#endif - /************************************************ * box constraints ************************************************/ @@ -300,13 +269,6 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(ocp_qp_dims *dims) d_zeros(&ub0, nb[0], 1); int *idxbxe0; int_zeros(&idxbxe0, nx[0], 1); -#if defined(ELIMINATE_X0) - for (int jj = 0; jj < nb[0]; jj++) { - lb0[jj] = -0.5; // umin - ub0[jj] = +0.5; // umin - idxb0[jj] = jj; - } -#else jj_end = nu[0] < nb[0] ? nu[0] : nb[0]; for (int jj = 0; jj < jj_end; jj++) { lb0[jj] = -0.5; // umin @@ -318,9 +280,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(ocp_qp_dims *dims) ub0[jj] = x0[jj-jj_end]; // initial state idxb0[jj] = jj; } -#endif - for(int jj=0; jj 0 ? nb_ - nu_ : 0; int nx[N+1]; -#if defined(ELIMINATE_X0) - nx[0] = 0; -#else nx[0] = nx_; -#endif for (int ii = 1; ii <= N; ii++) { nx[ii] = nx_; } @@ -624,21 +542,12 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, nbu[N] = 0; int nbx[N+1]; -#if defined(ELIMINATE_X0) - nbx[0] = 0; -#else nbx[0] = nx_; -#endif for (int ii = 1; ii <= N; ii++) { nbx[ii] = nbx_; } - int nb[N+1]; - for (int ii = 0; ii <= N; ii++) { - nb[ii] = nbu[ii]+nbx[ii]; - } - int ng[N+1]; for (int ii = 0; ii < N; ii++) { ng[ii] = ng_; @@ -646,31 +555,36 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, ng[N] = ngN; int ns[N+1]; - for (int ii = 0; ii <= N; ii++) { + ns[0] = ng[0]; + for (int ii = 1; ii <= N; ii++) { ns[ii] = nbx[ii]+ng[ii]; } + int nbxe[N+1]; + nbxe[0] = nx_; + for (int ii = 1; ii <= N; ii++) + { + nbxe[ii] = 0; + } // dims - int dims_size = ocp_qp_dims_calculate_size(N); + int dims_size = ocp_qp_xcond_solver_dims_calculate_size(config, N); void *dims_mem = malloc(dims_size); - ocp_qp_dims *dims = ocp_qp_dims_assign(N, dims_mem); + ocp_qp_xcond_solver_dims *dims = ocp_qp_xcond_solver_dims_assign(config, N, dims_mem); - dims->N = N; for (int ii=0; ii<=N; ii++) { - dims->nx[ii] = nx[ii]; - dims->nu[ii] = nu[ii]; - dims->nb[ii] = nb[ii]; - dims->ng[ii] = ng[ii]; - dims->ns[ii] = ns[ii]; - dims->nsbx[ii] = nbx[ii]; - dims->nsbu[ii] = 0; - dims->nsg[ii] = ng[ii]; - dims->nbu[ii] = nbu[ii]; - dims->nbx[ii] = nbx[ii]; + config->dims_set(config, dims, ii, "nx", &nx[ii]); + config->dims_set(config, dims, ii, "nu", &nu[ii]); + config->dims_set(config, dims, ii, "nbx", &nbx[ii]); + config->dims_set(config, dims, ii, "nbu", &nbu[ii]); + config->dims_set(config, dims, ii, "ng", &ng[ii]); + config->dims_set(config, dims, ii, "ns", &ns[ii]); } + config->dims_set(config, dims, 0, "nbxe", &nbxe[0]); + +//d_ocp_qp_dim_print(dims->orig_dims); return dims; @@ -718,7 +632,7 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) mass_spring_system(Ts, nx_, nu_, A, B, b); // TODO(dimitris): @giaf, why do we overwrite b here? - for (int jj = 0; jj < nx_; jj++) b[jj] = 0.0; + for (int jj = 0; jj < nx_; jj++) b[jj] = 0.1; // initial state for (int jj = 0; jj < nx_; jj++) x0[jj] = 0; @@ -730,20 +644,6 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) // d_print_mat(nx_, 1, b, nx_); // d_print_mat(nx_, 1, x0, nx_); -#if defined(ELIMINATE_X0) - // compute b0 = b + A*x0 - double *b0; - d_zeros(&b0, nx_, 1); - dcopy_3l(nx_, b, 1, b0, 1); - dgemv_n_3l(nx_, nx_, A, nx_, x0, b0); - // d_print_mat(nx_, 1, b, nx_); - // d_print_mat(nx_, 1, b0, nx_); - - // then A0 is a matrix of size 0x0 - double *A0; - d_zeros(&A0, 0, 0); -#endif - /************************************************ * box constraints ************************************************/ @@ -756,13 +656,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) d_zeros(&lb0, nb[0], 1); double *ub0; d_zeros(&ub0, nb[0], 1); -#if defined(ELIMINATE_X0) - for (int jj = 0; jj < nb[0]; jj++) { - lb0[jj] = -0.5; // umin - ub0[jj] = +0.5; // umin - idxb0[jj] = jj; - } -#else + int *idxbxe0; + int_zeros(&idxbxe0, nx[0], 1); jj_end = nu[0] < nb[0] ? nu[0] : nb[0]; for (int jj = 0; jj < jj_end; jj++) { lb0[jj] = -0.5; // umin @@ -774,7 +669,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) ub0[jj] = x0[jj-jj_end]; // initial state idxb0[jj] = jj; } -#endif + for(int jj=0; jj Date: Tue, 21 May 2024 18:05:01 +0200 Subject: [PATCH 3/4] add soft constraints to osqp interface --- acados/ocp_qp/ocp_qp_common.c | 35 +- acados/ocp_qp/ocp_qp_osqp.c | 664 +++++++++++++----- .../mass_spring_example.c | 24 +- .../mass_spring_model/mass_spring_qp.c | 12 + 4 files changed, 536 insertions(+), 199 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 68f7e62564..0b707bac11 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -616,6 +616,7 @@ void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out) int *nu = qp_in->dim->nu; int *nb = qp_in->dim->nb; int *ng = qp_in->dim->ng; + int *ns = qp_in->dim->ns; struct blasfeo_dmat *DCt = qp_in->DCt; struct blasfeo_dvec *d = qp_in->d; @@ -628,21 +629,29 @@ void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out) for (ii = 0; ii <= N; ii++) { - nx_i = nx[ii]; - nu_i = nu[ii]; - nb_i = nb[ii]; - ng_i = ng[ii]; - // compute slacks for bounds - blasfeo_dvecex_sp(nb_i, 1.0, idxb[ii], ux + ii, 0, t+ii, nb_i + ng_i); - blasfeo_daxpby(nb_i, 1.0, t + ii, nb_i + ng_i, -1.0, d + ii, 0, t + ii, 0); - blasfeo_daxpby(nb_i, -1.0, t + ii, nb_i + ng_i, -1.0, d + ii, nb_i + ng_i, t + ii, - nb_i + ng_i); + blasfeo_dvecex_sp(nb[ii], 1.0, idxb[ii], ux + ii, 0, t+ii, nb[ii] + ng[ii]); + blasfeo_daxpby(nb[ii], 1.0, t + ii, nb[ii] + ng[ii], -1.0, d + ii, 0, t + ii, 0); + blasfeo_daxpby(nb[ii], -1.0, t + ii, nb[ii] + ng[ii], -1.0, d + ii, nb[ii] + ng[ii], t + ii, + nb[ii] + ng[ii]); // compute slacks for general constraints - blasfeo_dgemv_t(nu_i + nx_i, ng_i, 1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, nb_i, - t + ii, nb_i); - blasfeo_dgemv_t(nu_i + nx_i, ng_i, -1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, - 2 * nb_i + ng_i, t + ii, 2 * nb_i + ng_i); + blasfeo_dgemv_t(nu[ii] + nx[ii], ng[ii], 1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, nb[ii], + t + ii, nb[ii]); + blasfeo_dgemv_t(nu[ii] + nx[ii], ng[ii], -1.0, DCt + ii, 0, 0, ux + ii, 0, -1.0, d + ii, + 2 * nb[ii] + ng[ii], t + ii, 2 * nb[ii] + ng[ii]); + + // compute slacks for soft constraints + for(int jj=0; jjidxs_rev[ii][jj]; + if(idx!=-1) + { + BLASFEO_DVECEL(t+ii, jj) += BLASFEO_DVECEL(ux+ii, nu[ii]+nx[ii]+idx); + BLASFEO_DVECEL(t+ii, nb[ii]+ng[ii]+jj) += BLASFEO_DVECEL(ux+ii, nu[ii]+nx[ii]+ns[ii]+idx); + } + } + + blasfeo_dveccp(2*ns[ii], qp_out->ux+ii, nu[ii]+nx[ii], qp_out->t+ii, 2*nb[ii]+2*ng[ii]); } } diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index 051fb6d1f2..63f8d9927f 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -31,6 +31,9 @@ #include +// blasfeo +#include "blasfeo/include/blasfeo_d_blasfeo_api.h" + // acados #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/ocp_qp/ocp_qp_osqp.h" @@ -51,42 +54,53 @@ +#define OSQP_INF 1e8; + + + /************************************************ * helper functions ************************************************/ -// static void print_csc_as_dns(csc *M) -// { -// c_int i, j = 0; // Predefine row index and column index -// c_int idx; - -// // Initialize matrix of zeros -// c_float *A = (c_float *)c_calloc(M->m * M->n, sizeof(c_float)); - -// // Allocate elements -// for (idx = 0; idx < M->p[M->n]; idx++) -// { -// // Get row index i (starting from 1) -// i = M->i[idx]; - -// // Get column index j (increase if necessary) (starting from 1) -// while (M->p[j + 1] <= idx) j++; - -// // Assign values to A -// A[j * (M->m) + i] = M->x[idx]; -// } - -// for (i = 0; i < M->m; i++) -// { -// for (j = 0; j < M->n; j++) -// { -// printf("%f ", A[j * (M->m) + i]); -// } -// printf("\n"); -// } - -// free(A); -// } +#if 0 +static void print_csc_as_dns(csc *M) +{ + c_int i, j = 0; // Predefine row index and column index + c_int idx; + + // Initialize matrix of zeros + c_float *A = (c_float *)c_calloc(M->m * M->n, sizeof(c_float)); + for(c_int ii=0; iim*M->n; ii++) + A[ii] = 1e30; + + // Allocate elements + for (idx = 0; idx < M->p[M->n]; idx++) + { + // Get row index i (starting from 1) + i = M->i[idx]; + + // Get column index j (increase if necessary) (starting from 1) + while (M->p[j + 1] <= idx) j++; + + // Assign values to A + A[j * (M->m) + i] = M->x[idx]; + } + + for (i = 0; i < M->m; i++) + { + for (j = 0; j < M->n; j++) + { + if(A[j * (M->m) + i]==1e30) + printf(" "); + else + printf("%8.4f ", A[j * (M->m) + i]); + } + printf("\n"); + } + + free(A); +} +#endif @@ -214,11 +228,18 @@ static void cpy_osqp_settings(OSQPSettings *from, OSQPSettings *to) static int acados_osqp_num_vars(ocp_qp_dims *dims) { + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int n = 0; - for (int ii = 0; ii <= dims->N; ii++) + for (int ii = 0; ii <= N; ii++) { - n += dims->nx[ii] + dims->nu[ii]; + n += nx[ii]; // states + n += nu[ii]; // controls + n += 2*ns[ii]; // slacks } return n; @@ -228,17 +249,26 @@ static int acados_osqp_num_vars(ocp_qp_dims *dims) static int acados_osqp_num_constr(ocp_qp_dims *dims) { + int N = dims->N; + int *nx = dims->nx; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + int m = 0; - for (int ii = 0; ii <= dims->N; ii++) + // inequality constraints + for (int ii = 0; ii <= N; ii++) { - m += dims->nb[ii]; - m += dims->ng[ii]; + m += nb[ii]; // box constraints + m += ng[ii]; // general constraints + m += 2*ns[ii]; // slacks nonnegativity constraints + } - if (ii < dims->N) - { - m += dims->nx[ii + 1]; - } + // dynamics equality constraints + for (int ii = 0; ii < N; ii++) + { + m += nx[ii + 1]; } return m; @@ -248,13 +278,19 @@ static int acados_osqp_num_constr(ocp_qp_dims *dims) static int acados_osqp_nnzmax_P(const ocp_qp_dims *dims) { + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + int nnz = 0; - for (int ii = 0; ii <= dims->N; ii++) + for (int ii = 0; ii <= N; ii++) { - nnz += dims->nx[ii] * dims->nx[ii]; // Q - nnz += dims->nu[ii] * dims->nu[ii]; // R - nnz += 2 * dims->nx[ii] * dims->nu[ii]; // S + nnz += nx[ii] * nx[ii]; // Q + nnz += nu[ii] * nu[ii]; // R + nnz += 2 * nx[ii] * nu[ii]; // S // TODO 1*, likely only the L or U are needed + nnz += 2 * ns[ii]; // Z } return nnz; @@ -264,22 +300,31 @@ static int acados_osqp_nnzmax_P(const ocp_qp_dims *dims) static int acados_osqp_nnzmax_A(const ocp_qp_dims *dims) { + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + int nnz = 0; - for (int ii = 0; ii <= dims->N; ii++) + // inequality constraints + for (int ii = 0; ii <= N; ii++) { - // inequality constraints - nnz += dims->nb[ii]; // eye - nnz += dims->ng[ii] * dims->nx[ii]; // C - nnz += dims->ng[ii] * dims->nu[ii]; // D + nnz += nb[ii]; // eye of box constraints + nnz += ng[ii] * nx[ii]; // C + nnz += ng[ii] * nu[ii]; // D + nnz += (nb[ii] + ng[ii]) * 2 * ns[ii]; // soft constraints at worst case, when idxs_rev encoding is used. Typically just 2*ns + nnz += 2 * ns[ii]; // eye of slacks nonnegativity constraints + } - // equality constraints - if (ii < dims->N) - { - nnz += dims->nx[ii + 1] * dims->nx[ii]; // A - nnz += dims->nx[ii + 1] * dims->nu[ii]; // B - nnz += dims->nx[ii + 1]; // eye - } + // dynamics equality constraints + for (int ii = 0; ii < N; ii++) + { + nnz += nx[ii + 1] * nx[ii]; // A + nnz += nx[ii + 1] * nu[ii]; // B + nnz += nx[ii + 1]; // eye } return nnz; @@ -289,13 +334,18 @@ static int acados_osqp_nnzmax_A(const ocp_qp_dims *dims) static void update_gradient(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - int kk, nn = 0; ocp_qp_dims *dims = in->dim; - for (kk = 0; kk <= dims->N; kk++) + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int kk, nn = 0; + for (kk = 0; kk <= N; kk++) { - blasfeo_unpack_dvec(dims->nu[kk] + dims->nx[kk], in->rqz + kk, 0, &mem->q[nn], 1); - nn += dims->nu[kk] + dims->nx[kk]; + blasfeo_unpack_dvec(nu[kk]+nx[kk]+2*ns[kk], in->rqz + kk, 0, &mem->q[nn], 1); + nn += nu[kk]+nx[kk]+2*ns[kk]; } } @@ -303,25 +353,46 @@ static void update_gradient(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) static void update_hessian_structure(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - c_int ii, jj, kk, nn = 0, offset = 0, col = 0; ocp_qp_dims *dims = in->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int ii, jj, kk; + // CSC format: P_i are row indices and P_p are column pointers - for (kk = 0; kk <= dims->N; kk++) + c_int nn = 0, offset = 0, col = 0; + for (kk = 0; kk <= N; kk++) { - // writing RSQ[kk] - for (jj = 0; jj < dims->nx[kk] + dims->nu[kk]; jj++) + // write RSQ[kk] + for (jj = 0; jj < nx[kk] + nu[kk]; jj++) { - mem->P_p[col++] = nn; + mem->P_p[col] = nn; + col++; for (ii = 0; ii <= jj; ii++) { // we write only the upper triangular part - mem->P_i[nn++] = offset + ii; + mem->P_i[nn] = offset + ii; + nn++; } } + offset += nx[kk] + nu[kk]; + + // write Z[kk] + for (jj = 0; jj < 2*ns[kk]; jj++) + { + mem->P_p[col] = nn; + col++; + + // diagonal + mem->P_i[nn] = offset + jj; + nn++; + } - offset += dims->nx[kk] + dims->nu[kk]; + offset += 2*ns[kk]; } mem->P_p[col] = nn; @@ -331,23 +402,32 @@ static void update_hessian_structure(const ocp_qp_in *in, ocp_qp_osqp_memory *me static void update_hessian_data(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - c_int ii, jj, kk, nn = 0; ocp_qp_dims *dims = in->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ns = dims->ns; + + int ii, kk; + // Traversing the matrix in column-major order - for (kk = 0; kk <= dims->N; kk++) + c_int nn = 0; + for (kk = 0; kk <= N; kk++) { // writing RSQ[kk] - for (ii = 0; ii < dims->nx[kk] + dims->nu[kk]; ii++) + // we write the lower triangular part in row-major order + // that's the same as writing the upper triangular part in + // column-major order + for (ii = 0; ii < nx[kk] + nu[kk]; ii++) { - for (jj = 0; jj <= ii; jj++) - { - // we write the lower triangular part in row-major order - // that's the same as writing the upper triangular part in - // column-major order - mem->P_x[nn++] = BLASFEO_DMATEL(&in->RSQrq[kk], ii, jj); - } + blasfeo_unpack_dmat(1, ii+1, in->RSQrq+kk, ii, 0, mem->P_x+nn, 1); + nn += ii+1; } + + // write Z[kk] + blasfeo_unpack_dvec(2*ns[kk], in->Z+kk, 0, mem->P_x+nn, 1); + nn += 2*ns[kk]; } } @@ -355,227 +435,404 @@ static void update_hessian_data(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) static void update_constraints_matrix_structure(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - c_int ii, jj, kk, nn = 0, col = 0; - c_int con_start = 0, bnd_start = 0; - c_int row_offset_dyn = 0, row_offset_con = 0, row_offset_bnd = 0; ocp_qp_dims *dims = in->dim; - for (kk = 0; kk <= dims->N; kk++) + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, jj, kk; + + c_int row_offset_dyn = 0, row_offset_con = 0, row_offset_bnd = 0, row_offset_slk = 0; + + c_int con_start = 0, bnd_start = 0, slk_start = 0; + for (kk = 0; kk <= N; kk++) { - con_start += kk < dims->N ? dims->nx[kk + 1] : 0; - bnd_start += dims->ng[kk]; + con_start += kk < N ? nx[kk + 1] : 0; + bnd_start += ng[kk]; + slk_start += nb[kk]; } bnd_start += con_start; + slk_start += bnd_start; // CSC format: A_i are row indices and A_p are column pointers - for (kk = 0; kk <= dims->N; kk++) + c_int nn = 0, col = 0; + for (kk = 0; kk <= N; kk++) { - int nbu = 0; - for (jj = 0; jj < dims->nu[kk]; jj++) + // control variables + for (jj = 0; jj < nu[kk]; jj++) { - mem->A_p[col++] = nn; + mem->A_p[col] = nn; + col++; if (kk < dims->N) { // write column from B - for (ii = 0; ii < dims->nx[kk + 1]; ii++) + for (ii = 0; ii < nx[kk + 1]; ii++) { - mem->A_i[nn++] = ii + row_offset_dyn; + mem->A_i[nn] = row_offset_dyn + ii; + nn++; } } // write column from D - for (ii = 0; ii < dims->ng[kk]; ii++) + for (ii = 0; ii < ng[kk]; ii++) { - mem->A_i[nn++] = ii + con_start + row_offset_con; + mem->A_i[nn] = con_start + row_offset_con + ii; + nn++; } // write bound on u - for (ii = 0; ii < dims->nb[kk]; ii++) + for (ii = 0; ii < nb[kk]; ii++) { if (in->idxb[kk][ii] == jj) { - mem->A_i[nn++] = ii + bnd_start + row_offset_bnd; - nbu++; + mem->A_i[nn] = bnd_start + row_offset_bnd + ii; + nn++; break; } } } - for (jj = 0; jj < dims->nx[kk]; jj++) + // state variables + for (jj = 0; jj < nx[kk]; jj++) { - mem->A_p[col++] = nn; + mem->A_p[col] = nn; + col++; if (kk > 0) { // write column from -I - mem->A_i[nn++] = jj + row_offset_dyn - dims->nx[kk]; + mem->A_i[nn] = row_offset_dyn - nx[kk] + jj; + nn++; } - if (kk < dims->N) + if (kk < N) { // write column from A - for (ii = 0; ii < dims->nx[kk + 1]; ii++) + for (ii = 0; ii < nx[kk + 1]; ii++) { - mem->A_i[nn++] = ii + row_offset_dyn; + mem->A_i[nn] = row_offset_dyn + ii; + nn++; } } // write column from C - for (ii = 0; ii < dims->ng[kk]; ii++) + for (ii = 0; ii < ng[kk]; ii++) { - mem->A_i[nn++] = ii + con_start + row_offset_con; + mem->A_i[nn] = con_start + row_offset_con + ii; + nn++; } // write bound on x - for (ii = 0; ii < dims->nb[kk]; ii++) + for (ii = 0; ii < nb[kk]; ii++) { - if (in->idxb[kk][ii] == jj + dims->nu[kk]) + if (in->idxb[kk][ii] == nu[kk] + jj) { - mem->A_i[nn++] = ii + bnd_start + row_offset_bnd; + mem->A_i[nn] = bnd_start + row_offset_bnd + ii; + nn++; break; } } } - row_offset_bnd += dims->nb[kk]; - row_offset_con += dims->ng[kk]; - row_offset_dyn += kk < dims->N ? dims->nx[kk + 1] : 0; + // slack variables on lower inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + mem->A_p[col] = nn; + col++; + + // soft constraint + for(ii=0; iiidxs_rev[kk][ii]==jj) + { + mem->A_i[nn] = bnd_start + row_offset_bnd + ii; + nn++; + // no break, there could possibly be multiple + } + } + for(ii=0; iiidxs_rev[kk][nb[kk]+ii]==jj) + { + mem->A_i[nn] = con_start + row_offset_con + ii; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_i[nn] = slk_start + row_offset_slk + jj; + nn++; + } + + // slack variables on upper inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + mem->A_p[col] = nn; + col++; + + // soft constraint + for(ii=0; iiidxs_rev[kk][ii]==jj) + { + mem->A_i[nn] = bnd_start + row_offset_bnd + ii; + nn++; + // no break, there could possibly be multiple + } + } + for(ii=0; iiidxs_rev[kk][nb[kk]+ii]==jj) + { + mem->A_i[nn] = con_start + row_offset_con + ii; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_i[nn] = slk_start + row_offset_slk + ns[kk] + jj; + nn++; + } + + row_offset_bnd += nb[kk]; + row_offset_con += ng[kk]; + row_offset_dyn += kk < N ? nx[kk + 1] : 0; + row_offset_slk += 2*ns[kk]; } + // end of matrix mem->A_p[col] = nn; } +// TODO move constant stuff like I to structure routine static void update_constraints_matrix_data(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - c_int ii, jj, kk, nn = 0; ocp_qp_dims *dims = in->dim; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, jj, kk; + + // Traverse matrix in column-major order - for (kk = 0; kk <= dims->N; kk++) + c_int nn = 0; + for (kk = 0; kk <= N; kk++) { - int nbu = 0; - for (jj = 0; jj < dims->nu[kk]; jj++) + // control variables + for (jj = 0; jj < nu[kk]; jj++) { if (kk < dims->N) { // write column from B - for (ii = 0; ii < dims->nx[kk + 1]; ii++) - { - mem->A_x[nn++] = BLASFEO_DMATEL(&in->BAbt[kk], jj, ii); - } + blasfeo_unpack_dmat(1, nx[kk+1], in->BAbt+kk, jj, 0, mem->A_x+nn, 1); + nn += nx[kk+1]; } // write column from D - for (ii = 0; ii < dims->ng[kk]; ii++) - { - mem->A_x[nn++] = BLASFEO_DMATEL(&in->DCt[kk], jj, ii); - } + blasfeo_unpack_dmat(1, ng[kk], in->DCt+kk, jj, 0, mem->A_x+nn, 1); + nn += ng[kk]; // write bound on u for (ii = 0; ii < dims->nb[kk]; ii++) { if (in->idxb[kk][ii] == jj) { - mem->A_x[nn++] = 1.0; - nbu++; + mem->A_x[nn] = 1.0; + nn++; break; } } } - for (jj = 0; jj < dims->nx[kk]; jj++) + // state variables + for (jj = 0; jj < nx[kk]; jj++) { if (kk > 0) { // write column from -I - mem->A_x[nn++] = -1.0; + mem->A_x[nn] = -1.0; + nn++; } - if (kk < dims->N) + if (kk < N) { // write column from A - for (ii = 0; ii < dims->nx[kk + 1]; ii++) + blasfeo_unpack_dmat(1, nx[kk+1], in->BAbt+kk, nu[kk]+jj, 0, mem->A_x+nn, 1); + nn += nx[kk+1]; + } + + // write column from C + blasfeo_unpack_dmat(1, ng[kk], in->DCt+kk, nu[kk]+jj, 0, mem->A_x+nn, 1); + nn += ng[kk]; + + // write bound on x + for (ii = 0; ii < dims->nb[kk]; ii++) + { + if (in->idxb[kk][ii] == dims->nu[kk] + jj) { - mem->A_x[nn++] = BLASFEO_DMATEL(&in->BAbt[kk], jj + dims->nu[kk], ii); + mem->A_x[nn] = 1.0; + nn++; + break; } } + } - // write column from C - for (ii = 0; ii < dims->ng[kk]; ii++) + // slack variables on lower inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + + // soft constraint + for(ii=0; iiA_x[nn++] = BLASFEO_DMATEL(&in->DCt[kk], jj + dims->nu[kk], ii); + if(in->idxs_rev[kk][ii]==jj) + { + mem->A_x[nn] = 1.0; + nn++; + // no break, there could possibly be multiple + } + } + for(ii=0; iiidxs_rev[kk][nb[kk]+ii]==jj) + { + mem->A_x[nn] = 1.0; + nn++; + // no break, there could possibly be multiple + } } - // write bound on x - for (ii = 0; ii < dims->nb[kk]; ii++) + // nonnegativity constraint + mem->A_x[nn] = 1.0; + nn++; + } + + // slack variables on upper inequalities + for (jj = 0; jj < ns[kk]; jj++) + { + + // soft constraint + for(ii=0; iiidxb[kk][ii] == jj + dims->nu[kk]) + if(in->idxs_rev[kk][ii]==jj) { - mem->A_x[nn++] = 1.0; + mem->A_x[nn] = -1.0; + nn++; + // no break, there could possibly be multiple } } + for(ii=0; iiidxs_rev[kk][nb[kk]+ii]==jj) + { + mem->A_x[nn] = -1.0; + nn++; + // no break, there could possibly be multiple + } + } + + // nonnegativity constraint + mem->A_x[nn] = 1.0; + nn++; } + + } + } static void update_bounds(const ocp_qp_in *in, ocp_qp_osqp_memory *mem) { - int ii, kk, nn = 0; ocp_qp_dims *dims = in->dim; + int N = dims->N; + int *nx = dims->nx; + //int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; + + int ii, kk, nn = 0; + // write -b to l and u - for (kk = 0; kk < dims->N; kk++) + for (kk = 0; kk < N; kk++) { // unpack b to l - blasfeo_unpack_dvec(dims->nx[kk + 1], in->b + kk, 0, &mem->l[nn], 1); + blasfeo_unpack_dvec(nx[kk + 1], in->b + kk, 0, &mem->l[nn], 1); // change sign of l (to get -b) and copy to u - for (ii = 0; ii < dims->nx[kk + 1]; ii++) + for (ii = 0; ii < nx[kk + 1]; ii++) { mem->l[nn + ii] = -mem->l[nn + ii]; mem->u[nn + ii] = mem->l[nn + ii]; } - nn += dims->nx[kk + 1]; + nn += nx[kk + 1]; } // write lg and ug - for (kk = 0; kk <= dims->N; kk++) + for (kk = 0; kk <= N; kk++) { // unpack lg to l - blasfeo_unpack_dvec(dims->ng[kk], in->d + kk, dims->nb[kk], &mem->l[nn], 1); + blasfeo_unpack_dvec(ng[kk], in->d + kk, nb[kk], &mem->l[nn], 1); // unpack ug to u and flip signs because in HPIPM the signs are flipped for upper bounds - for (ii = 0; ii < dims->ng[kk]; ii++) + for (ii = 0; ii < ng[kk]; ii++) { - mem->u[nn + ii] = -BLASFEO_DVECEL(&in->d[kk], ii + 2 * dims->nb[kk] + dims->ng[kk]); + mem->u[nn + ii] = -BLASFEO_DVECEL(&in->d[kk], ii + 2 * nb[kk] + ng[kk]); } - nn += dims->ng[kk]; + nn += ng[kk]; } // write lb and ub - for (kk = 0; kk <= dims->N; kk++) + for (kk = 0; kk <= N; kk++) { // unpack lb to l - blasfeo_unpack_dvec(dims->nb[kk], in->d + kk, 0, &mem->l[nn], 1); + blasfeo_unpack_dvec(nb[kk], in->d + kk, 0, &mem->l[nn], 1); // unpack ub to u and flip signs because in HPIPM the signs are flipped for upper bounds - for (ii = 0; ii < dims->nb[kk]; ii++) + for (ii = 0; ii < nb[kk]; ii++) + { + mem->u[nn + ii] = -BLASFEO_DVECEL(&in->d[kk], ii + nb[kk] + ng[kk]); + } + + nn += nb[kk]; + } + + // write ls and us + for (kk = 0; kk <= N; kk++) + { + // unpack ls and us to l + blasfeo_unpack_dvec(2*ns[kk], in->d + kk, 2*nb[kk]+2*ng[kk], &mem->l[nn], 1); + + // OSQP_INF at upper bound + for (ii = 0; ii < 2*ns[kk]; ii++) { - mem->u[nn + ii] = -BLASFEO_DVECEL(&in->d[kk], ii + dims->nb[kk] + dims->ng[kk]); + mem->u[nn + ii] = OSQP_INF; } - nn += dims->nb[kk]; + nn += 2*ns[kk]; } + } @@ -593,6 +850,11 @@ static void ocp_qp_osqp_update_memory(const ocp_qp_in *in, const ocp_qp_osqp_opt update_gradient(in, mem); update_hessian_data(in, mem); update_constraints_matrix_data(in, mem); + + //printf("\nP\n"); + //print_csc_as_dns(mem->osqp_data->P); + //printf("\nA\n"); + //print_csc_as_dns(mem->osqp_data->A); } @@ -1254,56 +1516,87 @@ acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config_, void *dims_, v static void fill_in_qp_out(const ocp_qp_in *in, ocp_qp_out *out, ocp_qp_osqp_memory *mem) { - int ii, kk, nn = 0, mm, con_start = 0, bnd_start = 0; ocp_qp_dims *dims = in->dim; - OSQPSolution *sol = mem->osqp_work->solution; - for (kk = 0; kk <= dims->N; kk++) - { - blasfeo_pack_dvec(dims->nx[kk] + dims->nu[kk], &sol->x[nn], 1, out->ux + kk, 0); - nn += dims->nx[kk] + dims->nu[kk]; + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; - con_start += kk < dims->N ? dims->nx[kk + 1] : 0; - bnd_start += dims->ng[kk]; + int ii, kk, nn, mm; + + c_int con_start = 0, bnd_start = 0, slk_start = 0; + for (kk = 0; kk <= N; kk++) + { + con_start += kk < N ? nx[kk + 1] : 0; + bnd_start += ng[kk]; + slk_start += nb[kk]; } bnd_start += con_start; + slk_start += bnd_start; + //printf("\nstart con bnd slk %d %d %d\n", con_start, bnd_start, slk_start); + + OSQPSolution *sol = mem->osqp_work->solution; + + // primal variables + nn = 0; + for (kk = 0; kk <= N; kk++) + { + blasfeo_pack_dvec(nx[kk]+nu[kk]+2*ns[kk], &sol->x[nn], 1, out->ux + kk, 0); + nn += nx[kk] + nu[kk] + 2*ns[kk]; + } + // dual variables nn = 0; - for (kk = 0; kk < dims->N; kk++) + for (kk = 0; kk < N; kk++) { - blasfeo_pack_dvec(dims->nx[kk + 1], &sol->y[nn], 1, out->pi + kk, 0); - nn += dims->nx[kk + 1]; + blasfeo_pack_dvec(nx[kk + 1], &sol->y[nn], 1, out->pi + kk, 0); + nn += nx[kk + 1]; } nn = 0; mm = 0; - for (kk = 0; kk <= dims->N; kk++) + for (kk = 0; kk <= N; kk++) { - for (ii = 0; ii < 2 * dims->nb[kk] + 2 * dims->ng[kk] + 2 * dims->ns[kk]; ii++) - out->lam[kk].pa[ii] = 0.0; + blasfeo_dvecse(2*nb[kk]+2*ng[kk]+2*ns[kk], 0.0, out->lam+kk, 0); - for (ii = 0; ii < dims->nb[kk]; ii++) + for (ii = 0; ii < nb[kk]; ii++) { double lam = sol->y[bnd_start + nn + ii]; if (lam <= 0) - out->lam[kk].pa[ii] = -lam; + //out->lam[kk].pa[ii] = -lam; + BLASFEO_DVECEL(out->lam+kk, ii) = -lam; else - out->lam[kk].pa[dims->nb[kk] + dims->ng[kk] + ii] = lam; + //out->lam[kk].pa[nb[kk] + ng[kk] + ii] = lam; + BLASFEO_DVECEL(out->lam+kk, nb[kk] + ng[kk] + ii) = lam; } + nn += nb[kk]; - nn += dims->nb[kk]; - - for (ii = 0; ii < dims->ng[kk]; ii++) + for (ii = 0; ii < ng[kk]; ii++) { double lam = sol->y[con_start + mm + ii]; if (lam <= 0) - out->lam[kk].pa[dims->nb[kk] + ii] = -lam; + //out->lam[kk].pa[nb[kk] + ii] = -lam; + BLASFEO_DVECEL(out->lam+kk, nb[kk] + ii) = -lam; else - out->lam[kk].pa[2 * dims->nb[kk] + dims->ng[kk] + ii] = lam; + //out->lam[kk].pa[2 * nb[kk] + ng[kk] + ii] = lam; + BLASFEO_DVECEL(out->lam+kk, 2*nb[kk] + ng[kk] + ii) = lam; } + mm += ng[kk]; - mm += dims->ng[kk]; + blasfeo_dgemv_d(2*ns[kk], 1.0, in->Z+kk, 0, out->ux+kk, nu[kk]+nx[kk], 1.0, in->rqz+kk, nu[kk]+nx[kk], out->lam+kk, 2*nb[kk]+2*ng[kk]); + for(ii=0; iiidxs_rev[kk][ii]; + if(idx!=-1) + { + BLASFEO_DVECEL(out->lam+kk, 2*nb[kk]+2*ng[kk]+idx) -= BLASFEO_DVECEL(out->lam+kk, ii); + BLASFEO_DVECEL(out->lam+kk, 2*nb[kk]+2*ng[kk]+ns[kk]+idx) -= BLASFEO_DVECEL(out->lam+kk, nb[kk]+ng[kk]+ii); + } + } } } @@ -1315,18 +1608,25 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m ocp_qp_out *qp_out = qp_out_; int N = qp_in->dim->N; + int *nb = qp_in->dim->nb; + int *ng = qp_in->dim->ng; int *ns = qp_in->dim->ns; // print_ocp_qp_dims(qp_in->dim); + #if 1 for (int ii = 0; ii <= N; ii++) { - if (ns[ii] > 0) + for (int jj=0; jj<2*ns[ii]; jj++) { - printf("\nOSQP interface can not handle ns>0 yet: what about implementing it? :)\n"); - exit(1); + if (BLASFEO_DVECEL(qp_in->d+ii, 2*nb[ii]+2*ng[ii]+jj)!=0.0) + { + printf("\nOSQP interface can not handle lls!=0 and lus!=0 yet: what about implementing it? :)\n"); + exit(1); + } } } + #endif // print_ocp_qp_in(qp_in); @@ -1386,8 +1686,16 @@ int ocp_qp_osqp(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *m int acados_status = osqp_status; // check exit conditions - if (osqp_status == OSQP_SOLVED) acados_status = ACADOS_SUCCESS; - if (osqp_status == OSQP_MAX_ITER_REACHED) acados_status = ACADOS_MAXITER; + if (osqp_status == OSQP_SOLVED) + { + //printf("\nOSQP solved\n"); + acados_status = ACADOS_SUCCESS; + } + if (osqp_status == OSQP_MAX_ITER_REACHED) + { + //printf("\nOSQP max iter reached\n"); + acados_status = ACADOS_MAXITER; + } mem->status = acados_status; return acados_status; diff --git a/examples/c/no_interface_examples/mass_spring_example.c b/examples/c/no_interface_examples/mass_spring_example.c index 860a8c3039..a4d2a3da70 100644 --- a/examples/c/no_interface_examples/mass_spring_example.c +++ b/examples/c/no_interface_examples/mass_spring_example.c @@ -54,7 +54,7 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims); #define GENERAL_CONSTRAINT_AT_TERMINAL_STAGE -//#define SOFT_CONSTRAINTS +#define SOFT_CONSTRAINTS #define NREP 1 @@ -77,12 +77,11 @@ int main() { // at most nx_/2 for the mass-spring system test problem) int N = 15; // horizon length - int nb_ = 11; // number of box constrained inputs and states + int nb_ = nx_+nu_; //11; // number of box constrained inputs and states int ng_ = 0; // 4; // number of general constraints #ifdef GENERAL_CONSTRAINT_AT_TERMINAL_STAGE - int num_of_stages_equal_to_zero = 4; // number of states to be enforced to zero at last stage - int ngN = num_of_stages_equal_to_zero; + int ngN = nx_/2; // number of states to be enforced to zero at last stage #else int ngN = 0; #endif @@ -95,6 +94,8 @@ int main() { int num_N2_values = 3; int N2_values[3] = {15,10,5}; + int acados_return = 0; + int ii_max = 9; #ifndef ACADOS_WITH_HPMPC @@ -313,8 +314,6 @@ int main() { // printf("\nafter solver create\n"); // exit(1); - int acados_return = 0; - qp_info *info = (qp_info *) qp_out->misc; qp_info min_info; @@ -323,7 +322,9 @@ int main() { // run QP solver NREP times and record min timings for (int rep = 0; rep < NREP; rep++) { - acados_return += ocp_qp_solve(qp_solver, qp_in, qp_out); + int qp_return = ocp_qp_solve(qp_solver, qp_in, qp_out); + acados_return += qp_return; + //printf("\nqp return %d\n", qp_return); if (rep == 0) { @@ -380,5 +381,12 @@ int main() { ocp_qp_out_free(qp_out); } - printf("\nsuccess!\n\n"); + if(acados_return==0) + { + printf("\nsuccess!\n\n"); + } + else + { + printf("\nsome issued happened, acados_return = %d\n\n", acados_return); + } } diff --git a/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c b/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c index ab56806b31..0171e13f69 100644 --- a/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c +++ b/examples/c/no_interface_examples/mass_spring_model/mass_spring_qp.c @@ -862,6 +862,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) double *hzl[N+1]; double *hzu[N+1]; int *hidxs[N+1]; + double *hlls[N+1]; + double *hlus[N+1]; hA[0] = A; hb[0] = b; @@ -883,6 +885,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) hzl[0] = zl0; hzu[0] = zu0; hidxs[0] = idxs0; + hlls[0] = d_ls0; + hlus[0] = d_us0; for (int ii = 1; ii < N; ii++) { hA[ii] = A; hB[ii] = B; @@ -904,6 +908,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) hzl[ii] = zl1; hzu[ii] = zu1; hidxs[ii] = idxs1; + hlls[ii] = d_ls1; + hlus[ii] = d_us1; } hQ[N] = Q; // or maybe initialize to the solution of the DARE??? hq[N] = q; // or maybe initialize to the solution of the DARE??? @@ -918,6 +924,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) hzl[N] = zlN; hzu[N] = zuN; hidxs[N] = idxsN; + hlls[N] = d_lsN; + hlus[N] = d_usN; ocp_qp_in *qp_in = ocp_qp_in_create(dims); @@ -945,6 +953,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) d_ocp_qp_set_zl(ii, hzl[ii], qp_in); d_ocp_qp_set_zu(ii, hzu[ii], qp_in); d_ocp_qp_set_idxs(ii, hidxs[ii], qp_in); + d_ocp_qp_set_lls(ii, hlls[ii], qp_in); + d_ocp_qp_set_lus(ii, hlus[ii], qp_in); } d_ocp_qp_set_R(ii, hR[ii], qp_in); d_ocp_qp_set_S(ii, hS[ii], qp_in); @@ -963,6 +973,8 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(ocp_qp_dims *dims) d_ocp_qp_set_zl(ii, hzl[ii], qp_in); d_ocp_qp_set_zu(ii, hzu[ii], qp_in); d_ocp_qp_set_idxs(ii, hidxs[ii], qp_in); + d_ocp_qp_set_lls(ii, hlls[ii], qp_in); + d_ocp_qp_set_lus(ii, hlus[ii], qp_in); // d_ocp_qp_set_idxbxe(0, idxbxe0, qp_in); From 7a26663052326281738bde29fe7e7aeef626a48b Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 21 May 2024 18:09:02 +0200 Subject: [PATCH 4/4] remove some unused variables --- acados/ocp_qp/ocp_qp_common.c | 2 -- 1 file changed, 2 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index a8d9b101d8..90205a683b 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -643,8 +643,6 @@ void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out) struct blasfeo_dvec *ux = qp_out->ux; struct blasfeo_dvec *t = qp_out->t; - int nx_i, nu_i, nb_i, ng_i; - for (ii = 0; ii <= N; ii++) { // compute slacks for bounds