diff --git a/.travis_install.sh b/.travis_install.sh index f9de761ccd..7af47d1077 100644 --- a/.travis_install.sh +++ b/.travis_install.sh @@ -2,7 +2,7 @@ sudo add-apt-repository -y ppa:octave/stable sudo apt-get update -yqq -sudo apt-get install -yqq $CXX $CC $COVERAGE libgsl0-dev liblapack-dev libopenblas-dev liboctave-dev mingw-w64 bsdtar +sudo apt-get --allow-unauthenticated install -yqq $CXX $CC $COVERAGE libgsl0-dev liblapack-dev libopenblas-dev liboctave-dev mingw-w64 bsdtar pip install numpy scipy matplotlib diff --git a/acados/CMakeLists.txt b/acados/CMakeLists.txt index 15d457bde6..b03207a1e9 100644 --- a/acados/CMakeLists.txt +++ b/acados/CMakeLists.txt @@ -62,6 +62,7 @@ target_include_directories(acados $ $ $ + $ $ $ $) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 0de739b489..dde27e5d41 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -117,6 +117,8 @@ dense_qp_in *dense_qp_in_assign(void *config, dense_qp_dims *dims, void *raw_mem qp_in->dim->nb = dims->nb; qp_in->dim->ng = dims->ng; qp_in->dim->ns = dims->ns; + qp_in->dim->nsb = dims->nsb; + qp_in->dim->nsg = dims->nsg; assert((char *) raw_memory + dense_qp_in_calculate_size(config, dims) == c_ptr); @@ -203,8 +205,7 @@ dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_me return res_ws; } -void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, - dense_qp_res_ws *res_ws) +void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out) { int nvd = qp_in->dim->nv; // int ned = qp_in->dim->ne; @@ -215,7 +216,10 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res int *idxb = qp_in->idxb; int *idxs = qp_in->idxs; - struct blasfeo_dvec *tmp_nbg = res_ws->tmp_nbg; + // compute slacks for bounds + blasfeo_dvecex_sp(nbd, 1.0, idxb, qp_out->v, 0, qp_out->t, nbd+ngd); + blasfeo_daxpby(nbd, 1.0, qp_out->t, nbd+ngd, -1.0, qp_in->d, 0, qp_out->t, 0); + blasfeo_daxpby(nbd, -1.0, qp_out->t, nbd+ngd, -1.0, qp_in->d, nbd + ngd, qp_out->t, nbd + ngd); // compute slacks for general constraints blasfeo_dgemv_t(nvd, ngd, 1.0, qp_in->Ct, 0, 0, qp_out->v, 0, -1.0, qp_in->d, nbd, qp_out->t, @@ -223,16 +227,25 @@ void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res blasfeo_dgemv_t(nvd, ngd, -1.0, qp_in->Ct, 0, 0, qp_out->v, 0, -1.0, qp_in->d, 2 * nbd + ngd, qp_out->t, 2 * nbd + ngd); - // compute slacks for bounds - blasfeo_dvecex_sp(nbd, 1.0, idxb, qp_out->v, 0, tmp_nbg + 0, 0); - blasfeo_daxpby(nbd, 1.0, tmp_nbg + 0, 0, -1.0, qp_in->d, 0, qp_out->t, 0); - blasfeo_daxpby(nbd, -1.0, tmp_nbg + 0, 0, -1.0, qp_in->d, nbd + ngd, qp_out->t, nbd + ngd); - // soft blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd, idxs, qp_out->t, 0); blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd+nsd, idxs, qp_out->t, nbd+ngd); -// blasfeo_daxpy(2*nsd, -1.0, qp_out->v, nvd, qp_out->t, 2*nbd+2*ngd, qp_out->t, 2*nbd+2*ngd); - blasfeo_dvecse(2*nsd, 0.0, qp_out->t, 2*nbd+2*ngd); + blasfeo_dveccp(2*nsd, qp_out->v, nvd, qp_out->t, 2*nbd+2*ngd); + blasfeo_daxpy(2*nsd, -1.0, qp_in->d, 2*nbd+2*ngd, qp_out->t, 2*nbd+2*ngd, qp_out->t, + 2*nbd+2*ngd); + +} + +void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, + dense_qp_res_ws *res_ws) +{ + dense_qp_info *info = (dense_qp_info *) qp_out->misc; + + if (info->t_computed == 0) + { + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; + } // compute residuals d_compute_res_dense_qp(qp_in, qp_out, qp_res, res_ws); @@ -255,3 +268,274 @@ void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]) } + +void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out) +{ + out->nv = in->nv + 2 * in->ns; + out->ne = in->ne; + out->nb = in->nb - in->nsb + 2 * in->ns; + out->ng = in->ns > 0 ? in->ng + in->nsb : in->ng; + out->ns = 0; + out->nsb = 0; + out->nsg = 0; +} + + + +void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) +{ + int nv = in->dim->nv; + int ne = in->dim->ne; + int nb = in->dim->nb; + int ng = in->dim->ng; + int ns = in->dim->ns; + int nsb = in->dim->nsb; + int nsg = in->dim->nsg; + int *idxs = in->idxs; + int *idxb = in->idxb; + + int nv2 = out->dim->nv; + int ne2 = out->dim->ne; + int nb2 = out->dim->nb; + int ng2 = out->dim->ng; + + assert(nv2 == nv+2*ns && "Dimensions are wrong!"); + assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!"); + assert(ng2 == ng+nsb && "Dimensions are wrong!"); + + // set matrices to 0.0 + blasfeo_dgese(nv2, nv2, 0.0, out->Hv, 0, 0); + blasfeo_dgese(ne2, nv2, 0.0, out->A, 0, 0); + blasfeo_dgese(nv2, ng2, 0.0, out->Ct, 0, 0); + + // copy in->Hv to upper left corner of out->Hv, out->Hv = [in->Hv 0; 0 0] + blasfeo_dgecp(nv, nv, in->Hv, 0, 0, out->Hv, 0, 0); + + // copy in->Z to main diagonal of out->Hv, out->Hv = [in->Hv 0; 0 Z] + blasfeo_ddiain(2 * ns, 1.0, in->Z, 0, out->Hv, nv, nv); + + // copy in->gz to out->gz + blasfeo_dveccp(nv + 2 * ns, in->gz, 0, out->gz, 0); + + if (ne > 0) + { + // copy in->A to out->A + blasfeo_dgecp(ne, nv, in->A, 0, 0, out->A, 0, 0); + + // copy in->b to out->b + blasfeo_dveccp(ne, in->b, 0, out->b, 0); + } + + // copy in->Ct to out->Ct, out->Ct = [in->Ct 0; 0 0] + blasfeo_dgecp(nv, ng, in->Ct, 0, 0, out->Ct, 0, 0); + + if (ns > 0) + { + // set flags for non-softened box constraints + // use out->m temporarily for this + for (int ii = 0; ii < nb; ii++) + { + // TODO(bnovoselnik): pick up some workspace for this + BLASFEO_DVECEL(out->m, ii) = 1.0; + } + + int col_b = ng; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + int idx_v_ls0 = nv+ii; + int idx_v_us0 = nv+ns+ii; + int idx_v_ls1 = nv+ii; + int idx_v_us1 = nv+ns+ii; + + int idx_d_ls0 = js; + int idx_d_us0 = nb+ng+js; + int idx_d_ls1; + int idx_d_us1; + + if (js < nb) // soft box constraint + { + // index of a soft box constraint + int jv = idxb[js]; + + idx_d_ls1 = nb2+col_b; + idx_d_us1 = 2*nb2+ng2+col_b; + + // softened box constraint, set its flag to -1 + BLASFEO_DVECEL(out->m, js) = -1.0; + + // insert softened box constraint into out->Ct, lb_i <= x_i + sl_i - su_i <= ub_i + BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0; + BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_b) = +1.0; + BLASFEO_DMATEL(out->Ct, idx_v_us1, col_b) = -1.0; + BLASFEO_DVECEL(out->d, idx_d_ls1) = BLASFEO_DVECEL(in->d, idx_d_ls0); + BLASFEO_DVECEL(out->d, idx_d_us1) = -BLASFEO_DVECEL(in->d, idx_d_us0); + + col_b++; + } + else // soft general constraint + { + // index of a soft general constraint + int col_g = js - nb; + + // soft general constraint, lg_i <= C_i x + sl_i - su_i <= ug_i + BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_g) = +1.0; + BLASFEO_DMATEL(out->Ct, idx_v_us1, col_g) = -1.0; + } + + // slack variables have box constraints + out->idxb[nb-nsb+ii] = ii + nv; + out->idxb[nb-nsb+ns+ii] = ii + nv + ns; + } + + int k_nsb = 0; + for (int ii = 0; ii < nb; ii++) + { + if (BLASFEO_DVECEL(out->m, ii) > 0) + { + // copy nonsoftened box constraint bounds to out->d + BLASFEO_DVECEL(out->d, k_nsb) = BLASFEO_DVECEL(in->d, ii); + BLASFEO_DVECEL(out->d, nb2+ng2+k_nsb) = -BLASFEO_DVECEL(in->d, nb+ng+ii); + out->idxb[k_nsb] = ii; + k_nsb++; + } + } + + assert(k_nsb == nb-nsb && "Dimensions are wrong!"); + + // copy ls and us to out->lb, jump over nonsoftened box constraints + blasfeo_dveccp(2*ns, in->d, 2*nb+2*ng, out->d, k_nsb); + + // for slack variables out->ub is +INFTY + blasfeo_dvecse(2*ns, 1.0e6, out->d, nb2+ng2+k_nsb); + + // copy in->lg to out->lg + blasfeo_dveccp(ng, in->d, nb, out->d, nb2); + + // copy in->ug to out->ug + blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2); + + // flip signs for ub and ug + blasfeo_dvecsc(nb2+ng2, -1.0, out->d, nb2+ng2); + + // set out->m to 0.0 + blasfeo_dvecse(2*nb2+2*ng2, 0.0, out->m, 0); + } + else + { + blasfeo_dveccp(2*nb+2*ng, in->d, 0, out->d, 0); + blasfeo_dveccp(2*nb+2*ng, in->m, 0, out->m, 0); + for (int ii = 0; ii < nb; ii++) out->idxb[ii] = in->idxb[ii]; + } +} + + + +void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out) +{ + int nv = qp_out->dim->nv; + int ne = qp_out->dim->ne; + int nb = qp_out->dim->nb; + int ng = qp_out->dim->ng; + int ns = qp_out->dim->ns; + int nsb = qp_out->dim->nsb; + int nsg = qp_out->dim->nsg; + + int *idxs = qp_out->idxs; + + int nv2 = in->dim->nv; + int ne2 = in->dim->ne; + int nb2 = in->dim->nb; + int ng2 = in->dim->ng; + + assert(nv2 == nv+2*ns && "Dimensions are wrong!"); + assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!"); + assert(ng2 == 2*ng+2*nsb && "Dimensions are wrong!"); + + // inequality constraints multipliers and slacks + if (ns > 0) + { + + // set flags for non-softened box constraints + // use out->v temporarily for this + // XXX assume that nb<=nv + assert(nv+2*ns >= nb); + + for (int ii = 0; ii < nb; ii++) + { + // TODO(bnovoselnik): pick up some workspace for this + BLASFEO_DVECEL(out->v, ii) = 1.0; + } + + int col_b = 2*ng; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + int idx_d_ls0 = js; + int idx_d_us0 = nb+ng+js; + int idx_d_ls1; + int idx_d_us1; + + if (js < nb) + { + // softened box constraint, set its flag to -1 + BLASFEO_DVECEL(out->v, js) = -1.0; + + idx_d_ls1 = nb2+col_b; + idx_d_us1 = 2*nb2+ng2+col_b; + + BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1); + BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1); + + BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1); + BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1); + + col_b++; + } + } + + int k_nsb = 0; // number of non-softed bounds + for (int ii = 0; ii < nb; ii++) + { + int idx_d_ls0 = ii; + int idx_d_us0 = nb+ng+ii; + int idx_d_ls1; + int idx_d_us1; + + if (BLASFEO_DVECEL(out->v, ii) > 0) + { + idx_d_ls1 = k_nsb; + idx_d_us1 = nb2+ng2+k_nsb; + + BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1); + BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1); + + BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1); + BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1); + + k_nsb++; + } + } + + assert(k_nsb == nb-nsb && "Dimensions are wrong!"); + + blasfeo_dveccp(2*ns, in->t, k_nsb, out->t, 2*nb+2*ng); + blasfeo_dveccp(ng, in->t, 2*nb2+ng2, out->t, 2*nb+ng); + blasfeo_dveccp(ng, in->t, nb2, out->t, nb); + + blasfeo_dveccp(2*ns, in->lam, k_nsb, out->lam, 2*nb+2*ng); + blasfeo_dveccp(ng, in->lam, 2*nb2+ng2, out->lam, 2*nb+ng); + blasfeo_dveccp(ng, in->lam, nb2, out->lam, nb); + + // variables + blasfeo_dveccp(nv+2*ns, in->v, 0, out->v, 0); + + // equality constraints multipliers + blasfeo_dveccp(ne, in->pi, 0, out->pi, 0); + + } + + return; +} diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index bf7c85a397..faf706d76f 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -61,6 +61,7 @@ typedef struct double interface_time; double total_time; int num_iter; + int t_computed; } dense_qp_info; // @@ -88,11 +89,18 @@ int dense_qp_res_workspace_calculate_size(dense_qp_dims *dims); // dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory); // +void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out); +// void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws); // void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]); // +void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); +// +void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out); +// +void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index de1f7bfb09..4a5e4b96d8 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -72,7 +72,7 @@ void dense_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *op { dense_qp_hpipm_opts *opts = opts_; - d_set_default_dense_qp_ipm_arg(opts->hpipm_opts); + d_set_default_dense_qp_ipm_arg(BALANCE, opts->hpipm_opts); // overwrite some default options opts->hpipm_opts->res_g_max = 1e-6; opts->hpipm_opts->res_b_max = 1e-8; @@ -163,6 +163,7 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void info->interface_time = 0; // there are no conversions for hpipm info->total_time = acados_toc(&tot_timer); info->num_iter = memory->hpipm_workspace->iter; + info->t_computed = 1; // check exit conditions int acados_status = hpipm_status; diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 68b11de2f1..14475a6658 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -65,7 +65,8 @@ void dense_qp_qore_opts_initialize_default(void *config_, dense_qp_dims *dims, v opts->warm_strategy = 0; opts->nsmax = 400; opts->hot_start = 0; - opts->max_iter = 100; + opts->max_iter = 1000; + opts->compute_t = 1; return; } @@ -84,28 +85,48 @@ void dense_qp_qore_opts_update(void *config_, dense_qp_dims *dims, void *opts_) int dense_qp_qore_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) { dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_; + dense_qp_dims dims_stacked; - int nvd = dims->nv; - int ned = dims->ne; - int ngd = dims->ng; - int nbd = dims->nb; - int nsmax = (2 * nvd >= opts->nsmax) ? opts->nsmax : 2 * nvd; + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int ns = dims->ns; + int nsb = dims->nsb; + int nsg = dims->nsg; + int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv; + + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2*ns; // size in bytes int size = sizeof(dense_qp_qore_memory); - size += 1 * nvd * nvd * sizeof(double); // H - size += 1 * nvd * ned * sizeof(double); // A - size += 2 * nvd * ngd * sizeof(double); // C, Ct - size += 3 * nvd * sizeof(double); // g d_lb d_ub - size += 1 * ned * sizeof(double); // b - size += 2 * nbd * sizeof(double); // d_lb0 d_ub0 - size += 2 * ngd * sizeof(double); // d_lg d_ug - size += 1 * nbd * sizeof(int); // idxb - size += 2 * (nvd + ngd) * sizeof(double); // lb, ub - size += (nvd + ngd) * sizeof(double); // prim_sol - size += (nvd + ngd) * sizeof(double); // dual_sol - size += QPDenseSize(nvd, ngd, nsmax); + size += 1 * nv * nv * sizeof(double); // H + size += 1 * nv2 * nv2 * sizeof(double); // HH + size += 1 * nv2 * ne * sizeof(double); // A + size += 2 * nv * ng * sizeof(double); // C, Ct + size += 2 * nv2 * ng2 * sizeof(double); // CC, CCt + size += 1 * nv * sizeof(double); // g + size += 3 * nv2 * sizeof(double); // gg d_lb d_ub + size += 1 * ne * sizeof(double); // b + size += 2 * nb2 * sizeof(double); // d_lb0 d_ub0 + size += 2 * ng2 * sizeof(double); // d_lg d_ug + size += 1 * nb * sizeof(int); // idxb + size += 1 * nb2 * sizeof(int); // idxb_stacked + size += 1 * ns * sizeof(int); + size += 2 * (nv2 + ng2) * sizeof(double); // lb, ub + size += 1 * (nv2 + ng2) * sizeof(double); // prim_sol + size += 1 * (nv2 + ng2) * sizeof(double); // dual_sol + size += 6 * ns * sizeof(double); // Zl, Zu, zl, zu, d_ls, d_us + size += QPDenseSize(nv2, ng2, nsmax); + + if (ns > 0) + { + dense_qp_stack_slacks_dims(dims, &dims_stacked); + size += dense_qp_in_calculate_size(config_, &dims_stacked); + } make_int_multiple_of(8, &size); @@ -116,12 +137,20 @@ void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts { dense_qp_qore_memory *mem; dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_; + dense_qp_dims dims_stacked; + + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int ns = dims->ns; + int nsb = dims->nsb; + int nsg = dims->nsg; + int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv; - int nvd = dims->nv; - int ned = dims->ne; - int ngd = dims->ng; - int nbd = dims->nb; - int nsmax = (2 * nvd >= opts->nsmax) ? opts->nsmax : 2 * nvd; + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2*ns; // char pointer char *c_ptr = (char *) raw_memory; @@ -129,33 +158,58 @@ void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts mem = (dense_qp_qore_memory *) c_ptr; c_ptr += sizeof(dense_qp_qore_memory); + assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); + + if (ns > 0) + { + dense_qp_stack_slacks_dims(dims, &dims_stacked); + mem->qp_stacked = dense_qp_in_assign(config_, &dims_stacked, c_ptr); + c_ptr += dense_qp_in_calculate_size(config_, &dims_stacked); + } + else + { + mem->qp_stacked = NULL; + } + assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); - assign_and_advance_double(nvd * nvd, &mem->H, &c_ptr); - assign_and_advance_double(nvd * ned, &mem->A, &c_ptr); - assign_and_advance_double(nvd * ngd, &mem->C, &c_ptr); - assign_and_advance_double(nvd * ngd, &mem->Ct, &c_ptr); - assign_and_advance_double(nvd, &mem->g, &c_ptr); - assign_and_advance_double(ned, &mem->b, &c_ptr); - assign_and_advance_double(nbd, &mem->d_lb0, &c_ptr); - assign_and_advance_double(nbd, &mem->d_ub0, &c_ptr); - assign_and_advance_double(nvd, &mem->d_lb, &c_ptr); - assign_and_advance_double(nvd, &mem->d_ub, &c_ptr); - assign_and_advance_double(ngd, &mem->d_lg, &c_ptr); - assign_and_advance_double(ngd, &mem->d_ug, &c_ptr); - assign_and_advance_double(nvd + ngd, &mem->lb, &c_ptr); - assign_and_advance_double(nvd + ngd, &mem->ub, &c_ptr); - assign_and_advance_double(nvd + ngd, &mem->prim_sol, &c_ptr); - assign_and_advance_double(nvd + ngd, &mem->dual_sol, &c_ptr); + assign_and_advance_double(nv * nv, &mem->H, &c_ptr); + assign_and_advance_double(nv2 * nv2, &mem->HH, &c_ptr); + assign_and_advance_double(nv2 * ne, &mem->A, &c_ptr); + assign_and_advance_double(nv * ng, &mem->C, &c_ptr); + assign_and_advance_double(nv * ng, &mem->Ct, &c_ptr); + assign_and_advance_double(nv2 * ng2, &mem->CC, &c_ptr); + assign_and_advance_double(nv2 * ng2, &mem->CCt, &c_ptr); + assign_and_advance_double(nv, &mem->g, &c_ptr); + assign_and_advance_double(nv2, &mem->gg, &c_ptr); + assign_and_advance_double(ne, &mem->b, &c_ptr); + assign_and_advance_double(nb2, &mem->d_lb0, &c_ptr); + assign_and_advance_double(nb2, &mem->d_ub0, &c_ptr); + assign_and_advance_double(nv2, &mem->d_lb, &c_ptr); + assign_and_advance_double(nv2, &mem->d_ub, &c_ptr); + assign_and_advance_double(ng2, &mem->d_lg, &c_ptr); + assign_and_advance_double(ng2, &mem->d_ug, &c_ptr); + assign_and_advance_double(ns, &mem->Zl, &c_ptr); + assign_and_advance_double(ns, &mem->Zu, &c_ptr); + assign_and_advance_double(ns, &mem->zl, &c_ptr); + assign_and_advance_double(ns, &mem->zu, &c_ptr); + assign_and_advance_double(ns, &mem->d_ls, &c_ptr); + assign_and_advance_double(ns, &mem->d_us, &c_ptr); + assign_and_advance_double(nv2 + ng2, &mem->lb, &c_ptr); + assign_and_advance_double(nv2 + ng2, &mem->ub, &c_ptr); + assign_and_advance_double(nv2 + ng2, &mem->prim_sol, &c_ptr); + assign_and_advance_double(nv2 + ng2, &mem->dual_sol, &c_ptr); assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); mem->QP = (QoreProblemDense *) c_ptr; - QPDenseCreate(&mem->QP, nvd, ngd, nsmax, c_ptr); - c_ptr += QPDenseSize(nvd, ngd, nsmax); + QPDenseCreate(&mem->QP, nv2, ng2, nsmax, c_ptr); + c_ptr += QPDenseSize(nv2, ng2, nsmax); // int stuff - assign_and_advance_int(nbd, &mem->idxb, &c_ptr); + assign_and_advance_int(nb, &mem->idxb, &c_ptr); + assign_and_advance_int(nb2, &mem->idxb_stacked, &c_ptr); + assign_and_advance_int(ns, &mem->idxs, &c_ptr); assert((char *) raw_memory + dense_qp_qore_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -183,6 +237,8 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void acados_timer tot_timer, qp_timer, interface_timer; acados_tic(&tot_timer); + acados_tic(&interface_timer); + info->t_computed = 0; // cast structures dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_; @@ -190,10 +246,14 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void // extract qpoases data double *H = memory->H; + double *HH = memory->HH; double *A = memory->A; double *C = memory->C; + double *CC = memory->CC; double *Ct = memory->Ct; + double *CCt = memory->CCt; double *g = memory->g; + double *gg = memory->gg; double *b = memory->b; double *d_lb0 = memory->d_lb0; double *d_ub0 = memory->d_ub0; @@ -203,57 +263,92 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void double *d_ug = memory->d_ug; double *lb = memory->lb; double *ub = memory->ub; + double *Zl = memory->Zl; + double *Zu = memory->Zu; + double *zl = memory->zl; + double *zu = memory->zu; + double *d_ls = memory->d_ls; + double *d_us = memory->d_us; int *idxb = memory->idxb; + int *idxb_stacked = memory->idxb_stacked; + int *idxs = memory->idxs; QoreProblemDense *QP = memory->QP; + double *prim_sol = memory->prim_sol; + double *dual_sol = memory->dual_sol; + dense_qp_in *qp_stacked = memory->qp_stacked; // extract dense qp size - int nvd = qp_in->dim->nv; - int ngd = qp_in->dim->ng; - int nbd = qp_in->dim->nb; - int nsd = qp_in->dim->ns; + int nv = qp_in->dim->nv; + int ng = qp_in->dim->ng; + int nb = qp_in->dim->nb; + int ns = qp_in->dim->ns; + int nsb = qp_in->dim->nsb; + int nsg = qp_in->dim->nsg; - if (nsd > 0) - { - printf("\nQORE interface can not handle ns>0 yet: what about implementing it? :)\n"); - return ACADOS_FAILURE; - } - - acados_tic(&interface_timer); + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2 * ns; // fill in the upper triangular of H in dense_qp - blasfeo_dtrtr_l(nvd, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); + blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); - // dense qp row-major - d_cvt_dense_qp_to_colmaj(qp_in, H, g, A, b, idxb, d_lb0, d_ub0, C, d_lg, d_ug, NULL, NULL, NULL, - NULL, NULL, NULL, NULL); + // extract data from qp_in in col-major + d_cvt_dense_qp_to_colmaj(qp_in, H, gg, A, b, idxb, d_lb0, d_ub0, C, d_lg, d_ug, + Zl, Zu, zl, zu, idxs, d_ls, d_us); // reorder bounds - for (int ii = 0; ii < nvd; ii++) + for (int ii = 0; ii < nv2; ii++) { d_lb[ii] = -INFINITY; d_ub[ii] = +INFINITY; } - for (int ii = 0; ii < nbd; ii++) - { - d_lb[idxb[ii]] = d_lb0[ii]; - d_ub[idxb[ii]] = d_ub0[ii]; - } - memcpy(lb, d_lb, nvd * sizeof(double)); - memcpy(lb + nvd, d_lg, ngd * sizeof(double)); + if (ns > 0) + { + dense_qp_stack_slacks(qp_in, qp_stacked); + d_cvt_dense_qp_to_colmaj(qp_stacked, HH, gg, A, b, idxb_stacked, d_lb0, d_ub0, CC, d_lg, + d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL); - memcpy(ub, d_ub, nvd * sizeof(double)); - memcpy(ub + nvd, d_ug, ngd * sizeof(double)); + for (int ii = 0; ii < nb2; ii++) + { + d_lb[idxb_stacked[ii]] = d_lb0[ii]; + d_ub[idxb_stacked[ii]] = d_ub0[ii]; + } - /* transpose C as expected by QORE */ - int i, j; - for (j = 0; j < nvd; j++) + // transpose CC as expected by QORE + for (int j = 0; j < nv2; j++) + { + for (int i = 0; i < ng2; i++) + { + CCt[j + i * nv2] = CC[i + j * ng2]; + } + } + } + else { - for (i = 0; i < ngd; i++) + for (int ii = 0; ii < nb; ii++) { - Ct[j + i * nvd] = C[i + j * ngd]; + d_lb[idxb[ii]] = d_lb0[ii]; + d_ub[idxb[ii]] = d_ub0[ii]; + } + + // transpose C as expected by QORE + for (int j = 0; j < nv; j++) + { + for (int i = 0; i < ng; i++) + { + Ct[j + i * nv] = C[i + j * ng]; + } } } + + memcpy(lb, d_lb, nv2 * sizeof(double)); + memcpy(lb + nv2, d_lg, ng2 * sizeof(double)); + + memcpy(ub, d_ub, nv2 * sizeof(double)); + memcpy(ub + nv2, d_ug, ng2 * sizeof(double)); + + info->interface_time = acados_toc(&interface_timer); // solve dense qp @@ -262,21 +357,21 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void if (opts->warm_start) { QPDenseSetInt(QP, "warmstrategy", opts->warm_strategy); - QPDenseUpdateMatrices(QP, nvd, ngd, Ct, H); + (ns > 0) ? QPDenseUpdateMatrices(QP, nv2, ng2, CCt, HH) : + QPDenseUpdateMatrices(QP, nv, ng, Ct, H); } else if (!opts->hot_start) { - QPDenseSetData(QP, nvd, ngd, Ct, H); + (ns > 0) ? QPDenseSetData(QP, nv2, ng2, CCt, HH) : + QPDenseSetData(QP, nv, ng, Ct, H); } QPDenseSetInt(QP, "maxiter", opts->max_iter); QPDenseSetInt(QP, "prtfreq", opts->print_freq); - QPDenseOptimize(QP, lb, ub, g, 0, 0); + QPDenseOptimize(QP, lb, ub, gg, 0, 0); int qore_status; QPDenseGetInt(QP, "status", &qore_status); - double *prim_sol = memory->prim_sol; - double *dual_sol = memory->dual_sol; QPDenseGetDblVector(QP, "primalsol", prim_sol); QPDenseGetDblVector(QP, "dualsol", dual_sol); int num_iter; @@ -287,27 +382,73 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void acados_tic(&interface_timer); // copy prim_sol and dual_sol to qpd_sol - blasfeo_pack_dvec(nvd, prim_sol, qp_out->v, 0); - for (int ii = 0; ii < 2 * nbd + 2 * ngd; ii++) qp_out->lam->pa[ii] = 0.0; - for (int ii = 0; ii < nbd; ii++) + blasfeo_pack_dvec(nv2, prim_sol, qp_out->v, 0); + for (int ii = 0; ii < 2 * nb + 2 * ng + 2 * ns; ii++) qp_out->lam->pa[ii] = 0.0; + for (int ii = 0; ii < nb; ii++) { if (dual_sol[idxb[ii]] >= 0.0) qp_out->lam->pa[ii] = dual_sol[idxb[ii]]; else - qp_out->lam->pa[nbd + ngd + ii] = -dual_sol[idxb[ii]]; + qp_out->lam->pa[nb + ng + ii] = -dual_sol[idxb[ii]]; } - for (int ii = 0; ii < ngd; ii++) + + for (int ii = 0; ii < ng; ii++) { - if (dual_sol[nvd + ii] >= 0.0) - qp_out->lam->pa[nbd + ii] = dual_sol[nvd + ii]; + if (dual_sol[nv2 + ii] >= 0.0) + qp_out->lam->pa[nb + ii] = dual_sol[nv2 + ii]; else - qp_out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; + } + + int k = 0; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + double offset_l = 0.0; + double offset_u = 0.0; + + if (js < nb) + { + if (dual_sol[nv2 + ng + k] <= 0.0) // softened upper box constraints + { + qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + ng + k]; + offset_u = -dual_sol[nv2 + ng + k]; + } + else // softened lower box constraints + { + qp_out->lam->pa[js] = dual_sol[nv2 + ng + k]; + offset_l = dual_sol[nv2 + ng + k]; + } + + k++; + } + else + { + offset_l = qp_out->lam->pa[nb+js-nb]; + offset_u = qp_out->lam->pa[2*nb+ng+js-nb]; + } + + // dual variables for sl >= d_ls + if (dual_sol[nv + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ii] = dual_sol[nv + ii] - offset_u; + + // dual variables for su >= d_us + if (dual_sol[nv + ns + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = num_iter; + // compute slacks + if (opts->compute_t) + { + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; + } + int acados_status = qore_status; if (qore_status == QPSOLVER_DENSE_OPTIMAL) acados_status = ACADOS_SUCCESS; if (qore_status == QPSOLVER_DENSE_ITER_LIMIT) acados_status = ACADOS_MAXITER; diff --git a/acados/dense_qp/dense_qp_qore.h b/acados/dense_qp/dense_qp_qore.h index 6c0b38e4ca..2800dd743b 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -43,29 +43,43 @@ typedef struct dense_qp_qore_opts_ // solution int hot_start; // hot start with unchanged matrices H and C int max_iter; // maximum number of iterations + int compute_t; // compute t in qp_out (to have correct residuals in NLP) } dense_qp_qore_opts; typedef struct dense_qp_qore_memory_ { double *H; + double *HH; double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; double *A; double *b; double *C; + double *CC; double *Ct; + double *CCt; double *d_lb0; double *d_ub0; double *d_lb; double *d_ub; double *d_lg; double *d_ug; + double *d_ls; + double *d_us; double *lb; double *ub; int *idxb; + int *idxb_stacked; + int *idxs; double *prim_sol; double *dual_sol; QoreProblemDense *QP; int num_iter; + dense_qp_in *qp_stacked; } dense_qp_qore_memory; int dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims); diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 9ad2cdf39e..8a1f0db5cc 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -57,6 +57,9 @@ #include "acados/dense_qp/dense_qp_qpoases.h" #include "acados/utils/mem.h" #include "acados/utils/timing.h" +#include "acados/utils/print.h" + +#include "acados_c/dense_qp_interface.h" /************************************************ * opts @@ -112,30 +115,53 @@ void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) { - int nvd = dims->nv; - int ned = dims->ne; - int ngd = dims->ng; - int nbd = dims->nb; + dense_qp_dims dims_stacked; + + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int nsb = dims->nsb; + int nsg = dims->nsg; + int ns = dims->ns; + + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2 * ns; // size in bytes int size = sizeof(dense_qp_qpoases_memory); - size += 1 * nvd * nvd * sizeof(double); // H - size += 1 * nvd * nvd * sizeof(double); // R - size += 1 * nvd * ned * sizeof(double); // A - size += 1 * nvd * ngd * sizeof(double); // C - size += 3 * nvd * sizeof(double); // g d_lb d_ub - size += 1 * ned * sizeof(double); // b - size += 2 * nbd * sizeof(double); // d_lb0 d_ub0 - size += 2 * ngd * sizeof(double); // d_lg d_ug - size += 1 * nbd * sizeof(int); // idxb - size += 1 * nvd * sizeof(double); // prim_sol - size += (nvd + ngd) * sizeof(double); // dual_sol - - if (ngd > 0) // QProblem - size += QProblem_calculateMemorySize(nvd, ngd); + size += 1 * nv * nv * sizeof(double); // H + size += 1 * nv2 * nv2 * sizeof(double); // HH + size += 1 * nv2 * nv2 * sizeof(double); // R + size += 1 * nv2 * ne * sizeof(double); // A + size += 1 * nv * ng * sizeof(double); // C + size += 1 * nv2 * ng2 * sizeof(double); // CC + size += 1 * nv * sizeof(double); // g + size += 1 * nv2 * sizeof(double); // gg + size += 2 * nv2 * sizeof(double); // d_lb d_ub + size += 1 * ne * sizeof(double); // b + size += 2 * nb2 * sizeof(double); // d_lb0 d_ub0 + size += 2 * ng * sizeof(double); // d_lg0 d_ug0 + size += 2 * ng2 * sizeof(double); // d_lg d_ug + size += 1 * nb * sizeof(int); // idxb + size += 1 * nb2 * sizeof(int); // idxb_stacked + size += 1 * ns * sizeof(int); // idxs + size += 1 * nv2 * sizeof(double); // prim_sol + size += 1 * (nv2 + ng2) * sizeof(double); // dual_sol + size += 6 * ns * sizeof(double); // Zl, Zu, zl, zu, d_ls, d_us + + if (ns > 0) + { + dense_qp_stack_slacks_dims(dims, &dims_stacked); + size += dense_qp_in_calculate_size(config_, &dims_stacked); + } + + if (ng > 0 || ns > 0) // QProblem + size += QProblem_calculateMemorySize(nv2, ng2); else // QProblemB - size += QProblemB_calculateMemorySize(nvd); + size += QProblemB_calculateMemorySize(nv); make_int_multiple_of(8, &size); @@ -146,11 +172,19 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o void *raw_memory) { dense_qp_qpoases_memory *mem; + dense_qp_dims dims_stacked; - int nvd = dims->nv; - int ned = dims->ne; - int ngd = dims->ng; - int nbd = dims->nb; + int nv = dims->nv; + int ne = dims->ne; + int ng = dims->ng; + int nb = dims->nb; + int nsb = dims->nsb; + int nsg = dims->nsg; + int ns = dims->ns; + + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2 * ns; // char pointer char *c_ptr = (char *) raw_memory; @@ -158,38 +192,64 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o mem = (dense_qp_qpoases_memory *) c_ptr; c_ptr += sizeof(dense_qp_qpoases_memory); - assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); + assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); + + if (ns > 0) + { + dense_qp_stack_slacks_dims(dims, &dims_stacked); + mem->qp_stacked = dense_qp_in_assign(config_, &dims_stacked, c_ptr); + c_ptr += dense_qp_in_calculate_size(config_, &dims_stacked); + } + else + { + mem->qp_stacked = NULL; + } - assign_and_advance_double(nvd * nvd, &mem->H, &c_ptr); - assign_and_advance_double(nvd * nvd, &mem->R, &c_ptr); - assign_and_advance_double(nvd * ned, &mem->A, &c_ptr); - assign_and_advance_double(nvd * ngd, &mem->C, &c_ptr); - assign_and_advance_double(nvd, &mem->g, &c_ptr); - assign_and_advance_double(ned, &mem->b, &c_ptr); - assign_and_advance_double(nbd, &mem->d_lb0, &c_ptr); - assign_and_advance_double(nbd, &mem->d_ub0, &c_ptr); - assign_and_advance_double(nvd, &mem->d_lb, &c_ptr); - assign_and_advance_double(nvd, &mem->d_ub, &c_ptr); - assign_and_advance_double(ngd, &mem->d_lg, &c_ptr); - assign_and_advance_double(ngd, &mem->d_ug, &c_ptr); - assign_and_advance_double(nvd, &mem->prim_sol, &c_ptr); - assign_and_advance_double(nvd + ngd, &mem->dual_sol, &c_ptr); + assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!"); + + assign_and_advance_double(nv * nv, &mem->H, &c_ptr); + assign_and_advance_double(nv2 * nv2, &mem->HH, &c_ptr); + assign_and_advance_double(nv2 * nv2, &mem->R, &c_ptr); + assign_and_advance_double(nv2 * ne, &mem->A, &c_ptr); + assign_and_advance_double(nv * ng, &mem->C, &c_ptr); + assign_and_advance_double(nv2 * ng2, &mem->CC, &c_ptr); + assign_and_advance_double(nv, &mem->g, &c_ptr); + assign_and_advance_double(nv2, &mem->gg, &c_ptr); + assign_and_advance_double(ne, &mem->b, &c_ptr); + assign_and_advance_double(nb2, &mem->d_lb0, &c_ptr); + assign_and_advance_double(nb2, &mem->d_ub0, &c_ptr); + assign_and_advance_double(nv2, &mem->d_lb, &c_ptr); + assign_and_advance_double(nv2, &mem->d_ub, &c_ptr); + assign_and_advance_double(ng, &mem->d_lg0, &c_ptr); + assign_and_advance_double(ng, &mem->d_ug0, &c_ptr); + assign_and_advance_double(ng2, &mem->d_lg, &c_ptr); + assign_and_advance_double(ng2, &mem->d_ug, &c_ptr); + assign_and_advance_double(ns, &mem->d_ls, &c_ptr); + assign_and_advance_double(ns, &mem->d_us, &c_ptr); + assign_and_advance_double(ns, &mem->Zl, &c_ptr); + assign_and_advance_double(ns, &mem->Zu, &c_ptr); + assign_and_advance_double(ns, &mem->zl, &c_ptr); + assign_and_advance_double(ns, &mem->zu, &c_ptr); + assign_and_advance_double(nv2, &mem->prim_sol, &c_ptr); + assign_and_advance_double(nv2 + ng2, &mem->dual_sol, &c_ptr); // TODO(dimitris): update assign syntax in qpOASES assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!"); - if (ngd > 0) + if (ng > 0 || ns > 0) { // QProblem - QProblem_assignMemory(nvd, ngd, (QProblem **) &(mem->QP), c_ptr); - c_ptr += QProblem_calculateMemorySize(nvd, ngd); + QProblem_assignMemory(nv2, ng2, (QProblem **) &(mem->QP), c_ptr); + c_ptr += QProblem_calculateMemorySize(nv2, ng2); } else { // QProblemB - QProblemB_assignMemory(nvd, (QProblemB **) &(mem->QPB), c_ptr); - c_ptr += QProblemB_calculateMemorySize(nvd); + QProblemB_assignMemory(nv, (QProblemB **) &(mem->QPB), c_ptr); + c_ptr += QProblemB_calculateMemorySize(nv); } - assign_and_advance_int(nbd, &mem->idxb, &c_ptr); + assign_and_advance_int(nb, &mem->idxb, &c_ptr); + assign_and_advance_int(nb2, &mem->idxb_stacked, &c_ptr); + assign_and_advance_int(ns, &mem->idxs, &c_ptr); assert((char *) raw_memory + dense_qp_qpoases_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -221,6 +281,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo acados_tic(&tot_timer); acados_tic(&interface_timer); + info->t_computed = 0; // cast structures dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *) opts_; @@ -228,51 +289,82 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo // extract qpoases data double *H = memory->H; + double *HH = memory->HH; double *A = memory->A; double *C = memory->C; + double *CC = memory->CC; double *g = memory->g; + double *gg = memory->gg; double *b = memory->b; double *d_lb0 = memory->d_lb0; double *d_ub0 = memory->d_ub0; double *d_lb = memory->d_lb; double *d_ub = memory->d_ub; + double *d_lg0 = memory->d_lg0; + double *d_ug0 = memory->d_ug0; double *d_lg = memory->d_lg; double *d_ug = memory->d_ug; + double *Zl = memory->Zl; + double *Zu = memory->Zu; + double *zl = memory->zl; + double *zu = memory->zu; + double *d_ls = memory->d_ls; + double *d_us = memory->d_us; int *idxb = memory->idxb; + int *idxb_stacked = memory->idxb_stacked; + int *idxs = memory->idxs; double *prim_sol = memory->prim_sol; double *dual_sol = memory->dual_sol; QProblemB *QPB = memory->QPB; QProblem *QP = memory->QP; + dense_qp_in *qp_stacked = memory->qp_stacked; // extract dense qp size - int nvd = qp_in->dim->nv; - int ngd = qp_in->dim->ng; - int nbd = qp_in->dim->nb; - int nsd = qp_in->dim->ns; - - if (nsd > 0) - { - printf("\nqpOASES interface can not handle ns>0 yet: what about implementing it? :)\n"); - return ACADOS_FAILURE; - } + int nv = qp_in->dim->nv; + int ne = qp_in->dim->ne; + int ng = qp_in->dim->ng; + int nb = qp_in->dim->nb; + int nsb = qp_in->dim->nsb; + int nsg = qp_in->dim->nsg; + int ns = qp_in->dim->ns; + + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? ng + nsb : ng; + int nb2 = nb - nsb + 2 * ns; // fill in the upper triangular of H in dense_qp - blasfeo_dtrtr_l(nvd, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); + blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); - // dense qp row-major - d_cvt_dense_qp_to_rowmaj(qp_in, H, g, A, b, idxb, d_lb0, d_ub0, C, d_lg, d_ug, NULL, NULL, NULL, - NULL, NULL, NULL, NULL); + // extract data from qp_in in row-major + d_cvt_dense_qp_to_rowmaj(qp_in, H, g, A, b, idxb, d_lb0, d_ub0, C, d_lg0, d_ug0, + Zl, Zu, zl, zu, idxs, d_ls, d_us); - // reorder bounds - for (int ii = 0; ii < nvd; ii++) + // reorder box constraints bounds + for (int ii = 0; ii < nv2; ii++) { d_lb[ii] = -QPOASES_INFTY; d_ub[ii] = +QPOASES_INFTY; } - for (int ii = 0; ii < nbd; ii++) + + if (ns > 0) + { + dense_qp_stack_slacks(qp_in, qp_stacked); + d_cvt_dense_qp_to_rowmaj(qp_stacked, HH, gg, A, b, idxb_stacked, d_lb0, d_ub0, CC, d_lg, + d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL); + + for (int ii = 0; ii < nb2; ii++) + { + d_lb[idxb_stacked[ii]] = d_lb0[ii]; + d_ub[idxb_stacked[ii]] = d_ub0[ii]; + } + } + else { - d_lb[idxb[ii]] = d_lb0[ii]; - d_ub[idxb[ii]] = d_ub0[ii]; + for (int ii = 0; ii < nb; ii++) + { + d_lb[idxb[ii]] = d_lb0[ii]; + d_ub[idxb[ii]] = d_ub0[ii]; + } } // cholesky factorization of H @@ -294,19 +386,23 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo int qpoases_status = 0; if (opts->hotstart == 1) { // only to be used with fixed data matrices! - if (ngd > 0) + if (ng > 0 || ns > 0) { // QProblem if (memory->first_it == 1) { - QProblemCON(QP, nvd, ngd, HST_POSDEF); + QProblemCON(QP, nv2, ng2, HST_POSDEF); + QProblem_setPrintLevel(QP, PL_MEDIUM); + // QProblem_setPrintLevel(QP, PL_DEBUG_ITER); if (opts->set_acado_opts) { static Options options; Options_setToMPC(&options); QProblem_setOptions(QP, options); } - qpoases_status = - QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime); + + qpoases_status = (ns > 0) ? + QProblem_init(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) : + QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime); memory->first_it = 0; QProblem_getPrimalSolution(QP, prim_sol); @@ -314,7 +410,9 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } else { - qpoases_status = QProblem_hotstart(QP, g, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime); + qpoases_status = (ns > 0) ? + QProblem_hotstart(QP, gg, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) : + QProblem_hotstart(QP, g, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime); QProblem_getPrimalSolution(QP, prim_sol); QProblem_getDualSolution(QP, dual_sol); @@ -324,7 +422,9 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo { if (memory->first_it == 1) { - QProblemBCON(QPB, nvd, HST_POSDEF); + QProblemBCON(QPB, nv, HST_POSDEF); + QProblemB_setPrintLevel(QPB, PL_MEDIUM); + // QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER); if (opts->set_acado_opts) { static Options options; @@ -348,9 +448,12 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } else { // hotstart = 0 - if (ngd > 0) + if (ng > 0 || ns > 0) { - QProblemCON(QP, nvd, ngd, HST_POSDEF); + QProblemCON(QP, nv2, ng2, HST_POSDEF); + // QProblem_setPrintLevel(QP, PL_HIGH); + QProblem_setPrintLevel(QP, PL_DEBUG_ITER); + QProblem_printProperties(QP); if (opts->use_precomputed_cholesky == 1) { // static Options options; @@ -358,8 +461,12 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo // options.initialStatusBounds = ST_INACTIVE; // QProblem_setOptions( QP, options ); - qpoases_status = - QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime, + qpoases_status = (ns > 0) ? + QProblem_initW(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime, + /* primal_sol */ NULL, /* dual sol */ NULL, + /* guessed bounds */ NULL, /* guessed constraints */ NULL, + /* R */ memory->R) : + QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime, /* primal_sol */ NULL, /* dual sol */ NULL, /* guessed bounds */ NULL, /* guessed constraints */ NULL, /* R */ memory->R); // NOTE(dimitris): can pass either NULL or 0 @@ -374,13 +481,17 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } if (opts->warm_start) { - qpoases_status = QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, - &cputime, NULL, dual_sol, NULL, NULL, NULL); + qpoases_status = (ns > 0) ? + QProblem_initW(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, + &cputime, NULL, dual_sol, NULL, NULL, NULL) : + QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, + &cputime, NULL, dual_sol, NULL, NULL, NULL); } else { - qpoases_status = - QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime); + qpoases_status = (ns > 0) ? + QProblem_init(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) : + QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime); } } QProblem_getPrimalSolution(QP, prim_sol); @@ -388,7 +499,10 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } else { // QProblemB - QProblemBCON(QPB, nvd, HST_POSDEF); + QProblemBCON(QPB, nv, HST_POSDEF); + // QProblemB_setPrintLevel(QPB, PL_MEDIUM); + QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER); + QProblemB_printProperties(QPB); if (opts->use_precomputed_cholesky == 1) { // static Options options; @@ -431,23 +545,63 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo info->solve_QP_time = acados_toc(&qp_timer); acados_tic(&interface_timer); - // copy prim_sol and dual_sol to qpd_sol - blasfeo_pack_dvec(nvd, prim_sol, qp_out->v, 0); - for (int ii = 0; ii < 2 * nbd + 2 * ngd; ii++) qp_out->lam->pa[ii] = 0.0; - for (int ii = 0; ii < nbd; ii++) + // copy prim_sol and dual_sol to qp_out + blasfeo_pack_dvec(nv2, prim_sol, qp_out->v, 0); + for (int ii = 0; ii < 2 * nb + 2 * ng + 2 * ns; ii++) qp_out->lam->pa[ii] = 0.0; + for (int ii = 0; ii < nb; ii++) { if (dual_sol[idxb[ii]] >= 0.0) qp_out->lam->pa[ii] = dual_sol[idxb[ii]]; else - qp_out->lam->pa[nbd + ngd + ii] = -dual_sol[idxb[ii]]; + qp_out->lam->pa[nb + ng + ii] = -dual_sol[idxb[ii]]; + } + + for (int ii = 0; ii < ng; ii++) + { + if (dual_sol[nv2 + ii] >= 0.0) + qp_out->lam->pa[nb + ii] = dual_sol[nv2 + ii]; + else + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; } - for (int ii = 0; ii < ngd; ii++) + + int k = 0; + for (int ii = 0; ii < ns; ii++) { - if (dual_sol[nvd + ii] >= 0.0) - qp_out->lam->pa[nbd + ii] = dual_sol[nvd + ii]; + int js = idxs[ii]; + + double offset_l = 0.0; + double offset_u = 0.0; + + if (js < nb) + { + if (dual_sol[nv2 + ng + k] <= 0.0) // softened upper box constraints + { + qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + ng + k]; + offset_u = -dual_sol[nv2 + ng + k]; + } + else // softened lower box constraints + { + qp_out->lam->pa[js] = dual_sol[nv2 + ng + k]; + offset_l = dual_sol[nv2 + ng + k]; + } + + k++; + } else - qp_out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; + { + offset_l = qp_out->lam->pa[nb+js-nb]; + offset_u = qp_out->lam->pa[2*nb+ng+js-nb]; + } + + // dual variables for sl >= d_ls + if (dual_sol[nv + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ii] = dual_sol[nv + ii] - offset_u; + + // dual variables for su >= d_us + if (dual_sol[nv + ns + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } + info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = nwsr; @@ -455,11 +609,8 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo // compute slacks if (opts->compute_t) { - blasfeo_dvecex_sp(nbd, 1.0, qp_in->idxb, qp_out->v, 0, qp_out->t, nbd + ngd); - blasfeo_dgemv_t(nvd, ngd, 1.0, qp_in->Ct, 0, 0, qp_out->v, 0, 0.0, qp_out->t, 2 * nbd + ngd, - qp_out->t, 2 * nbd + ngd); - blasfeo_dveccpsc(nbd + ngd, -1.0, qp_out->t, nbd + ngd, qp_out->t, 0); - blasfeo_daxpy(2 * nbd + 2 * ngd, -1.0, qp_in->d, 0, qp_out->t, 0, qp_out->t, 0); + dense_qp_compute_t(qp_in, qp_out); + info->t_computed = 1; } int acados_status = qpoases_status; diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 7c92f85dcb..b9ce99b4ba 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -24,6 +24,9 @@ extern "C" { #endif +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + // acados #include "acados/dense_qp/dense_qp_common.h" #include "acados/utils/types.h" @@ -43,8 +46,14 @@ typedef struct dense_qp_qpoases_opts_ typedef struct dense_qp_qpoases_memory_ { double *H; + double *HH; double *R; double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; double *A; double *b; double *d_lb0; @@ -52,9 +61,16 @@ typedef struct dense_qp_qpoases_memory_ double *d_lb; double *d_ub; double *C; + double *CC; + double *d_lg0; + double *d_ug0; double *d_lg; double *d_ug; + double *d_ls; + double *d_us; int *idxb; + int *idxb_stacked; + int *idxs; double *prim_sol; double *dual_sol; void *QPB; // NOTE(giaf): cast to QProblemB to use @@ -62,6 +78,7 @@ typedef struct dense_qp_qpoases_memory_ double cputime; // cputime of qpoases int nwsr; // performed number of working set recalculations int first_it; // to be used with hotstart + dense_qp_in *qp_stacked; } dense_qp_qpoases_memory; int dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims); diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 341c6da3e6..32de0faa90 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -168,6 +168,9 @@ ocp_qp_in *ocp_qp_in_assign(void *config, ocp_qp_dims *dims, void *raw_memory) dims_copy->ns[ii] = dims->ns[ii]; dims_copy->nbu[ii] = dims->nbu[ii]; dims_copy->nbx[ii] = dims->nbx[ii]; + dims_copy->nsbu[ii] = dims->nsbu[ii]; + dims_copy->nsbx[ii] = dims->nsbx[ii]; + dims_copy->nsg[ii] = dims->nsg[ii]; } qp_in->dim = dims_copy; @@ -215,6 +218,9 @@ ocp_qp_out *ocp_qp_out_assign(void *config, ocp_qp_dims *dims, void *raw_memory) dims_copy->nb[ii] = dims->nb[ii]; dims_copy->ng[ii] = dims->ng[ii]; dims_copy->ns[ii] = dims->ns[ii]; + dims_copy->nsbx[ii] = dims->nsbx[ii]; + dims_copy->nsbu[ii] = dims->nsbu[ii]; + dims_copy->nsg[ii] = dims->nsg[ii]; dims_copy->nbu[ii] = dims->nbu[ii]; dims_copy->nbx[ii] = dims->nbx[ii]; } @@ -279,44 +285,52 @@ ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws) { - // loop index - int ii; - - // - int N = qp_in->dim->N; - int *nx = qp_in->dim->nx; - int *nu = qp_in->dim->nu; - int *nb = qp_in->dim->nb; - int *ng = qp_in->dim->ng; - - struct blasfeo_dmat *DCt = qp_in->DCt; - struct blasfeo_dvec *d = qp_in->d; - int **idxb = qp_in->idxb; - - struct blasfeo_dvec *ux = qp_out->ux; - struct blasfeo_dvec *t = qp_out->t; + ocp_qp_info *info = (ocp_qp_info *) qp_out->misc; - struct blasfeo_dvec *tmp_nbgM = res_ws->tmp_nbgM; - - int nx_i, nu_i, nb_i, ng_i; - - for (ii = 0; ii <= N; ii++) + if (info->t_computed == 0) { - nx_i = nx[ii]; - nu_i = nu[ii]; - nb_i = nb[ii]; - ng_i = 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); - - // compute slacks for bounds - blasfeo_dvecex_sp(nb_i, 1.0, idxb[ii], ux + ii, 0, tmp_nbgM + 0, 0); - blasfeo_daxpby(nb_i, 1.0, tmp_nbgM + 0, 0, -1.0, d + ii, 0, t + ii, 0); - blasfeo_daxpby(nb_i, -1.0, tmp_nbgM + 0, 0, -1.0, d + ii, nb_i + ng_i, t + ii, nb_i + ng_i); + // loop index + int ii; + + // + int N = qp_in->dim->N; + int *nx = qp_in->dim->nx; + int *nu = qp_in->dim->nu; + int *nb = qp_in->dim->nb; + int *ng = qp_in->dim->ng; + + struct blasfeo_dmat *DCt = qp_in->DCt; + struct blasfeo_dvec *d = qp_in->d; + int **idxb = qp_in->idxb; + + struct blasfeo_dvec *ux = qp_out->ux; + struct blasfeo_dvec *t = qp_out->t; + + struct blasfeo_dvec *tmp_nbgM = res_ws->tmp_nbgM; + + int nx_i, nu_i, nb_i, ng_i; + + 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 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); + + // compute slacks for bounds + blasfeo_dvecex_sp(nb_i, 1.0, idxb[ii], ux + ii, 0, tmp_nbgM + 0, 0); + blasfeo_daxpby(nb_i, 1.0, tmp_nbgM + 0, 0, -1.0, d + ii, 0, t + ii, 0); + blasfeo_daxpby(nb_i, -1.0, tmp_nbgM + 0, 0, -1.0, d + ii, nb_i + ng_i, t + ii, + nb_i + ng_i); + } + + info->t_computed = 1; } d_compute_res_ocp_qp(qp_in, qp_out, qp_res, res_ws); @@ -398,3 +412,190 @@ void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]) return; } + +void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out) +{ + int N = in->N; + int *nx = in->nx; + int *nu = in->nu; + int *nb = in->nb; + int *nbx = in->nbx; + int *nbu = in->nbu; + int *ng = in->ng; + int *ns = in->ns; + int *nsbx = in->nsbx; + int *nsbu = in->nsbu; + int *nsg = in->nsg; + + out->N = N; + + for (int ii = 0; ii <= N; ii++) + { + out->nx[ii] = nx[ii]; + out->nu[ii] = nu[ii] + 2*ns[ii]; + out->nb[ii] = nb[ii] - nsbx[ii] - nsbu[ii] + 2*ns[ii]; + out->nbx[ii] = nbx[ii] - nsbx[ii]; + out->nbu[ii] = nbu[ii] - nsbu[ii] + 2*ns[ii]; + out->ng[ii] = ns[ii] > 0 ? ng[ii] + nsbx[ii] + nsbu[ii] : ng[ii]; + out->ns[ii] = 0; + out->nsbx[ii] = 0; + out->nsbu[ii] = 0; + out->nsg[ii] = 0; + } +} + +void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) +{ + int N = in->dim->N; + int *nx = in->dim->nx; + int *nu = in->dim->nu; + int *nb = in->dim->nb; + int *nbx = in->dim->nbx; + int *nbu = in->dim->nbu; + int *ng = in->dim->ng; + int *ns = in->dim->ns; + int *nsbx = in->dim->nsbx; + int *nsbu = in->dim->nsbu; + int *nsg = in->dim->nsg; + int **idxb = in->idxb; + int **idxs = in->idxs; + + int *nx2 = out->dim->nx; + int *nu2 = out->dim->nu; + int *nb2 = out->dim->nb; + int *nbx2 = out->dim->nbx; + int *nbu2 = out->dim->nbu; + int *ng2 = out->dim->ng; + + for (int ii = 0; ii <= N; ii++) + { + if (ii < N) + { + // set matrices to 0.0 + blasfeo_dgese(nu2[ii]+nx2[ii]+1, nx2[ii+1], 0.0, out->BAbt+ii, 0, 0); + + // copy in->BAbt to out->BAbt, out->BAbt = [0; B'; A'; b'] + blasfeo_dgecp(nu[ii]+nx[ii]+1, nx[ii+1], in->BAbt+ii, 0, 0, out->BAbt+ii, 2*ns[ii], 0); + + // copy in->b to out->b + blasfeo_dveccp(nx2[ii+1], in->b+ii, 0, out->b+ii, 0); + } + + // set matrices to 0.0 + blasfeo_dgese(nu2[ii]+nx2[ii]+1, nu2[ii]+nx2[ii], 0.0, out->RSQrq+ii, 0, 0); + blasfeo_dgese(nu2[ii]+nx2[ii], ng2[ii], 0.0, out->DCt+ii, 0, 0); + + // copy in->Z to the main diagonal of out->RSQrq, out->RSQrq = [Z 0 0; 0 0 0; 0 0 0; 0 0 0] + blasfeo_ddiain(2*ns[ii], 1.0, in->Z+ii, 0, out->RSQrq+ii, 0, 0); + + // copy in->RSQrq to out->RSQrq, out->RSQrq = [Z 0 0; 0 R S; 0 S' Q; 0 r' q'] + blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], in->RSQrq+ii, 0, 0, out->RSQrq+ii, 2*ns[ii], + 2*ns[ii]); + + // copy in->rqz to out->rqz, out->rqz = [z r q] + blasfeo_dveccp(2*ns[ii], in->rqz+ii, nu[ii]+nx[ii], out->rqz+ii, 0); + blasfeo_dveccp(nu[ii]+nx[ii], in->rqz+ii, 0, out->rqz+ii, 2*ns[ii]); + + // copy in->DCt to out->DCt, out->DCt = [0 0; DCt 0] + blasfeo_dgecp(nu[ii]+nx[ii], ng[ii], in->DCt+ii, 0, 0, out->DCt+ii, 2*ns[ii], 0); + + if (ns[ii] > 0) + { + // set flags for non-softened box constraints + // use out->m temporarily for this + for (int jj = 0; jj < nb[ii]; jj++) + { + // TODO(bnovoselnik): pick up some workspace for this + BLASFEO_DVECEL(out->m+ii, jj) = 1.0; + } + + int col_b = ng[ii]; + for (int jj = 0; jj < ns[ii]; jj++) + { + int js = idxs[ii][jj]; + + int idx_v_ls1 = jj; + int idx_v_us1 = ns[ii]+jj; + + int idx_d_ls0 = js; + int idx_d_us0 = nb[ii]+ng[ii]+js; + int idx_d_ls1; + int idx_d_us1; + + if (js < nb[ii]) // soft box constraint + { + // index of a soft box constraint + int jv = idxb[ii][js]+2*ns[ii]; + + idx_d_ls1 = nb2[ii]+col_b; + idx_d_us1 = 2*nb2[ii]+ng2[ii]+col_b; + + // soft box constraint, set its flag to -1 + BLASFEO_DVECEL(out->m+ii, js) = -1.0; + + // insert soft box constraint into out->DCt, lb <= ux + sl - su <= ub + BLASFEO_DMATEL(out->DCt+ii, jv, col_b) = 1.0; + BLASFEO_DMATEL(out->DCt+ii, idx_v_ls1, col_b) = +1.0; + BLASFEO_DMATEL(out->DCt+ii, idx_v_us1, col_b) = -1.0; + BLASFEO_DVECEL(out->d+ii, idx_d_ls1) = BLASFEO_DVECEL(in->d+ii, idx_d_ls0); + BLASFEO_DVECEL(out->d+ii, idx_d_us1) = -BLASFEO_DVECEL(in->d+ii, idx_d_us0); + + col_b++; + } + else // soft general constraint + { + // index of a soft general constraint + int col_g = js - nb[ii]; + + // soft general constraint, lg <= D u + C x + sl - su <= ug + BLASFEO_DMATEL(out->DCt+ii, idx_v_ls1, col_g) = +1.0; + BLASFEO_DMATEL(out->DCt+ii, idx_v_us1, col_g) = -1.0; + } + + // slacks have box constraints + out->idxb[ii][idx_v_ls1] = idx_v_ls1; + out->idxb[ii][idx_v_us1] = idx_v_us1; + } + + int k_nsb = 0; + for (int jj = 0; jj < nb[ii]; jj++) + { + if (BLASFEO_DVECEL(out->m+ii, jj) > 0) + { + // copy nonsoftened box constraint bounds to out->d + BLASFEO_DVECEL(out->d+ii, 2*ns[ii]+k_nsb) = BLASFEO_DVECEL(in->d+ii, jj); + BLASFEO_DVECEL(out->d+ii, nb2[ii]+ng2[ii]+2*ns[ii]+k_nsb) = + -BLASFEO_DVECEL(in->d+ii, nb[ii]+ng[ii]+jj); + out->idxb[ii][2*ns[ii]+k_nsb] = 2*ns[ii] + jj; + k_nsb++; + } + } + + assert(k_nsb == nb[ii] - nsbu[ii] - nsbx[ii] && "Dimensions are wrong!"); + + // copy ls and us to out->lb + blasfeo_dveccp(2*ns[ii], in->d+ii, 2*nb[ii]+2*ng[ii], out->d+ii, 0); + + // for slacks out->ub is +INFTY + blasfeo_dvecse(2*ns[ii], 1.0e6, out->d+ii, nb2[ii]+ng2[ii]); + + // copy in->lg to out->lg + blasfeo_dveccp(ng[ii], in->d+ii, nb[ii], out->d+ii, nb2[ii]); + + // copy in->ug to out->ug + blasfeo_dveccpsc(ng[ii], -1.0, in->d+ii, 2*nb[ii]+ng[ii], out->d+ii, 2*nb2[ii]+ng2[ii]); + + // flip signs for ub and ug + blasfeo_dvecsc(nb2[ii]+ng2[ii], -1.0, out->d+ii, nb2[ii]+ng2[ii]); + + // set out->m to 0.0 + blasfeo_dvecse(2*nb2[ii]+2*ng2[ii], 0.0, out->m+ii, 0); + } + else + { + blasfeo_dveccp(2*nb[ii]+2*ng[ii], in->d+ii, 0, out->d+ii, 0); + blasfeo_dveccp(2*nb[ii]+2*ng[ii], in->m+ii, 0, out->m+ii, 0); + for (int jj = 0; jj < nb[ii]; jj++) out->idxb[ii][jj] = in->idxb[ii][jj]; + } + } +} diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index e097f6269a..faef2f77aa 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -90,6 +90,7 @@ typedef struct double interface_time; double total_time; int num_iter; + int t_computed; } ocp_qp_info; // @@ -129,6 +130,11 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res ocp_qp_res_ws *res_ws); // void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]); +// +void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out); +// +void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out); +// #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/ocp_qp/ocp_qp_full_condensing_solver.c b/acados/ocp_qp/ocp_qp_full_condensing_solver.c index fb399527c3..3d1ae3f24a 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_full_condensing_solver.c @@ -271,6 +271,7 @@ int ocp_qp_full_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out *q info->solve_QP_time = ((dense_qp_info *) (memory->qpd_out->misc))->solve_QP_time; info->interface_time = ((dense_qp_info *) (memory->qpd_out->misc))->interface_time; info->num_iter = ((dense_qp_info *) (memory->qpd_out->misc))->num_iter; + info->t_computed = ((dense_qp_info *) (memory->qpd_out->misc))->t_computed; return solver_status; } diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index 0760b73c17..2efbb5a3cd 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -76,7 +76,7 @@ void ocp_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts // ocp_qp_dims *dims = dims_; ocp_qp_hpipm_opts *opts = opts_; - d_set_default_ocp_qp_ipm_arg(opts->hpipm_opts); + d_set_default_ocp_qp_ipm_arg(BALANCE, opts->hpipm_opts); // overwrite some default options opts->hpipm_opts->res_g_max = 1e-6; opts->hpipm_opts->res_b_max = 1e-8; @@ -176,6 +176,7 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void * info->interface_time = 0; // there are no conversions for hpipm info->total_time = acados_toc(&tot_timer); info->num_iter = memory->hpipm_workspace->iter; + info->t_computed = 1; // check exit conditions int acados_status = hpipm_status; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 6219cb9dc6..3282d79456 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -346,6 +346,7 @@ int ocp_qp_partial_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out info->solve_QP_time = ((ocp_qp_info *) (memory->pcond_qp_out->misc))->solve_QP_time; info->interface_time = ((ocp_qp_info *) (memory->pcond_qp_out->misc))->interface_time; info->num_iter = ((ocp_qp_info *) (memory->pcond_qp_out->misc))->num_iter; + info->t_computed = ((ocp_qp_info *) (memory->pcond_qp_out->misc))->t_computed; return solver_status; } diff --git a/acados/utils/print.c b/acados/utils/print.c index 4d36025f48..7e71280078 100644 --- a/acados/utils/print.c +++ b/acados/utils/print.c @@ -461,6 +461,10 @@ void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res) blasfeo_print_exp_tran_dvec(2 * ni[ii], &nlp_res->res_m[ii], 0); } + printf("inf norm res=\t%e\t%e\t%e\t%e\n", nlp_res->inf_norm_res_g, nlp_res->inf_norm_res_b, + nlp_res->inf_norm_res_d, nlp_res->inf_norm_res_m); + + return; } @@ -665,6 +669,9 @@ void print_dense_qp_in(dense_qp_in *qp_in) { int nv = qp_in->dim->nv; int ne = qp_in->dim->ne; + int nb = qp_in->dim->nb; + int ng = qp_in->dim->ng; + int ns = qp_in->dim->ns; printf("H =\n"); blasfeo_print_dmat(nv, nv, qp_in->Hv, 0, 0); @@ -674,6 +681,10 @@ void print_dense_qp_in(dense_qp_in *qp_in) blasfeo_print_dmat(ne, nv, qp_in->A, 0, 0); printf("b =\n"); blasfeo_print_dvec(ne, qp_in->b, 0); + printf("Ct =\n"); + blasfeo_print_dmat(nv, ng, qp_in->Ct, 0, 0); + printf("d =\n"); + blasfeo_print_dvec(2*nb+2*ng+2*ns, qp_in->d, 0); } void print_ocp_qp_info(ocp_qp_info *info) diff --git a/examples/c/chain_model/chain_model.h b/examples/c/chain_model/chain_model.h index 347d622e4b..6696b09bcc 100755 --- a/examples/c/chain_model/chain_model.h +++ b/examples/c/chain_model/chain_model.h @@ -7,6 +7,24 @@ extern "C" { #endif +#define X0_NM2_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm2.txt" +#define X0_NM3_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm3.txt" +#define X0_NM4_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm4.txt" +#define X0_NM5_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm5.txt" +#define X0_NM6_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm6.txt" +#define X0_NM7_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm7.txt" +#define X0_NM8_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm8.txt" +#define X0_NM9_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/x0_nm9.txt" + +#define XN_NM2_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm2.txt" +#define XN_NM3_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm3.txt" +#define XN_NM4_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm4.txt" +#define XN_NM5_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm5.txt" +#define XN_NM6_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm6.txt" +#define XN_NM7_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm7.txt" +#define XN_NM8_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm8.txt" +#define XN_NM9_FILE "/home/bnovoselnik/Software/acados/examples/c/chain_model/xN_nm9.txt" + /* forward vde */ int vde_chain_nm2(const real_t **arg, real_t **res, int *iw, real_t *w, void *mem); int vde_chain_nm3(const real_t **arg, real_t **res, int *iw, real_t *w, void *mem); diff --git a/examples/c/dense_qp.c b/examples/c/dense_qp.c index 262e9c62d8..1a3ec3c332 100644 --- a/examples/c/dense_qp.c +++ b/examples/c/dense_qp.c @@ -13,6 +13,8 @@ // hpipm #include "hpipm/include/hpipm_d_dense_qp_kkt.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" + static dense_qp_res *dense_qp_res_create(dense_qp_dims *dims) { int size = dense_qp_res_calculate_size(dims); @@ -34,36 +36,36 @@ int main() { double H[] = {3., 1., 1., 4.}; double g[] = {2., 1.}; - int idxb[] = {0}; - double d_lb[] = {-1.}; - double d_ub[] = {1.}; + int idxb[] = {0, 1}; + double d_lb[] = {-1., -1.}; + double d_ub[] = {1., 1.}; - double C[] = {1.2, 1.3}; - double d_lg[] = {-2.}; - double d_ug[] = {2.}; + double C[] = {1.2, 1.4, 1.3, 1.5}; + double d_lg[] = {-2., -2.}; + double d_ug[] = {2., 2.}; - int idxs[] = {0, 1}; - double d_ls[] = {0.0, 0.0}; - double d_us[] = {0.0, 0.0}; + int idxs[] = {1, 2, 3}; + double d_ls[] = {0.0, 0.0, 0.0}; + double d_us[] = {0.0, 0.0, 0.0}; - double Zl[] = {1.0, 1.0}; - double zl[] = {1.0, 1.0}; - double Zu[] = {1.0, 1.0}; - double zu[] = {1.0, 1.0}; + double Zl[] = {1.0, 1.0, 1.0}; + double zl[] = {1.0, 1.0, 1.0}; + double Zu[] = {1.0, 1.0, 1.0}; + double zu[] = {1.0, 1.0, 1.0}; dense_qp_solver_plan plan; - plan.qp_solver = DENSE_QP_HPIPM; + plan.qp_solver = DENSE_QP_QPOASES; qp_solver_config *config = dense_qp_config_create(&plan); dense_qp_dims dims; dims.nv = 2; dims.ne = 0; // TODO(dimitris): is this even supported in acados? - dims.nb = 1; - dims.ng = 1; - dims.ns = 2; -// dims.nsb = 1; -// dims.nsg = 1; + dims.nb = 2; + dims.ng = 2; + dims.ns = 3; + dims.nsb = 1; + dims.nsg = 2; dense_qp_in *qp_in = dense_qp_in_create(config, &dims); @@ -73,31 +75,26 @@ int main() { void *opts = dense_qp_opts_create(config, &dims); + // overwrite default opts + // dense_qp_hpipm_opts *hpipm_opts = opts; + // hpipm_opts->hpipm_opts->mu0 = 1e1; + dense_qp_out *qp_out = dense_qp_out_create(config, &dims); dense_qp_solver *qp_solver = dense_qp_create(config, &dims, opts); - // overwrite default opts - dense_qp_hpipm_opts *hpipm_opts = opts; - hpipm_opts->hpipm_opts->mu0 = 1e1; - int acados_return = dense_qp_solve(qp_solver, qp_in, qp_out); - // if (acados_return != ACADOS_SUCCESS) - // return -1; - printf("STATUS: %d\n", acados_return); -// print_dense_qp_out(qp_out); + // if (acados_return != ACADOS_SUCCESS) + // return -1; /************************************************ * compute inf norm of residuals ************************************************/ double res[4]; - dense_qp_res *qp_res = dense_qp_res_create(&dims); - dense_qp_res_ws *res_ws = dense_qp_res_workspace_create(&dims); - d_compute_res_dense_qp(qp_in, qp_out, qp_res, res_ws); #if 0 blasfeo_print_exp_tran_dvec(dims.nv+2*dims.ns, qp_res->res_g, 0); @@ -106,6 +103,9 @@ int main() { blasfeo_print_exp_tran_dvec(2*dims.nb+2*dims.ng+2*dims.ns, qp_res->res_m, 0); #endif + printf("v=\n"); + blasfeo_print_exp_tran_dvec(dims.nv+2*dims.ns, qp_out->v, 0); + dense_qp_inf_norm_residuals(&dims, qp_in, qp_out, res); printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index 602939b3e7..e3add3f7ef 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -34,10 +34,10 @@ // mass spring helper functions // hard constraints ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, int ng_, int ngN); -ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); +ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, ocp_qp_dims *dims); // soft constraints 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_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); +ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, ocp_qp_dims *dims); #ifndef ACADOS_WITH_QPDUNES #define ELIMINATE_X0 @@ -45,9 +45,9 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, int N, int nx_ #define GENERAL_CONSTRAINT_AT_TERMINAL_STAGE -//#define SOFT_CONSTRAINTS +#define SOFT_CONSTRAINTS -#define NREP 100 +#define NREP 1 int main() { printf("\n"); @@ -92,7 +92,7 @@ int main() { int num_N2_values = 3; int N2_values[3] = {15, 5, 3}; - int ii_max = 6; + int ii_max = 4; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -113,10 +113,10 @@ int main() { { PARTIAL_CONDENSING_HPIPM, #ifdef ACADOS_WITH_HPMPC - PARTIAL_CONDENSING_HPMPC, + // PARTIAL_CONDENSING_HPMPC, #endif #ifdef ACADOS_WITH_QPDUNES - PARTIAL_CONDENSING_QPDUNES, + // PARTIAL_CONDENSING_QPDUNES, #endif FULL_CONDENSING_HPIPM, #ifdef ACADOS_WITH_QORE @@ -133,9 +133,9 @@ int main() { ************************************************/ #ifdef SOFT_CONSTRAINTS - ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring_soft_constr(NULL, N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring_soft_constr(NULL, qp_dims); #else - ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, qp_dims); #endif ocp_qp_out *qp_out = ocp_qp_out_create(NULL, qp_dims); @@ -284,7 +284,7 @@ int main() { * print solution ************************************************/ -// print_ocp_qp_out(qp_out); + print_ocp_qp_out(qp_out); /************************************************ * compute infinity norm of residuals diff --git a/examples/c/mass_spring_example_no_interface.c b/examples/c/mass_spring_example_no_interface.c index ec3332965f..f038285a87 100644 --- a/examples/c/mass_spring_example_no_interface.c +++ b/examples/c/mass_spring_example_no_interface.c @@ -42,7 +42,7 @@ // mass spring ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, int ng_, int ngN); -ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); +ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, ocp_qp_dims *dims); @@ -295,7 +295,7 @@ int main() { * ocp qp in ************************************************/ - ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(solver_config, N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(solver_config, qp_dims); /************************************************ * ocp qp out diff --git a/examples/c/mass_spring_fcond_split.c b/examples/c/mass_spring_fcond_split.c index dbaffe1fed..77cbc404ba 100644 --- a/examples/c/mass_spring_fcond_split.c +++ b/examples/c/mass_spring_fcond_split.c @@ -39,7 +39,7 @@ // mass spring ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, int ng_, int ngN); -ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN); +ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, ocp_qp_dims *dims); int main() { @@ -77,7 +77,7 @@ int main() * ocp qp in/out ************************************************/ - ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, qp_dims); ocp_qp_out *qp_out = ocp_qp_out_create(NULL, qp_dims); /************************************************ diff --git a/examples/c/mass_spring_model/mass_spring_qp.c b/examples/c/mass_spring_model/mass_spring_qp.c index 76a520c9dc..ef1bc90cea 100644 --- a/examples/c/mass_spring_model/mass_spring_qp.c +++ b/examples/c/mass_spring_model/mass_spring_qp.c @@ -120,7 +120,7 @@ static void mass_spring_system(double Ts, int nx, int nu, double *A, double *B, ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, int ng_, int ngN) { - int nbu_ = nu_ 0 ? nb_ - nu_ : 0; int nx[N+1]; @@ -143,14 +143,14 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, in for (int ii = 0; ii < N; ii++) { nbu[ii] = nbu_; - } - nbu[N] = 0; + } + nbu[N] = 0; int nbx[N+1]; #if defined(ELIMINATE_X0) - nbx[0] = 0; + nbx[0] = 0; #else - nbx[0] = nx_; + nbx[0] = nx_; #endif for (int ii = 1; ii <= N; ii++) { @@ -168,94 +168,56 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, in } ng[N] = ngN; - int ns[N+1]; - for (int ii = 0; ii <= N; ii++) { + int ns[N+1]; + for (int ii = 0; ii <= N; ii++) { ns[ii] = 0; } - // dims - int dims_size = ocp_qp_dims_calculate_size(N); - void *dims_mem = malloc(dims_size); - ocp_qp_dims *dims = ocp_qp_dims_assign(N, dims_mem); + // dims + int dims_size = ocp_qp_dims_calculate_size(N); + void *dims_mem = malloc(dims_size); + ocp_qp_dims *dims = ocp_qp_dims_assign(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->nbu[ii] = nbu[ii]; - dims->nbx[ii] = nbx[ii]; - } + 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] = ns[ii]; + dims->nsbu[ii] = ns[ii]; + dims->nsg[ii] = ns[ii]; + dims->nbu[ii] = nbu[ii]; + dims->nbx[ii] = nbx[ii]; + } - return dims; + return dims; } -ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN) +ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, ocp_qp_dims *dims) { - int nbu_ = 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_; - } - - int nu[N+1]; - for (int ii = 0; ii < N; ii++) { - nu[ii] = nu_; - } - nu[N] = 0; - - int nbu[N+1]; - for (int ii = 0; ii < N; ii++) - { - nbu[ii] = nbu_; - } - 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_; - } - ng[N] = ngN; + /************************************************ + * extract dims + ************************************************/ - int ns[N+1]; - for (int ii = 0; ii <= N; ii++) { - ns[ii] = 0; - } + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *nb = dims->nb; + int *ng = dims->ng; + int *ns = dims->ns; -// printf("Test problem: mass-spring system with %d masses and %d controls.\n\n", nx_ / 2, nu_); -// printf("MPC problem size: %d states, %d inputs, %d horizon length, %d two-sided box " -// "constraints, %d two-sided general constraints.\n\n", nx_, nu_, N, nb_, ng_); + int nx_ = nx[1]; + int nu_ = nu[1]; + int ng_ = ng[1]; + int ngN = ng[N]; /************************************************ * dynamical system @@ -519,20 +481,9 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, i hug[N] = ugN; - ocp_qp_dims dims; + ocp_qp_in *qp_in = ocp_qp_in_create(config, dims); - dims.N = N; - dims.nx = nx; - dims.nu = nu; - dims.nb = nb; - dims.ng = ng; - dims.ns = ns; - dims.nbu = nbu; - dims.nbx = nbx; - - ocp_qp_in *qp_in = ocp_qp_in_create(config, &dims); - - d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hlb, hub, hC, hD, hlg, hug, NULL, NULL, NULL, NULL, NULL, NULL, NULL, qp_in); + d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hlb, hub, hC, hD, hlg, hug, NULL, NULL, NULL, NULL, NULL, NULL, NULL, qp_in); // free objective free(Q); @@ -585,7 +536,7 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, i ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, int nb_, int ng_, int ngN) { - int nbu_ = nu_ 0 ? nb_ - nu_ : 0; int nx[N+1]; @@ -608,14 +559,14 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, for (int ii = 0; ii < N; ii++) { nbu[ii] = nbu_; - } - nbu[N] = 0; + } + nbu[N] = 0; int nbx[N+1]; #if defined(ELIMINATE_X0) - nbx[0] = 0; + nbx[0] = 0; #else - nbx[0] = nx_; + nbx[0] = nx_; #endif for (int ii = 1; ii <= N; ii++) { @@ -633,101 +584,59 @@ 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++) { + int ns[N+1]; + for (int ii = 0; ii <= N; ii++) { ns[ii] = nbx[ii]+ng[ii]; } - // dims - int dims_size = ocp_qp_dims_calculate_size(N); - void *dims_mem = malloc(dims_size); - ocp_qp_dims *dims = ocp_qp_dims_assign(N, dims_mem); + // dims + int dims_size = ocp_qp_dims_calculate_size(N); + void *dims_mem = malloc(dims_size); + ocp_qp_dims *dims = ocp_qp_dims_assign(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->nbu[ii] = nbu[ii]; - dims->nbx[ii] = nbx[ii]; - } + 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]; + } - return dims; + return dims; } -ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, int N, int nx_, int nu_, int nb_, int ng_, int ngN) +ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, ocp_qp_dims *dims) { - int ii; + int ii; /************************************************ - * dynamical system + * extract dims ************************************************/ - int nbu_ = 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_; - } - - int nu[N+1]; - for (int ii = 0; ii < N; ii++) { - nu[ii] = nu_; - } - nu[N] = 0; - - int nbu[N+1]; - for (int ii = 0; ii < N; ii++) - { - nbu[ii] = nbu_; - } - 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_; - } - ng[N] = ngN; + 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 ns[N+1]; - for (int ii = 0; ii <= N; ii++) { - ns[ii] = nbx[ii]+ng[ii]; - } - -// printf("Test problem: mass-spring system with %d masses and %d controls.\n\n", nx_ / 2, nu_); -// printf("MPC problem size: %d states, %d inputs, %d horizon length, %d two-sided box " -// "constraints, %d two-sided general constraints.\n\n", nx_, nu_, N, nb_, ng_); + int nx_ = nx[1]; + int nu_ = nu[1]; + int ng_ = ng[1]; + int ngN = ng[N]; /************************************************ * dynamical system @@ -883,71 +792,71 @@ ocp_qp_in *create_ocp_qp_in_mass_spring_soft_constr(void *config, int N, int nx_ * soft constraints ************************************************/ - double *Zl0; d_zeros(&Zl0, ns[0], 1); - for(ii=0; iinlp_cost[i] = LINEAR_LS; plan->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM; -// plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; -// plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES; + // plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + // plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES; + // plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QORE; for (int i = 0; i < NN; i++) { diff --git a/external/blasfeo b/external/blasfeo index 4815341368..6dd3bb1c7e 160000 --- a/external/blasfeo +++ b/external/blasfeo @@ -1 +1 @@ -Subproject commit 4815341368f2816de3db8b634d9baf2353a2e7b0 +Subproject commit 6dd3bb1c7e889c953df697ec2d937d3d5c305db6 diff --git a/external/hpipm b/external/hpipm index 2eb8b1f284..7dfadfe8df 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 2eb8b1f2846eb2a17558f1747f8b6af2da9e692e +Subproject commit 7dfadfe8df8c7fd7ace7ad2ee43f604aa3da5d93 diff --git a/external/swig b/external/swig index cd3f6c5fe9..260ed47c44 160000 --- a/external/swig +++ b/external/swig @@ -1 +1 @@ -Subproject commit cd3f6c5fe9ed273fcc981dcae3e8b50d51529664 +Subproject commit 260ed47c4414e61c66ae84a639707b1fef916ba8 diff --git a/test/ocp_qp/test_qpsolvers.cpp b/test/ocp_qp/test_qpsolvers.cpp index 40a734c0a5..a92652efb5 100644 --- a/test/ocp_qp/test_qpsolvers.cpp +++ b/test/ocp_qp/test_qpsolvers.cpp @@ -29,8 +29,7 @@ extern "C" { ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, int ng_, int ngN); -ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, int nb_, int ng_, - int ngN); +ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, ocp_qp_dims *dims); } using std::vector; @@ -133,7 +132,7 @@ TEST_CASE("mass spring example", "[QP solvers]") ocp_qp_dims *qp_dims = create_ocp_qp_dims_mass_spring(N, nx_, nu_, nb_, ng_, ngN); - ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, N, nx_, nu_, nb_, ng_, ngN); + ocp_qp_in *qp_in = create_ocp_qp_in_mass_spring(NULL, qp_dims); ocp_qp_out *qp_out = ocp_qp_out_create(NULL, qp_dims);