From 098489b3ccdf07f8eb0595fe96c8a7e2ac491266 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 25 May 2018 15:10:46 +0200 Subject: [PATCH 01/27] added soft constraints to qpoases interface (TODO: fix dual sol) --- acados/dense_qp/dense_qp_qpoases.c | 364 ++++++++++++++++++++++------- acados/dense_qp/dense_qp_qpoases.h | 12 + 2 files changed, 290 insertions(+), 86 deletions(-) diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 17fd804049..ef0b0a9082 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -112,30 +112,43 @@ 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; + 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) ? 2*(ng + nsb) : ng; // 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 * nv * 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 * nb * 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 * 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 (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); @@ -147,10 +160,16 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o { dense_qp_qpoases_memory *mem; - 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) ? 2*(ng + nsb) : ng; // char pointer char *c_ptr = (char *) raw_memory; @@ -160,36 +179,49 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o 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 * 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); + 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(nv * 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(nb, &mem->d_lb0, &c_ptr); + assign_and_advance_double(nb, &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->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, &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(ns, &mem->idxs, &c_ptr); assert((char *) raw_memory + dense_qp_qpoases_memory_calculate_size(config_, dims, opts_) >= c_ptr); @@ -228,53 +260,201 @@ 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 *d_ls = memory->d_ls; + double *d_us = memory->d_us; + double *Zl = memory->Zl; + double *Zu = memory->Zu; + double *zl = memory->zl; + double *zu = memory->zu; int *idxb = memory->idxb; + int *idxs = memory->idxs; double *prim_sol = memory->prim_sol; double *dual_sol = memory->dual_sol; QProblemB *QPB = memory->QPB; QProblem *QP = memory->QP; // 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 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; - if (nsd > 0) - { - printf("\nqpOASES interface can not handle ns>0 yet: what about implementing it? :)\n"); - return ACADOS_FAILURE; - } + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; // 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); + 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++) + // in case ns > 0, slack variables are introduced, i.e. v2 = [v; sl; su] + // 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++) + for (int ii = 0; ii < nb; ii++) { d_lb[idxb[ii]] = d_lb0[ii]; d_ub[idxb[ii]] = d_ub0[ii]; } + // if a box constraint is softened, remove it from box constraints bounds + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + if (js < nb) + { + d_lb[idxb[js]] = -QPOASES_INFTY; + d_ub[idxb[js]] = +QPOASES_INFTY; + } + } + + // copy d_ls and d_us to box constraints bounds + for (int ii = 0; ii < ns; ii++) + { + d_lb[nv+ii] = d_ls[ii]; + d_lb[nv+ns+ii] = d_ls[ii]; + + d_ub[nv+ii] = d_us[ii]; + d_ub[nv+ns+ii] = d_us[ii]; + } + + if (ns > 0) + { + // in case ns > 0, HH = [H 0 0; 0 Zl 0; 0 0 Zu] + + // initialize HH tp 0.0 + for (int ii = 0; ii < nv2 * nv2; ii++) HH[ii] = 0.0; + + // copy H to upper left corner of HH + for (int ii = 0; ii < nv; ii++) + { + for (int jj = 0; jj < nv; jj++) + { + HH[jj + nv2*ii] = H[jj + nv*ii]; + } + } + + // copy Zl and Zu to the main diagonal of HH + for (int ii = 0; ii < ns; ii++) + { + int ll = ii + nv; + int uu = ii + nv + ns; + + HH[ll + nv2*ll] = Zl[ii]; + HH[uu + nv2*uu] = Zu[ii]; + } + + // in case ns > 0, gg = [g; zl; zu] + // copy g to gg + for (int ii = 0; ii < nv; ii++) gg[ii] = g[ii]; + + // copy zl and zu to gg + for (int ii = 0; ii < ns; ii++) + { + gg[ii+nv] = zl[ii]; + gg[ii+nv+ns] = zu[ii]; + } + + for (int ii = 0; ii < ng2; ii++) + { + // initialize d_ug to 0.0 + d_ug[ii] = 0.0; + + // set d_lg[i] to -INFTY + d_lg[ii] = -QPOASES_INFTY; + } + + // copy [d_ug0;-d_lg0] to d_ug + for (int ii = 0; ii < ng; ii++) + { + d_ug[ii] = d_ug0[ii]; + d_ug[ii+ng] = -d_lg0[ii]; + } + + // in case ns > 0, CC contains softened box constraints and + // both regular and softened general constraints + + // initialize CC to 0.0 + for (int ii = 0; ii < nv2 * ng2; ii++) CC[ii] = 0.0; + + // copy [C; -C] to upper left corner of CC + for (int ii = 0; ii < ng; ii++) + { + for (int jj = 0; jj < nv; jj++) + { + CC[jj + nv2*ii] = C[jj + nv*ii]; + CC[jj + nv2*(ii+ng)] = -C[jj + nv*ii]; + } + } + + // insert soft constraints into CC + int k_sb = 0, k_sg = 0, row_b = 2*ng; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + if (js < nb) + { + // index of a soft box constraint + int jx = idxb[js]; + + // x_i - su_i <= ub_i + CC[jx + nv2*row_b] = 1.0; + CC[nv + ns + k_sb + nv2*row_b] = -1.0; + d_ug[row_b] = d_ub0[js]; + + row_b++; + + // -x_i - sl_i <= -lb_i + CC[jx + nv2*row_b] = -1.0; + CC[nv + k_sb + nv2*row_b] = -1.0; + d_ug[row_b] = -d_lb0[js]; + + row_b++; k_sb++; + } + else + { + // index of a soft general constraint + int row_g = js - nb; + + // C_i x - su_i <= ug_i + CC[nv + ns + nsb + k_sg + nv2*row_g] = -1.0; + + row_g += ng; + + // -C_i x - sl_i <= -lg_i + CC[nv + nsb + k_sg + nv2*row_g] = -1.0; + + k_sg++; + } + } + } + + // cholesky factorization of H // blasfeo_dpotrf_l(nvd, qpd->Hv, 0, 0, sR, 0, 0); @@ -294,11 +474,11 @@ 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) @@ -307,8 +487,10 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo 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); @@ -316,7 +498,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); @@ -326,7 +510,7 @@ 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) @@ -352,9 +536,9 @@ 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); @@ -365,8 +549,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 @@ -381,13 +569,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); @@ -395,7 +587,7 @@ 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); @@ -441,21 +633,21 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo 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(nv, prim_sol, qp_out->v, 0); + for (int ii = 0; ii < 2 * nb + 2 * ng; 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[nv + ii] >= 0.0) + qp_out->lam->pa[nb + ii] = dual_sol[nv + ii]; else - qp_out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv + ii]; } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); @@ -464,11 +656,11 @@ 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); + blasfeo_dvecex_sp(nb, 1.0, qp_in->idxb, qp_out->v, 0, qp_out->t, nb + ng); + blasfeo_dgemv_t(nv, ng, 1.0, qp_in->Ct, 0, 0, qp_out->v, 0, 0.0, qp_out->t, 2 * nb + ng, + qp_out->t, 2 * nb + ng); + blasfeo_dveccpsc(nb + ng, -1.0, qp_out->t, nb + ng, qp_out->t, 0); + blasfeo_daxpy(2 * nb + 2 * ng, -1.0, qp_in->d, 0, qp_out->t, 0, qp_out->t, 0); } 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..34574eaea3 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -43,8 +43,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 +58,15 @@ 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 *idxs; double *prim_sol; double *dual_sol; void *QPB; // NOTE(giaf): cast to QProblemB to use From ed9193449634ab93fdc72ec985695183503d9a46 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 28 May 2018 13:12:07 +0200 Subject: [PATCH 02/27] fixed dual variabls in qpoases for ns>0 --- acados/dense_qp/dense_qp_common.c | 2 ++ acados/dense_qp/dense_qp_qpoases.c | 50 +++++++++++++++++++++++------- examples/c/dense_qp.c | 19 +++++------- 3 files changed, 48 insertions(+), 23 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 2f69255de9..2cfa0fa549 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); diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index ef0b0a9082..fc0066411c 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -57,6 +57,7 @@ #include "acados/dense_qp/dense_qp_qpoases.h" #include "acados/utils/mem.h" #include "acados/utils/timing.h" +#include "acados/utils/print.h" /************************************************ * opts @@ -336,10 +337,10 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo for (int ii = 0; ii < ns; ii++) { d_lb[nv+ii] = d_ls[ii]; - d_lb[nv+ns+ii] = d_ls[ii]; + d_lb[nv+ns+ii] = d_us[ii]; - d_ub[nv+ii] = d_us[ii]; - d_ub[nv+ns+ii] = d_us[ii]; + d_ub[nv+ii] = +QPOASES_INFTY; + d_ub[nv+ns+ii] = +QPOASES_INFTY; } if (ns > 0) @@ -427,12 +428,11 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo CC[nv + ns + k_sb + nv2*row_b] = -1.0; d_ug[row_b] = d_ub0[js]; - row_b++; // -x_i - sl_i <= -lb_i CC[jx + nv2*row_b] = -1.0; - CC[nv + k_sb + nv2*row_b] = -1.0; - d_ug[row_b] = -d_lb0[js]; + CC[nv + k_sb + nv2*(row_b+nsb)] = -1.0; + d_ug[row_b+nsb] = -d_lb0[js]; row_b++; k_sb++; } @@ -454,7 +454,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } } - // cholesky factorization of H // blasfeo_dpotrf_l(nvd, qpd->Hv, 0, 0, sR, 0, 0); @@ -626,6 +625,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo QProblemB_getDualSolution(QPB, dual_sol); } } + // save solution statistics to memory memory->cputime = cputime; memory->nwsr = nwsr; @@ -633,8 +633,8 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo acados_tic(&interface_timer); // copy prim_sol and dual_sol to qpd_sol - blasfeo_pack_dvec(nv, prim_sol, qp_out->v, 0); - for (int ii = 0; ii < 2 * nb + 2 * ng; ii++) qp_out->lam->pa[ii] = 0.0; + 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) @@ -642,12 +642,38 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo else qp_out->lam->pa[nb + ng + ii] = -dual_sol[idxb[ii]]; } + + int k = 0; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + if (js < nb) + { + // dual variables for softened upper box constraints + if (dual_sol[2*ng+k] <= 0.0) + qp_out->lam->pa[nb+ng+js] = -dual_sol[2*ng+k]; + + // dual variables for softened lower box constraints + if (dual_sol[2*ng+nsb+k] <= 0.0) + qp_out->lam->pa[js] = -dual_sol[2*ng+nsb+k]; + } + + // 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]; + + // 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]; + } + for (int ii = 0; ii < ng; ii++) { - if (dual_sol[nv + ii] >= 0.0) - qp_out->lam->pa[nb + ii] = dual_sol[nv + 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[nv + ii]; + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); diff --git a/examples/c/dense_qp.c b/examples/c/dense_qp.c index 510c5854c7..2eabb619d4 100644 --- a/examples/c/dense_qp.c +++ b/examples/c/dense_qp.c @@ -48,7 +48,7 @@ int main() { double zu[] = {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); @@ -58,8 +58,8 @@ int main() { dims.nb = 1; dims.ng = 1; dims.ns = 2; -// dims.nsb = 1; -// dims.nsg = 1; + dims.nsb = 1; + dims.nsg = 1; dense_qp_in *qp_in = dense_qp_in_create(config, &dims); @@ -74,16 +74,16 @@ int main() { 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; + // 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); + if (acados_return != ACADOS_SUCCESS) + return -1; + // print_dense_qp_out(qp_out); /************************************************ @@ -91,9 +91,6 @@ int main() { ************************************************/ 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); From 8360de3a88b8fe36b2ceeaea07c980e67e2351ea Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 28 May 2018 13:15:35 +0200 Subject: [PATCH 03/27] small bug fix --- acados/dense_qp/dense_qp_qpoases.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index fc0066411c..1c6d05f842 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -657,6 +657,8 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo // dual variables for softened lower box constraints if (dual_sol[2*ng+nsb+k] <= 0.0) qp_out->lam->pa[js] = -dual_sol[2*ng+nsb+k]; + + k++; } // dual variables for sl >= d_ls From b8b113477d3909b1a2c1bcec3c2903aa736de5ab Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Thu, 7 Jun 2018 10:33:35 +0200 Subject: [PATCH 04/27] compute slacks for dense qp, soft constraints in qpoases --- acados/dense_qp/dense_qp_common.c | 181 +++++++++++++++++- acados/dense_qp/dense_qp_common.h | 6 + acados/dense_qp/dense_qp_hpipm.c | 1 + acados/dense_qp/dense_qp_qore.c | 1 + acados/dense_qp/dense_qp_qpoases.c | 291 ++++++++++++++--------------- acados/dense_qp/dense_qp_qpoases.h | 5 + 6 files changed, 324 insertions(+), 161 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 2cfa0fa549..f155fc5244 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -205,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; @@ -217,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, @@ -225,16 +227,26 @@ 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_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); @@ -257,3 +269,154 @@ 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 ? 2 * in->ng + 2 * 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 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 == 2*ng+2*nsb && "Dimensions are wrong!"); + + // 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; 0 0 0 0] + blasfeo_dgecp(nv, ng, in->Ct, 0, 0, out->Ct, 0, 0); + + if (ns > 0) + { + // copy -in->Ct to out->Ct, out->Ct = [in->Ct -in->Ct 0 0; 0 0 0 0] + blasfeo_dgecpsc(nv, ng, -1.0, in->Ct, 0, 0, out->Ct, 0, ng); + + // set flags for non-softened box constraints + // use out->m temporarily for this + for (int ii = 0; ii < nb; ii++) + { + BLASFEO_DVECEL(out->m, ii) = 1.0; + } + + int k_sb = 0, k_sg = 0, col_b = 2*ng; + for (int ii = 0; ii < ns; ii++) + { + int js = idxs[ii]; + + if (js < nb) + { + // index of a soft box constraint + int jv = idxb[js]; + + // softened box constraint, set its flag to -1 + BLASFEO_DVECEL(out->m, js) = -1.0; + + // insert softened box constraint into out->Ct, x_i - su_i <= ub_i + BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0; + BLASFEO_DMATEL(out->Ct, nv+ns+k_sb, col_b) = -1.0; + BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b) = -BLASFEO_DVECEL(in->d, nb+ng+js); + + // insert softened box constraint into out->Ct, -x_i - sl_i <= -lb_i + BLASFEO_DMATEL(out->Ct, jv, col_b+nsb) = -1.0; + BLASFEO_DMATEL(out->Ct, nv+k_sb, col_b+nsb) = -1.0; + BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b+nsb) = -BLASFEO_DVECEL(in->d, js); + + col_b++; k_sb++; + } + else + { + // index of a soft general constraint + int col_g = js - nb; + + // C_i x - su_i <= ug_i + BLASFEO_DMATEL(out->Ct, nv + ns + nsb + k_sg, col_g) = -1.0; + col_g += ng; + + // -C_i x - sl_i <= -lg_i + BLASFEO_DMATEL(out->Ct, nv + nsb + k_sg, col_g) = -1.0; + k_sg++; + } + + // 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); + + // set out->lg to -INFTY + blasfeo_dvecse(ng2, -1.0e6, out->d, nb2); + + // copy in->ug and -in->lg to out->ug + blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2); + blasfeo_dveccpsc(ng, -1.0, in->d, nb, out->d, 2*nb2+ng2+ng); + + // 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); + } +} diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index bf7c85a397..34a470095a 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,16 @@ 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); #ifdef __cplusplus } /* extern "C" */ diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index de1f7bfb09..c074a8c7da 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -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..7dd1e40eb5 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -183,6 +183,7 @@ 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); + info->t_computed = 0; // cast structures dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 1c6d05f842..7258f1a74d 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -59,6 +59,8 @@ #include "acados/utils/timing.h" #include "acados/utils/print.h" +#include "acados_c/dense_qp_interface.h" + /************************************************ * opts ************************************************/ @@ -113,6 +115,8 @@ 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_) { + dense_qp_dims dims_stacked; + int nv = dims->nv; int ne = dims->ne; int ng = dims->ng; @@ -123,6 +127,7 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v int nv2 = nv + 2*ns; int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int nb2 = nb - nsb + 2 * ns; // size in bytes int size = sizeof(dense_qp_qpoases_memory); @@ -130,22 +135,29 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v size += 1 * nv * nv * sizeof(double); // H size += 1 * nv2 * nv2 * sizeof(double); // HH size += 1 * nv2 * nv2 * sizeof(double); // R - size += 1 * nv * ne * sizeof(double); // A + 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 * nb * sizeof(double); // d_lb0 d_ub0 + 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 @@ -160,6 +172,7 @@ 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 nv = dims->nv; int ne = dims->ne; @@ -171,6 +184,7 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o int nv2 = nv + 2*ns; int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int nb2 = nb - nsb + 2 * ns; // char pointer char *c_ptr = (char *) raw_memory; @@ -178,35 +192,47 @@ 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; + } + + 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(nv * ne, &mem->A, &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(nb, &mem->d_lb0, &c_ptr); - assign_and_advance_double(nb, &mem->d_ub0, &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(ns, &mem->d_ls, &c_ptr); - assign_and_advance_double(ns, &mem->d_us, &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!"); @@ -222,6 +248,7 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o } 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_) >= @@ -254,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_; @@ -276,18 +304,20 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo double *d_ug0 = memory->d_ug0; double *d_lg = memory->d_lg; double *d_ug = memory->d_ug; - double *d_ls = memory->d_ls; - double *d_us = memory->d_us; 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 nv = qp_in->dim->nv; @@ -300,159 +330,72 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo int nv2 = nv + 2*ns; int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int nb2 = nb - nsb + 2 * ns; // fill in the upper triangular of H in dense_qp blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); - // dense qp row-major + // 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); + Zl, Zu, zl, zu, idxs, d_ls, d_us); - // in case ns > 0, slack variables are introduced, i.e. v2 = [v; sl; su] // 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 < nb; ii++) - { - d_lb[idxb[ii]] = d_lb0[ii]; - d_ub[idxb[ii]] = d_ub0[ii]; - } - - // if a box constraint is softened, remove it from box constraints bounds - for (int ii = 0; ii < ns; ii++) - { - int js = idxs[ii]; - - if (js < nb) - { - d_lb[idxb[js]] = -QPOASES_INFTY; - d_ub[idxb[js]] = +QPOASES_INFTY; - } - } - - // copy d_ls and d_us to box constraints bounds - for (int ii = 0; ii < ns; ii++) - { - d_lb[nv+ii] = d_ls[ii]; - d_lb[nv+ns+ii] = d_us[ii]; - - d_ub[nv+ii] = +QPOASES_INFTY; - d_ub[nv+ns+ii] = +QPOASES_INFTY; - } if (ns > 0) { - // in case ns > 0, HH = [H 0 0; 0 Zl 0; 0 0 Zu] + dense_qp_stack_slacks(qp_in, qp_stacked); - // initialize HH tp 0.0 - for (int ii = 0; ii < nv2 * nv2; ii++) HH[ii] = 0.0; + #if 0 + dense_qp_solver_plan plan; + plan.qp_solver = DENSE_QP_HPIPM; - // copy H to upper left corner of HH - for (int ii = 0; ii < nv; ii++) - { - for (int jj = 0; jj < nv; jj++) - { - HH[jj + nv2*ii] = H[jj + nv*ii]; - } - } + qp_solver_config *config = dense_qp_config_create(&plan); + void *opts = dense_qp_opts_create(config, qp_stacked->dim); + dense_qp_out *out = dense_qp_out_create(config, qp_stacked->dim); + dense_qp_solver *qp_solver = dense_qp_create(config, qp_stacked->dim, opts); + int acados_return = dense_qp_solve(qp_solver, qp_stacked, out); - // copy Zl and Zu to the main diagonal of HH - for (int ii = 0; ii < ns; ii++) - { - int ll = ii + nv; - int uu = ii + nv + ns; + printf("v=\n"); + blasfeo_print_exp_tran_dvec(nv2, out->v, 0); - HH[ll + nv2*ll] = Zl[ii]; - HH[uu + nv2*uu] = Zu[ii]; - } + printf("lam=\n"); + blasfeo_print_exp_tran_dvec(2*qp_stacked->dim->nb + 2*qp_stacked->dim->ng, out->lam, 0); - // in case ns > 0, gg = [g; zl; zu] - // copy g to gg - for (int ii = 0; ii < nv; ii++) gg[ii] = g[ii]; + double res[4]; + dense_qp_inf_norm_residuals(qp_stacked->dim, qp_stacked, out, res); + printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); - // copy zl and zu to gg - for (int ii = 0; ii < ns; ii++) - { - gg[ii+nv] = zl[ii]; - gg[ii+nv+ns] = zu[ii]; - } - - for (int ii = 0; ii < ng2; ii++) - { - // initialize d_ug to 0.0 - d_ug[ii] = 0.0; + exit(1); + #endif - // set d_lg[i] to -INFTY - d_lg[ii] = -QPOASES_INFTY; - } + 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); - // copy [d_ug0;-d_lg0] to d_ug - for (int ii = 0; ii < ng; ii++) + for (int ii = 0; ii < nb2; ii++) { - d_ug[ii] = d_ug0[ii]; - d_ug[ii+ng] = -d_lg0[ii]; + d_lb[idxb_stacked[ii]] = d_lb0[ii]; + d_ub[idxb_stacked[ii]] = d_ub0[ii]; } - - // in case ns > 0, CC contains softened box constraints and - // both regular and softened general constraints - - // initialize CC to 0.0 - for (int ii = 0; ii < nv2 * ng2; ii++) CC[ii] = 0.0; - - // copy [C; -C] to upper left corner of CC - for (int ii = 0; ii < ng; ii++) + } + else + { + for (int ii = 0; ii < nb; ii++) { - for (int jj = 0; jj < nv; jj++) - { - CC[jj + nv2*ii] = C[jj + nv*ii]; - CC[jj + nv2*(ii+ng)] = -C[jj + nv*ii]; - } + d_lb[idxb[ii]] = d_lb0[ii]; + d_ub[idxb[ii]] = d_ub0[ii]; } + } - // insert soft constraints into CC - int k_sb = 0, k_sg = 0, row_b = 2*ng; - for (int ii = 0; ii < ns; ii++) - { - int js = idxs[ii]; - - if (js < nb) - { - // index of a soft box constraint - int jx = idxb[js]; - - // x_i - su_i <= ub_i - CC[jx + nv2*row_b] = 1.0; - CC[nv + ns + k_sb + nv2*row_b] = -1.0; - d_ug[row_b] = d_ub0[js]; - - - // -x_i - sl_i <= -lb_i - CC[jx + nv2*row_b] = -1.0; - CC[nv + k_sb + nv2*(row_b+nsb)] = -1.0; - d_ug[row_b+nsb] = -d_lb0[js]; - - row_b++; k_sb++; - } - else - { - // index of a soft general constraint - int row_g = js - nb; - - // C_i x - su_i <= ug_i - CC[nv + ns + nsb + k_sg + nv2*row_g] = -1.0; - - row_g += ng; - - // -C_i x - sl_i <= -lg_i - CC[nv + nsb + k_sg + nv2*row_g] = -1.0; + // printf("d_lb =\n"); + // for (int ii = 0; ii < nv2; ii++) printf("%e\n", d_lb[ii]); - k_sg++; - } - } - } + // printf("d_ub =\n"); + // for (int ii = 0; ii < nv2; ii++) printf("%e\n", d_ub[ii]); // cholesky factorization of H // blasfeo_dpotrf_l(nvd, qpd->Hv, 0, 0, sR, 0, 0); @@ -626,6 +569,10 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } } + // printf("dual_sol=\n"); + // for (int ii = 0; ii < nv2 + ng2; ii++) printf("%e\t", dual_sol[ii]); + // printf("\n"); + // save solution statistics to memory memory->cputime = cputime; memory->nwsr = nwsr; @@ -651,31 +598,34 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo if (js < nb) { // dual variables for softened upper box constraints - if (dual_sol[2*ng+k] <= 0.0) - qp_out->lam->pa[nb+ng+js] = -dual_sol[2*ng+k]; + if (dual_sol[nv2 + 2*ng + k] <= 0.0) + qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + 2*ng + k]; // dual variables for softened lower box constraints - if (dual_sol[2*ng+nsb+k] <= 0.0) - qp_out->lam->pa[js] = -dual_sol[2*ng+nsb+k]; + if (dual_sol[nv2 + 2*ng + nsb + k] <= 0.0) + qp_out->lam->pa[js] = -dual_sol[nv2 + 2*ng + nsb + k]; k++; } // 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]; + if (dual_sol[nv + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ii] = dual_sol[nv + ii]; // 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]; + if (dual_sol[nv + ns + ii] >= 0) + qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + 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]; + // dual variables for upper general constraints + if (dual_sol[nv2 + ii] <= 0.0) + qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ii]; + + // dual variables for lower general constraints + if (dual_sol[nv2 + ng + ii] <= 0.0) + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ng + ii]; } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); @@ -684,13 +634,50 @@ 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(nb, 1.0, qp_in->idxb, qp_out->v, 0, qp_out->t, nb + ng); - blasfeo_dgemv_t(nv, ng, 1.0, qp_in->Ct, 0, 0, qp_out->v, 0, 0.0, qp_out->t, 2 * nb + ng, - qp_out->t, 2 * nb + ng); - blasfeo_dveccpsc(nb + ng, -1.0, qp_out->t, nb + ng, qp_out->t, 0); - blasfeo_daxpy(2 * nb + 2 * ng, -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; } + double res[4]; + dense_qp_inf_norm_residuals(qp_in->dim, qp_in, qp_out, res); + printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); + + printf("status = %d\n", qpoases_status); + +#if 1 + int nbd = qp_stacked->dim->nb; + int ngd = qp_stacked->dim->ng; + int nvd = qp_stacked->dim->nv; + + dense_qp_out *out = dense_qp_out_create(config_, qp_stacked->dim); + blasfeo_pack_dvec(nvd, prim_sol, out->v, 0); + for (int ii = 0; ii < 2 * nbd + 2 * ngd; ii++) out->lam->pa[ii] = 0.0; + for (int ii = 0; ii < nbd; ii++) + { + if (dual_sol[idxb_stacked[ii]] >= 0.0) + out->lam->pa[ii] = dual_sol[idxb_stacked[ii]]; + else + out->lam->pa[nbd + ngd + ii] = -dual_sol[idxb_stacked[ii]]; + } + for (int ii = 0; ii < ngd; ii++) + { + if (dual_sol[nvd + ii] >= 0.0) + out->lam->pa[nbd + ii] = dual_sol[nvd + ii]; + else + out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; + } + + blasfeo_print_exp_tran_dvec(nvd, out->v, 0); + blasfeo_print_exp_tran_dvec(2*nbd+2*ngd, out->lam, 0); + + dense_qp_compute_t(qp_stacked, out); + + dense_qp_inf_norm_residuals(qp_stacked->dim, qp_stacked, out, res); + printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); + + exit(1); +#endif + int acados_status = qpoases_status; if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS; if (qpoases_status == RET_MAX_NWSR_REACHED) acados_status = ACADOS_MAXITER; diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 34574eaea3..31eb30f9a5 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" @@ -66,6 +69,7 @@ typedef struct dense_qp_qpoases_memory_ double *d_ls; double *d_us; int *idxb; + int *idxb_stacked; int *idxs; double *prim_sol; double *dual_sol; @@ -74,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); From 25b8851657ce187b27a3ea457905b6af6d9f7340 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Thu, 7 Jun 2018 10:34:01 +0200 Subject: [PATCH 05/27] compute slacks in ocp qp --- acados/ocp_qp/ocp_qp_common.c | 85 +++++++++++-------- acados/ocp_qp/ocp_qp_common.h | 1 + acados/ocp_qp/ocp_qp_full_condensing_solver.c | 1 + acados/ocp_qp/ocp_qp_hpipm.c | 1 + 4 files changed, 52 insertions(+), 36 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 341c6da3e6..f1045d62a7 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,51 @@ 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; + ocp_qp_info *info = (ocp_qp_info *) qp_out->misc; - 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++) + 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); diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index e097f6269a..dc06e36a09 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; // 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..347200c7eb 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -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; From 082e94d98c4234a9e4e4f31779582d434caddff0 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Thu, 7 Jun 2018 10:34:22 +0200 Subject: [PATCH 06/27] print --- acados/utils/print.c | 11 +++++++++++ 1 file changed, 11 insertions(+) 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) From 753399f1b9a7971078916e5d351fac85d75021fe Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Thu, 7 Jun 2018 10:34:59 +0200 Subject: [PATCH 07/27] examples, bump blasfeo, hpipm --- examples/c/chain_model/chain_model.h | 18 +++++++ examples/c/dense_qp.c | 53 ++++++++++--------- examples/c/mass_spring_example.c | 16 +++--- examples/c/mass_spring_model/mass_spring_qp.c | 6 +++ examples/c/ocp_qp.c | 6 +++ examples/c/wind_turbine_nmpc.c | 8 +-- external/blasfeo | 2 +- external/hpipm | 2 +- 8 files changed, 72 insertions(+), 39 deletions(-) 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 2eabb619d4..77835fdb4a 100644 --- a/examples/c/dense_qp.c +++ b/examples/c/dense_qp.c @@ -9,6 +9,8 @@ #include "acados/utils/print.h" #include "acados_c/dense_qp_interface.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); @@ -30,22 +32,22 @@ 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_QPOASES; @@ -55,11 +57,11 @@ int main() { 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.nb = 2; + dims.ng = 2; + dims.ns = 3; dims.nsb = 1; - dims.nsg = 1; + dims.nsg = 2; dense_qp_in *qp_in = dense_qp_in_create(config, &dims); @@ -69,22 +71,20 @@ 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); printf("STATUS: %d\n", acados_return); - if (acados_return != ACADOS_SUCCESS) - return -1; - -// print_dense_qp_out(qp_out); + // if (acados_return != ACADOS_SUCCESS) + // return -1; /************************************************ * compute inf norm of residuals @@ -99,6 +99,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 10c4888971..5d6f3767b7 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -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 = 2; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -111,16 +111,16 @@ int main() { // choose ocp qp solvers ocp_qp_solver_t ocp_qp_solvers[] = { - PARTIAL_CONDENSING_HPIPM, + // 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 - FULL_CONDENSING_QORE, + // FULL_CONDENSING_QORE, #endif #ifdef ACADOS_WITH_QPOASES FULL_CONDENSING_QPOASES, @@ -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_model/mass_spring_qp.c b/examples/c/mass_spring_model/mass_spring_qp.c index 76a520c9dc..681507212a 100644 --- a/examples/c/mass_spring_model/mass_spring_qp.c +++ b/examples/c/mass_spring_model/mass_spring_qp.c @@ -187,6 +187,9 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring(int N, int nx_, int nu_, int nb_, in 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]; } @@ -653,6 +656,9 @@ ocp_qp_dims *create_ocp_qp_dims_mass_spring_soft_constr(int N, int nx_, int nu_, 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]; } diff --git a/examples/c/ocp_qp.c b/examples/c/ocp_qp.c index c13789559b..c61cb4b13a 100644 --- a/examples/c/ocp_qp.c +++ b/examples/c/ocp_qp.c @@ -35,6 +35,9 @@ int main() { int nb[] = {2, 0, 0, 0, 0, 0}; int ng[] = {0, 0, 0, 0, 0, 0}; int ns[] = {0, 0, 0, 0, 0, 0}; + int nsbx[] = {0, 0, 0, 0, 0, 0}; + int nsbu[] = {0, 0, 0, 0, 0, 0}; + int nsg[] = {0, 0, 0, 0, 0, 0}; int nbx[] = {2, 0, 0, 0, 0, 0}; int nbu[] = {0, 0, 0, 0, 0, 0}; @@ -44,6 +47,9 @@ int main() { dims.nb = nb; dims.ng = ng; dims.ns = ns; + dims.nsbx = nsbx; + dims.nsbu = nsbu; + dims.nsg = nsg; dims.nbx = nbx; dims.nbu = nbu; diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index 17073c6f76..6854d674b1 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -125,7 +125,7 @@ static void select_dynamics_wt_casadi(int N, impl_ode_fun_jac_x_xdot_u[ii].casadi_sparsity_out = &wt_nx6p2_impl_ode_fun_jac_x_xdot_u_sparsity_out; impl_ode_fun_jac_x_xdot_u[ii].casadi_n_in = &wt_nx6p2_impl_ode_fun_jac_x_xdot_u_n_in; impl_ode_fun_jac_x_xdot_u[ii].casadi_n_out = &wt_nx6p2_impl_ode_fun_jac_x_xdot_u_n_out; - + // GNSF functions // phi_fun phi_fun[ii].casadi_fun = &wt_nx6p2_phi_fun; @@ -504,9 +504,9 @@ int main() for (int i = 0; i <= NN; i++) plan->nlp_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 = PARTIAL_CONDENSING_HPIPM; + // plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; + plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES; 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..3bf4aba778 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 2eb8b1f2846eb2a17558f1747f8b6af2da9e692e +Subproject commit 3bf4aba7788a394924bde3969081e87ab006f4b1 From 78ef1ace2bb01a46b0c1d274acd6295caa849ead Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 8 Jun 2018 14:48:20 +0200 Subject: [PATCH 08/27] changed the order of slacks in stacked qp to match the ordering in hpipm --- acados/dense_qp/dense_qp_common.c | 20 +++++++++------- acados/dense_qp/dense_qp_qpoases.c | 38 +++++++++++++++++++++--------- examples/c/mass_spring_example.c | 2 +- 3 files changed, 40 insertions(+), 20 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index f155fc5244..084ce894c7 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -303,6 +303,10 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!"); assert(ng2 == 2*ng+2*nsb && "Dimensions are wrong!"); + // set matrice to 0.0 + blasfeo_dgese(nv2, nv2, 0.0, out->Hv, 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); @@ -336,7 +340,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) BLASFEO_DVECEL(out->m, ii) = 1.0; } - int k_sb = 0, k_sg = 0, col_b = 2*ng; + int k_s = 0, col_b = 2*ng; for (int ii = 0; ii < ns; ii++) { int js = idxs[ii]; @@ -351,15 +355,15 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // insert softened box constraint into out->Ct, x_i - su_i <= ub_i BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0; - BLASFEO_DMATEL(out->Ct, nv+ns+k_sb, col_b) = -1.0; + BLASFEO_DMATEL(out->Ct, nv+ns+k_s, col_b) = -1.0; BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b) = -BLASFEO_DVECEL(in->d, nb+ng+js); // insert softened box constraint into out->Ct, -x_i - sl_i <= -lb_i BLASFEO_DMATEL(out->Ct, jv, col_b+nsb) = -1.0; - BLASFEO_DMATEL(out->Ct, nv+k_sb, col_b+nsb) = -1.0; + BLASFEO_DMATEL(out->Ct, nv+k_s, col_b+nsb) = -1.0; BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b+nsb) = -BLASFEO_DVECEL(in->d, js); - col_b++; k_sb++; + col_b++; } else { @@ -367,14 +371,14 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) int col_g = js - nb; // C_i x - su_i <= ug_i - BLASFEO_DMATEL(out->Ct, nv + ns + nsb + k_sg, col_g) = -1.0; - col_g += ng; + BLASFEO_DMATEL(out->Ct, nv + ns + k_s, col_g) = -1.0; // -C_i x - sl_i <= -lg_i - BLASFEO_DMATEL(out->Ct, nv + nsb + k_sg, col_g) = -1.0; - k_sg++; + BLASFEO_DMATEL(out->Ct, nv + k_s, col_g+ng) = -1.0; } + k_s++; + // slack variables have box constraints out->idxb[nb-nsb+ii] = ii + nv; out->idxb[nb-nsb+ns+ii] = ii + nv + ns; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 7258f1a74d..2696dd05c2 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -619,13 +619,23 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo for (int ii = 0; ii < ng; ii++) { - // dual variables for upper general constraints - if (dual_sol[nv2 + ii] <= 0.0) - qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ii]; + if (ns > 0) + { + // dual variables for upper general constraints + if (dual_sol[nv2 + ii] <= 0.0) + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; - // dual variables for lower general constraints - if (dual_sol[nv2 + ng + ii] <= 0.0) - qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ng + ii]; + // dual variables for lower general constraints + if (dual_sol[nv2 + ng + ii] <= 0.0) + qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ng + ii]; + } + else + { + if (dual_sol[nv + ii] >= 0.0) + qp_out->lam->pa[nb + ii] = dual_sol[nv + ii]; + else + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv + ii]; + } } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); @@ -638,13 +648,19 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo info->t_computed = 1; } - double res[4]; - dense_qp_inf_norm_residuals(qp_in->dim, qp_in, qp_out, res); - printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); + // print_dense_qp_dims(qp_in->dim); + // printf("v=\n"); + // blasfeo_print_exp_tran_dvec(nv+2*ns, qp_out->v, 0); + // printf("lam=\n"); + // blasfeo_print_exp_tran_dvec(2*nb+2*ng+2*ns, qp_out->lam, 0); + + // double res[4]; + // dense_qp_inf_norm_residuals(qp_in->dim, qp_in, qp_out, res); + // printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); - printf("status = %d\n", qpoases_status); + // printf("status = %d\n", qpoases_status); -#if 1 +#if 0 int nbd = qp_stacked->dim->nb; int ngd = qp_stacked->dim->ng; int nvd = qp_stacked->dim->nv; diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index 5d6f3767b7..7593048200 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -45,7 +45,7 @@ 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 1 From 63154231fe12603f78469eebeed4742ac7586ed3 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 8 Jun 2018 14:52:29 +0200 Subject: [PATCH 09/27] enabled soft constraints in mass spring example --- examples/c/mass_spring_example.c | 2 +- external/swig | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index 669b90863b..e7cff81a3d 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -45,7 +45,7 @@ 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 1 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 From fe6bc4df6f4ee09c4873f5d6b11edd7a6ced2a8b Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 8 Jun 2018 14:59:30 +0200 Subject: [PATCH 10/27] cleaned the code --- acados/dense_qp/dense_qp_qpoases.c | 82 +----------------------------- 1 file changed, 1 insertion(+), 81 deletions(-) diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 7f9d47a3d5..965f7fb353 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -349,30 +349,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo if (ns > 0) { dense_qp_stack_slacks(qp_in, qp_stacked); - - #if 0 - dense_qp_solver_plan plan; - plan.qp_solver = DENSE_QP_HPIPM; - - qp_solver_config *config = dense_qp_config_create(&plan); - void *opts = dense_qp_opts_create(config, qp_stacked->dim); - dense_qp_out *out = dense_qp_out_create(config, qp_stacked->dim); - dense_qp_solver *qp_solver = dense_qp_create(config, qp_stacked->dim, opts); - int acados_return = dense_qp_solve(qp_solver, qp_stacked, out); - - printf("v=\n"); - blasfeo_print_exp_tran_dvec(nv2, out->v, 0); - - printf("lam=\n"); - blasfeo_print_exp_tran_dvec(2*qp_stacked->dim->nb + 2*qp_stacked->dim->ng, out->lam, 0); - - double res[4]; - dense_qp_inf_norm_residuals(qp_stacked->dim, qp_stacked, out, res); - printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); - - exit(1); - #endif - 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); @@ -391,12 +367,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } } - // printf("d_lb =\n"); - // for (int ii = 0; ii < nv2; ii++) printf("%e\n", d_lb[ii]); - - // printf("d_ub =\n"); - // for (int ii = 0; ii < nv2; ii++) printf("%e\n", d_ub[ii]); - // cholesky factorization of H // blasfeo_dpotrf_l(nvd, qpd->Hv, 0, 0, sR, 0, 0); @@ -569,17 +539,13 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo } } - // printf("dual_sol=\n"); - // for (int ii = 0; ii < nv2 + ng2; ii++) printf("%e\t", dual_sol[ii]); - // printf("\n"); - // save solution statistics to memory memory->cputime = cputime; memory->nwsr = nwsr; info->solve_QP_time = acados_toc(&qp_timer); acados_tic(&interface_timer); - // copy prim_sol and dual_sol to qpd_sol + // 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++) @@ -648,52 +614,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo info->t_computed = 1; } - // print_dense_qp_dims(qp_in->dim); - // printf("v=\n"); - // blasfeo_print_exp_tran_dvec(nv+2*ns, qp_out->v, 0); - // printf("lam=\n"); - // blasfeo_print_exp_tran_dvec(2*nb+2*ng+2*ns, qp_out->lam, 0); - - // double res[4]; - // dense_qp_inf_norm_residuals(qp_in->dim, qp_in, qp_out, res); - // printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); - - // printf("status = %d\n", qpoases_status); - -#if 0 - int nbd = qp_stacked->dim->nb; - int ngd = qp_stacked->dim->ng; - int nvd = qp_stacked->dim->nv; - - dense_qp_out *out = dense_qp_out_create(config_, qp_stacked->dim); - blasfeo_pack_dvec(nvd, prim_sol, out->v, 0); - for (int ii = 0; ii < 2 * nbd + 2 * ngd; ii++) out->lam->pa[ii] = 0.0; - for (int ii = 0; ii < nbd; ii++) - { - if (dual_sol[idxb_stacked[ii]] >= 0.0) - out->lam->pa[ii] = dual_sol[idxb_stacked[ii]]; - else - out->lam->pa[nbd + ngd + ii] = -dual_sol[idxb_stacked[ii]]; - } - for (int ii = 0; ii < ngd; ii++) - { - if (dual_sol[nvd + ii] >= 0.0) - out->lam->pa[nbd + ii] = dual_sol[nvd + ii]; - else - out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; - } - - blasfeo_print_exp_tran_dvec(nvd, out->v, 0); - blasfeo_print_exp_tran_dvec(2*nbd+2*ngd, out->lam, 0); - - dense_qp_compute_t(qp_stacked, out); - - dense_qp_inf_norm_residuals(qp_stacked->dim, qp_stacked, out, res); - printf("\ninf norm res: %e, %e, %e, %e\n\n", res[0], res[1], res[2], res[3]); - - exit(1); -#endif - int acados_status = qpoases_status; if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS; if (qpoases_status == RET_MAX_NWSR_REACHED) acados_status = ACADOS_MAXITER; From 55dabaacb7a91ab5497e85bb71845f60db6fa889 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 8 Jun 2018 16:47:38 +0200 Subject: [PATCH 11/27] fixed mass spring --- examples/c/mass_spring_example.c | 4 ++-- examples/c/mass_spring_model/mass_spring_qp.c | 9 +++++++++ 2 files changed, 11 insertions(+), 2 deletions(-) diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index e7cff81a3d..52b67f234c 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -92,7 +92,7 @@ int main() { int num_N2_values = 3; int N2_values[3] = {15, 5, 3}; - int ii_max = 2; + int ii_max = 3; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -111,7 +111,7 @@ int main() { // choose ocp qp solvers ocp_qp_solver_t ocp_qp_solvers[] = { - // PARTIAL_CONDENSING_HPIPM, + PARTIAL_CONDENSING_HPIPM, #ifdef ACADOS_WITH_HPMPC // PARTIAL_CONDENSING_HPMPC, #endif diff --git a/examples/c/mass_spring_model/mass_spring_qp.c b/examples/c/mass_spring_model/mass_spring_qp.c index 681507212a..f121457b1e 100644 --- a/examples/c/mass_spring_model/mass_spring_qp.c +++ b/examples/c/mass_spring_model/mass_spring_qp.c @@ -252,8 +252,14 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, i ng[N] = ngN; int ns[N+1]; + int nsbx[N+1]; + int nsbu[N+1]; + int nsg[N+1]; for (int ii = 0; ii <= N; ii++) { ns[ii] = 0; + nsbx[ii] = 0; + nsbu[ii] = 0; + nsg[ii] = 0; } // printf("Test problem: mass-spring system with %d masses and %d controls.\n\n", nx_ / 2, nu_); @@ -530,6 +536,9 @@ ocp_qp_in *create_ocp_qp_in_mass_spring(void *config, int N, int nx_, int nu_, i dims.nb = nb; dims.ng = ng; dims.ns = ns; + dims.nsbx = nsbx; + dims.nsbu = nsbu; + dims.nsg = nsg; dims.nbu = nbu; dims.nbx = nbx; From 32b0331ab38650431a3f5f963955d1846dd04882 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 8 Jun 2018 16:48:20 +0200 Subject: [PATCH 12/27] added t_computed to partial condensed solver --- acados/ocp_qp/ocp_qp_partial_condensing_solver.c | 1 + 1 file changed, 1 insertion(+) diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 6219cb9dc6..9452943122 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; } From 590d8ae9ef80e57fa4e904c04da3438dcb8b6b60 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 11 Jun 2018 10:53:46 +0200 Subject: [PATCH 13/27] started on soft constraints in qore interface --- acados/dense_qp/dense_qp_qore.c | 150 +++++++++++++++++++++++--------- acados/dense_qp/dense_qp_qore.h | 15 ++++ 2 files changed, 122 insertions(+), 43 deletions(-) diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 7dd1e40eb5..8dff29dd1b 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -84,28 +84,49 @@ 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) ? 2*(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 * 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); + 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) ? 2*(ng + nsb) : ng; + int nb2 = nb - nsb + 2*ns; // char pointer char *c_ptr = (char *) raw_memory; @@ -129,33 +158,60 @@ 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(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->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); @@ -191,10 +247,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; @@ -202,10 +262,16 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void double *d_ub = memory->d_ub; double *d_lg = memory->d_lg; double *d_ug = memory->d_ug; + double *d_lg0 = memory->d_lg0; + double *d_ug0 = memory->d_ug0; double *lb = memory->lb; double *ub = memory->ub; 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; // extract dense qp size int nvd = qp_in->dim->nv; @@ -276,8 +342,6 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void 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; diff --git a/acados/dense_qp/dense_qp_qore.h b/acados/dense_qp/dense_qp_qore.h index 6c0b38e4ca..a22d49862b 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -48,24 +48,39 @@ typedef struct 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_lg0; + double *d_ug0; 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); From f4351ada67d1e7af0d26e92803f6666160df5af5 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 11 Jun 2018 11:18:10 +0200 Subject: [PATCH 14/27] merging tweaks from giaf fork --- acados/dense_qp/dense_qp_common.c | 163 +++++++++++++++++++++++++---- acados/dense_qp/dense_qp_common.h | 2 + acados/dense_qp/dense_qp_qpoases.c | 2 +- 3 files changed, 146 insertions(+), 21 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 084ce894c7..c77e11315e 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -227,12 +227,12 @@ void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out) 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); - // 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); + // 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_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); } @@ -296,6 +296,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) int *idxb = in->idxb; int nv2 = out->dim->nv; + int ne2 = out->dim->ne; int nb2 = out->dim->nb; int ng2 = out->dim->ng; @@ -303,8 +304,9 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!"); assert(ng2 == 2*ng+2*nsb && "Dimensions are wrong!"); - // set matrice to 0.0 + // 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] @@ -337,48 +339,60 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // use out->m temporarily for this for (int ii = 0; ii < nb; ii++) { + // TODO: pick up some workspace for this BLASFEO_DVECEL(out->m, ii) = 1.0; } - int k_s = 0, col_b = 2*ng; + int col_b = 2*ng; for (int ii = 0; ii < ns; ii++) { int js = idxs[ii]; - if (js < nb) + 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 = 2*nb2+ng2+col_b+nsb; + 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, x_i - su_i <= ub_i + // insert softened box constraint into out->Ct (upper bound), x_i - su_i <= ub_i BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0; - BLASFEO_DMATEL(out->Ct, nv+ns+k_s, col_b) = -1.0; - BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b) = -BLASFEO_DVECEL(in->d, nb+ng+js); + BLASFEO_DMATEL(out->Ct, idx_v_us1, col_b) = -1.0; + BLASFEO_DVECEL(out->d, idx_d_us1) = -BLASFEO_DVECEL(in->d, idx_d_us0); - // insert softened box constraint into out->Ct, -x_i - sl_i <= -lb_i + // insert softened box constraint into out->Ct (lower bound), -x_i - sl_i <= -lb_i BLASFEO_DMATEL(out->Ct, jv, col_b+nsb) = -1.0; - BLASFEO_DMATEL(out->Ct, nv+k_s, col_b+nsb) = -1.0; - BLASFEO_DVECEL(out->d, 2*nb2+ng2+col_b+nsb) = -BLASFEO_DVECEL(in->d, js); + BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_b+nsb) = -1.0; + BLASFEO_DVECEL(out->d, idx_d_ls1) = -BLASFEO_DVECEL(in->d, idx_d_ls0); col_b++; } - else + else // soft general constraint { // index of a soft general constraint int col_g = js - nb; - // C_i x - su_i <= ug_i - BLASFEO_DMATEL(out->Ct, nv + ns + k_s, col_g) = -1.0; + // soft general constraint (upper bound), C_i x - su_i <= ug_i + BLASFEO_DMATEL(out->Ct, idx_v_us1, col_g) = -1.0; - // -C_i x - sl_i <= -lg_i - BLASFEO_DMATEL(out->Ct, nv + k_s, col_g+ng) = -1.0; + // soft general constraint (lower bound), -C_i x - sl_i <= -lg_i + BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_g+ng) = -1.0; } - k_s++; - // slack variables have box constraints out->idxb[nb-nsb+ii] = ii + nv; out->idxb[nb-nsb+ns+ii] = ii + nv + ns; @@ -424,3 +438,112 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) blasfeo_dveccp(2*nb+2*ng, in->m, 0, out->m, 0); } } + + + +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++) + { + BLASFEO_DVECEL(out->v, ii) = 1.0; // TODO pick up some workspace for this + } + + 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 = 2*nb2+ng2+col_b+nsb; + 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, 2*nb2+ng2+ng, 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, 2*nb2+ng2+ng, 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; +} \ No newline at end of file diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index 34a470095a..faf706d76f 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -99,6 +99,8 @@ 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_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 965f7fb353..69097e4b49 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -135,7 +135,7 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v 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 * 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 From 2a9281a31e144568df02e41e850457fb0e793323 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 11 Jun 2018 11:18:44 +0200 Subject: [PATCH 15/27] fixed mass spring examples --- examples/c/mass_spring_example.c | 8 +- examples/c/mass_spring_example_no_interface.c | 4 +- examples/c/mass_spring_fcond_split.c | 4 +- examples/c/mass_spring_model/mass_spring_qp.c | 501 +++++++----------- .../mass_spring_offline_fcond_qpoases_split.c | 4 +- examples/c/mass_spring_pcond_split.c | 4 +- 6 files changed, 204 insertions(+), 321 deletions(-) diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index 52b67f234c..1f90ae535e 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 @@ -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); 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 f121457b1e..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,103 +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->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]; - } + 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]; - int nsbx[N+1]; - int nsbu[N+1]; - int nsg[N+1]; - for (int ii = 0; ii <= N; ii++) { - ns[ii] = 0; - nsbx[ii] = 0; - nsbu[ii] = 0; - nsg[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 @@ -528,23 +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.nsbx = nsbx; - dims.nsbu = nsbu; - dims.nsg = nsg; - 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); @@ -597,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]; @@ -620,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++) { @@ -645,104 +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->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]; - } + 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 @@ -898,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; ii Date: Mon, 11 Jun 2018 12:07:28 +0200 Subject: [PATCH 16/27] enabled soft constraints in qore interface --- acados/dense_qp/dense_qp_qore.c | 167 ++++++++++++++++++++++++-------- acados/dense_qp/dense_qp_qore.h | 1 + 2 files changed, 126 insertions(+), 42 deletions(-) diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 8dff29dd1b..912df63608 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; } @@ -239,6 +240,7 @@ 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 @@ -266,61 +268,92 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void double *d_ug0 = memory->d_ug0; 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; - - if (nsd > 0) - { - printf("\nQORE interface can not handle ns>0 yet: what about implementing it? :)\n"); - return ACADOS_FAILURE; - } + 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; - acados_tic(&interface_timer); + int nv2 = nv + 2*ns; + int ng2 = (ns > 0) ? 2*(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_lg0, d_ug0, + 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 @@ -329,16 +362,18 @@ 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); @@ -352,27 +387,75 @@ 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++) + + 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]; + + if (js < nb) + { + // dual variables for softened upper box constraints + if (dual_sol[nv2 + 2*ng + k] <= 0.0) + qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + 2*ng + k]; + + // dual variables for softened lower box constraints + if (dual_sol[nv2 + 2*ng + nsb + k] <= 0.0) + qp_out->lam->pa[js] = -dual_sol[nv2 + 2*ng + nsb + k]; + + k++; + } + + // 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]; + + // 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]; + } + + for (int ii = 0; ii < ng; ii++) + { + if (ns > 0) + { + // dual variables for upper general constraints + if (dual_sol[nv2 + ii] <= 0.0) + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; + + // dual variables for lower general constraints + if (dual_sol[nv2 + ng + ii] <= 0.0) + qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ng + ii]; + } else - qp_out->lam->pa[2 * nbd + ngd + ii] = -dual_sol[nvd + ii]; + { + if (dual_sol[nv + ii] >= 0.0) + qp_out->lam->pa[nb + ii] = dual_sol[nv + ii]; + else + qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv + ii]; + } } 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 a22d49862b..01a5253f0c 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -43,6 +43,7 @@ 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_ From caafd78fd5cc64358df7e33a325adc3928ff851a Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 11 Jun 2018 12:08:13 +0200 Subject: [PATCH 17/27] tested qore with soft constraints in mass spring and wind turbine examples --- examples/c/mass_spring_example.c | 4 ++-- examples/c/wind_turbine_nmpc.c | 5 +++-- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/examples/c/mass_spring_example.c b/examples/c/mass_spring_example.c index 1f90ae535e..e3add3f7ef 100644 --- a/examples/c/mass_spring_example.c +++ b/examples/c/mass_spring_example.c @@ -92,7 +92,7 @@ int main() { int num_N2_values = 3; int N2_values[3] = {15, 5, 3}; - int ii_max = 3; + int ii_max = 4; #ifndef ACADOS_WITH_HPMPC ii_max--; @@ -120,7 +120,7 @@ int main() { #endif FULL_CONDENSING_HPIPM, #ifdef ACADOS_WITH_QORE - // FULL_CONDENSING_QORE, + FULL_CONDENSING_QORE, #endif #ifdef ACADOS_WITH_QPOASES FULL_CONDENSING_QPOASES, diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index 9b9f74a7ef..f3d116264c 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -504,9 +504,10 @@ int main() for (int i = 0; i <= NN; i++) plan->nlp_cost[i] = LINEAR_LS; - // plan->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM; + 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_QPOASES; + // plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QORE; for (int i = 0; i < NN; i++) { From a247b8d4db221be3fabe813c5b0d9224bafb0702 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Thu, 14 Jun 2018 10:45:52 +0200 Subject: [PATCH 18/27] used acados infty for bounds --- acados/dense_qp/dense_qp_common.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index c77e11315e..440b922494 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -417,10 +417,10 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) 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); + blasfeo_dvecse(2*ns, ACADOS_POS_INFTY, out->d, nb2+ng2+k_nsb); // set out->lg to -INFTY - blasfeo_dvecse(ng2, -1.0e6, out->d, nb2); + blasfeo_dvecse(ng2, ACADOS_NEG_INFTY, out->d, nb2); // copy in->ug and -in->lg to out->ug blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2); From 466d55930927f39cd501cb8fa7a76556161ee7f1 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Fri, 15 Jun 2018 14:06:28 +0200 Subject: [PATCH 19/27] changed the number of general constraints in stacked qp --- acados/dense_qp/dense_qp_common.c | 41 ++++++++----------- acados/dense_qp/dense_qp_qore.c | 64 +++++++++++++++--------------- acados/dense_qp/dense_qp_qpoases.c | 63 +++++++++++++++-------------- 3 files changed, 78 insertions(+), 90 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 440b922494..b166082d4d 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -275,7 +275,7 @@ 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 ? 2 * in->ng + 2 * in->nsb : in->ng; + out->ng = in->ns > 0 ? in->ng + in->nsb : in->ng; out->ns = 0; out->nsb = 0; out->nsg = 0; @@ -302,7 +302,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) 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!"); + assert(ng2 == ng+nsb && "Dimensions are wrong!"); // set matrices to 0.0 blasfeo_dgese(nv2, nv2, 0.0, out->Hv, 0, 0); @@ -327,14 +327,11 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) blasfeo_dveccp(ne, in->b, 0, out->b, 0); } - // copy in->Ct to out->Ct, out->Ct = [in->Ct 0 0 0; 0 0 0 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) { - // copy -in->Ct to out->Ct, out->Ct = [in->Ct -in->Ct 0 0; 0 0 0 0] - blasfeo_dgecpsc(nv, ng, -1.0, in->Ct, 0, 0, out->Ct, 0, ng); - // set flags for non-softened box constraints // use out->m temporarily for this for (int ii = 0; ii < nb; ii++) @@ -343,7 +340,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) BLASFEO_DVECEL(out->m, ii) = 1.0; } - int col_b = 2*ng; + int col_b = ng; for (int ii = 0; ii < ns; ii++) { int js = idxs[ii]; @@ -363,22 +360,19 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // index of a soft box constraint int jv = idxb[js]; - idx_d_ls1 = 2*nb2+ng2+col_b+nsb; + 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 (upper bound), x_i - su_i <= ub_i + // 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); - // insert softened box constraint into out->Ct (lower bound), -x_i - sl_i <= -lb_i - BLASFEO_DMATEL(out->Ct, jv, col_b+nsb) = -1.0; - BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_b+nsb) = -1.0; - BLASFEO_DVECEL(out->d, idx_d_ls1) = -BLASFEO_DVECEL(in->d, idx_d_ls0); - col_b++; } else // soft general constraint @@ -386,11 +380,9 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // index of a soft general constraint int col_g = js - nb; - // soft general constraint (upper bound), C_i x - su_i <= ug_i + // 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; - - // soft general constraint (lower bound), -C_i x - sl_i <= -lg_i - BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_g+ng) = -1.0; } // slack variables have box constraints @@ -419,12 +411,11 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // for slack variables out->ub is +INFTY blasfeo_dvecse(2*ns, ACADOS_POS_INFTY, out->d, nb2+ng2+k_nsb); - // set out->lg to -INFTY - blasfeo_dvecse(ng2, ACADOS_NEG_INFTY, out->d, nb2); + // copy in->lg to out->lg + blasfeo_dveccp(ng, in->d, nb, out->d, nb2); - // copy in->ug and -in->lg to out->ug + // copy in->ug to out->ug blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2); - blasfeo_dveccpsc(ng, -1.0, in->d, nb, out->d, 2*nb2+ng2+ng); // flip signs for ub and ug blasfeo_dvecsc(nb2+ng2, -1.0, out->d, nb2+ng2); @@ -491,7 +482,7 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out // softened box constraint, set its flag to -1 BLASFEO_DVECEL(out->v, js) = -1.0; - idx_d_ls1 = 2*nb2+ng2+col_b+nsb; + 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); @@ -531,11 +522,11 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out 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, 2*nb2+ng2+ng, out->t, nb); + 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, 2*nb2+ng2+ng, out->lam, nb); + blasfeo_dveccp(ng, in->lam, nb2, out->lam, nb); // variables blasfeo_dveccp(nv+2*ns, in->v, 0, out->v, 0); diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index 912df63608..a572478d94 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -97,7 +97,7 @@ int dense_qp_qore_memory_calculate_size(void *config_, dense_qp_dims *dims, void int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2*ns; // size in bytes @@ -150,7 +150,7 @@ void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2*ns; // char pointer @@ -291,7 +291,7 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void int nsg = qp_in->dim->nsg; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2 * ns; // fill in the upper triangular of H in dense_qp @@ -397,52 +397,50 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void 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]; + } + 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) { - // dual variables for softened upper box constraints - if (dual_sol[nv2 + 2*ng + k] <= 0.0) - qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + 2*ng + k]; - - // dual variables for softened lower box constraints - if (dual_sol[nv2 + 2*ng + nsb + k] <= 0.0) - qp_out->lam->pa[js] = -dual_sol[nv2 + 2*ng + nsb + k]; + 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]; + 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]; - } - - for (int ii = 0; ii < ng; ii++) - { - if (ns > 0) - { - // dual variables for upper general constraints - if (dual_sol[nv2 + ii] <= 0.0) - qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; - - // dual variables for lower general constraints - if (dual_sol[nv2 + ng + ii] <= 0.0) - qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ng + ii]; - } - else - { - if (dual_sol[nv + ii] >= 0.0) - qp_out->lam->pa[nb + ii] = dual_sol[nv + ii]; - else - qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv + ii]; - } + qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } info->interface_time += acados_toc(&interface_timer); diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 69097e4b49..632fe38d31 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -126,7 +126,7 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v int ns = dims->ns; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2 * ns; // size in bytes @@ -183,7 +183,7 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o int ns = dims->ns; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2 * ns; // char pointer @@ -329,7 +329,7 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo int ns = qp_in->dim->ns; int nv2 = nv + 2*ns; - int ng2 = (ns > 0) ? 2*(ng + nsb) : ng; + int ng2 = (ns > 0) ? ng + nsb : ng; int nb2 = nb - nsb + 2 * ns; // fill in the upper triangular of H in dense_qp @@ -556,53 +556,52 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo 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]; + } + 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) { - // dual variables for softened upper box constraints - if (dual_sol[nv2 + 2*ng + k] <= 0.0) - qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + 2*ng + k]; - - // dual variables for softened lower box constraints - if (dual_sol[nv2 + 2*ng + nsb + k] <= 0.0) - qp_out->lam->pa[js] = -dual_sol[nv2 + 2*ng + nsb + k]; + 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]; + 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]; + qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l; } - for (int ii = 0; ii < ng; ii++) - { - if (ns > 0) - { - // dual variables for upper general constraints - if (dual_sol[nv2 + ii] <= 0.0) - qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii]; - - // dual variables for lower general constraints - if (dual_sol[nv2 + ng + ii] <= 0.0) - qp_out->lam->pa[nb + ii] = -dual_sol[nv2 + ng + ii]; - } - else - { - if (dual_sol[nv + ii] >= 0.0) - qp_out->lam->pa[nb + ii] = dual_sol[nv + ii]; - else - qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv + ii]; - } - } info->interface_time += acados_toc(&interface_timer); info->total_time = acados_toc(&tot_timer); info->num_iter = nwsr; From 5233fb3f1aec27e5269625eae421fd297e41fb37 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 18 Jun 2018 15:28:37 +0200 Subject: [PATCH 20/27] fixed a small bug in dense qp stack slacks --- acados/dense_qp/dense_qp_common.c | 1 + 1 file changed, 1 insertion(+) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index b166082d4d..9f8c116219 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -427,6 +427,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) { 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]; } } From edf15a9490ed8050bd35ab23c08388a2db18f129 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 18 Jun 2018 15:30:37 +0200 Subject: [PATCH 21/27] added slack stacking to ocp qp common --- acados/ocp_qp/ocp_qp_common.c | 186 ++++++++++++++++++++++++++++++++++ acados/ocp_qp/ocp_qp_common.h | 5 + 2 files changed, 191 insertions(+) diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index f1045d62a7..3fb6dde14f 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -411,3 +411,189 @@ 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: 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 dc06e36a09..faef2f77aa 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -130,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" */ From 81600073e298f588e8947546a191868aebdc2dcb Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 18 Jun 2018 16:30:49 +0200 Subject: [PATCH 22/27] decreased bounds for slacks --- acados/dense_qp/dense_qp_common.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index 9f8c116219..e4a1db413b 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -409,7 +409,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) 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, ACADOS_POS_INFTY, out->d, nb2+ng2+k_nsb); + 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); From c7c6e8cb8ba87de4550097016788f2ad93bc638d Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Mon, 18 Jun 2018 16:40:26 +0200 Subject: [PATCH 23/27] fixed lint --- acados/dense_qp/dense_qp_common.c | 16 +++---- acados/dense_qp/dense_qp_qore.c | 8 ++-- acados/dense_qp/dense_qp_qpoases.c | 8 ++-- acados/dense_qp/dense_qp_qpoases.h | 2 +- acados/ocp_nlp/ocp_nlp_sqp.c | 4 +- acados/ocp_qp/ocp_qp_common.c | 46 ++++++++++--------- .../ocp_qp/ocp_qp_partial_condensing_solver.c | 2 +- 7 files changed, 44 insertions(+), 42 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.c b/acados/dense_qp/dense_qp_common.c index e4a1db413b..dde27e5d41 100644 --- a/acados/dense_qp/dense_qp_common.c +++ b/acados/dense_qp/dense_qp_common.c @@ -230,10 +230,9 @@ void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out) // 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); + 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); } @@ -336,7 +335,7 @@ void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out) // use out->m temporarily for this for (int ii = 0; ii < nb; ii++) { - // TODO: pick up some workspace for this + // TODO(bnovoselnik): pick up some workspace for this BLASFEO_DVECEL(out->m, ii) = 1.0; } @@ -465,7 +464,8 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out for (int ii = 0; ii < nb; ii++) { - BLASFEO_DVECEL(out->v, ii) = 1.0; // TODO pick up some workspace for this + // TODO(bnovoselnik): pick up some workspace for this + BLASFEO_DVECEL(out->v, ii) = 1.0; } int col_b = 2*ng; @@ -496,7 +496,7 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out } } - int k_nsb = 0; // number of non-softed bounds + int k_nsb = 0; // number of non-softed bounds for (int ii = 0; ii < nb; ii++) { int idx_d_ls0 = ii; @@ -538,4 +538,4 @@ void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out } return; -} \ No newline at end of file +} diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index a572478d94..ff20cd91f6 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -311,8 +311,8 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void 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); + 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); for (int ii = 0; ii < nb2; ii++) { @@ -415,12 +415,12 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void if (js < nb) { - if (dual_sol[nv2 + ng + k] <= 0.0) // softened upper box constraints + 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 + else // softened lower box constraints { qp_out->lam->pa[js] = dual_sol[nv2 + ng + k]; offset_l = dual_sol[nv2 + ng + k]; diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index 632fe38d31..8a1f0db5cc 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -349,8 +349,8 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo 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); + 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++) { @@ -574,12 +574,12 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo if (js < nb) { - if (dual_sol[nv2 + ng + k] <= 0.0) // softened upper box constraints + 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 + else // softened lower box constraints { qp_out->lam->pa[js] = dual_sol[nv2 + ng + k]; offset_l = dual_sol[nv2 + ng + k]; diff --git a/acados/dense_qp/dense_qp_qpoases.h b/acados/dense_qp/dense_qp_qpoases.h index 31eb30f9a5..b9ce99b4ba 100644 --- a/acados/dense_qp/dense_qp_qpoases.h +++ b/acados/dense_qp/dense_qp_qpoases.h @@ -78,7 +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_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_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index f1bef9bbd1..5dd61ff2ce 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -782,8 +782,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, ocp_nlp_solver_config *config = config_; ocp_nlp_sqp_opts *opts = opts_; ocp_nlp_sqp_memory *mem = mem_; - ocp_nlp_in *nlp_in = nlp_in_; - ocp_nlp_out *nlp_out = nlp_out_; + ocp_nlp_in *nlp_in = nlp_in_; + ocp_nlp_out *nlp_out = nlp_out_; ocp_qp_xcond_solver_config *qp_solver = config->qp_solver; ocp_nlp_sqp_work *work = work_; diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 3fb6dde14f..32de0faa90 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -326,7 +326,8 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res // 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); + 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; @@ -416,15 +417,15 @@ 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; + 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; @@ -447,15 +448,15 @@ 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 *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; @@ -488,7 +489,8 @@ void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) 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]); + 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); @@ -503,7 +505,7 @@ void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) // use out->m temporarily for this for (int jj = 0; jj < nb[ii]; jj++) { - // TODO: pick up some workspace for this + // TODO(bnovoselnik): pick up some workspace for this BLASFEO_DVECEL(out->m+ii, jj) = 1.0; } @@ -520,7 +522,7 @@ void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) int idx_d_ls1; int idx_d_us1; - if (js < nb[ii]) // soft box constraint + if (js < nb[ii]) // soft box constraint { // index of a soft box constraint int jv = idxb[ii][js]+2*ns[ii]; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 9452943122..3282d79456 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -346,7 +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; + info->t_computed = ((ocp_qp_info *) (memory->pcond_qp_out->misc))->t_computed; return solver_status; } From 1631c6a8c1e1d01d4515711b8145c8350e01891a Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Tue, 19 Jun 2018 10:10:24 +0200 Subject: [PATCH 24/27] fixed a bug in qore --- acados/dense_qp/dense_qp_qore.c | 7 +------ acados/dense_qp/dense_qp_qore.h | 2 -- 2 files changed, 1 insertion(+), 8 deletions(-) diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index ff20cd91f6..14475a6658 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -112,7 +112,6 @@ int dense_qp_qore_memory_calculate_size(void *config_, dense_qp_dims *dims, void 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 * 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 @@ -188,8 +187,6 @@ void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts 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->Zl, &c_ptr); @@ -264,8 +261,6 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void double *d_ub = memory->d_ub; double *d_lg = memory->d_lg; double *d_ug = memory->d_ug; - double *d_lg0 = memory->d_lg0; - double *d_ug0 = memory->d_ug0; double *lb = memory->lb; double *ub = memory->ub; double *Zl = memory->Zl; @@ -298,7 +293,7 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0); // 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_lg0, d_ug0, + 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 diff --git a/acados/dense_qp/dense_qp_qore.h b/acados/dense_qp/dense_qp_qore.h index 01a5253f0c..2800dd743b 100644 --- a/acados/dense_qp/dense_qp_qore.h +++ b/acados/dense_qp/dense_qp_qore.h @@ -66,8 +66,6 @@ typedef struct dense_qp_qore_memory_ double *d_ub0; double *d_lb; double *d_ub; - double *d_lg0; - double *d_ug0; double *d_lg; double *d_ug; double *d_ls; From 834d499aab48cd338d3bc458e455046264b0ed76 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Tue, 3 Jul 2018 11:15:49 +0200 Subject: [PATCH 25/27] upgraded to the newest version of hpipm --- acados/CMakeLists.txt | 1 + acados/dense_qp/dense_qp_hpipm.c | 2 +- acados/ocp_qp/ocp_qp_hpipm.c | 2 +- external/hpipm | 2 +- 4 files changed, 4 insertions(+), 3 deletions(-) diff --git a/acados/CMakeLists.txt b/acados/CMakeLists.txt index 0463af9ba9..68285d6693 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_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index c074a8c7da..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; diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index 347200c7eb..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; diff --git a/external/hpipm b/external/hpipm index 3bf4aba778..7dfadfe8df 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 3bf4aba7788a394924bde3969081e87ab006f4b1 +Subproject commit 7dfadfe8df8c7fd7ace7ad2ee43f604aa3da5d93 From 2f98fb6c5bff446d3813f5f40f00b6840ddbb654 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Tue, 3 Jul 2018 13:20:10 +0200 Subject: [PATCH 26/27] fixed unit tests --- test/ocp_qp/test_qpsolvers.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) 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); From e980f3d9b7fb7f6154dd5ec1629da18ed3cd1c09 Mon Sep 17 00:00:00 2001 From: bnovoselnik Date: Tue, 3 Jul 2018 14:00:46 +0200 Subject: [PATCH 27/27] allow unauthenticated packages to bypass travis error --- .travis_install.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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