Skip to content

Commit

Permalink
Merge pull request #310 from bnovoselnik/ocp_soft
Browse files Browse the repository at this point in the history 
Soft constraints support for interfaces other than HPIPM
  • Loading branch information
@giaf
giaf committed Jul 17, 2018
2 parents 4bb0dad + e980f3d commit a05f719
Show file tree
Hide file tree
Showing 29 changed files with 1,343 additions and 583 deletions.
2 changes: 1 addition & 1 deletion .travis_install.sh
View file
Expand Up @@ -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

Expand Down
1 change: 1 addition & 0 deletions acados/CMakeLists.txt
View file
Expand Up @@ -62,6 +62,7 @@ target_include_directories(acados
$<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/KKTPACK_DENSE/source>
$<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/QPCORE/include>
$<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore>
$<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/hpipm/include>
$<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}>
$<BUILD_INTERFACE:${CMAKE_SOURCE_DIR}/interfaces>
$<INSTALL_INTERFACE:include>)
Expand Down
304 changes: 294 additions & 10 deletions acados/dense_qp/dense_qp_common.c
View file
Expand Up @@ -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);

Expand Down Expand Up @@ -203,8 +205,7 @@ dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_me
return res_ws;
}

void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res,
dense_qp_res_ws *res_ws)
void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out)
{
int nvd = qp_in->dim->nv;
// int ned = qp_in->dim->ne;
Expand All @@ -215,24 +216,36 @@ 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,
nbd);
blasfeo_dgemv_t(nvd, ngd, -1.0, qp_in->Ct, 0, 0, qp_out->v, 0, -1.0, qp_in->d, 2 * nbd + ngd,
qp_out->t, 2 * nbd + ngd);

// compute slacks for bounds
blasfeo_dvecex_sp(nbd, 1.0, idxb, qp_out->v, 0, tmp_nbg + 0, 0);
blasfeo_daxpby(nbd, 1.0, tmp_nbg + 0, 0, -1.0, qp_in->d, 0, qp_out->t, 0);
blasfeo_daxpby(nbd, -1.0, tmp_nbg + 0, 0, -1.0, qp_in->d, nbd + ngd, qp_out->t, nbd + ngd);

// soft
blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd, idxs, qp_out->t, 0);
blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd+nsd, idxs, qp_out->t, nbd+ngd);
// blasfeo_daxpy(2*nsd, -1.0, qp_out->v, nvd, qp_out->t, 2*nbd+2*ngd, qp_out->t, 2*nbd+2*ngd);
blasfeo_dvecse(2*nsd, 0.0, qp_out->t, 2*nbd+2*ngd);
blasfeo_dveccp(2*nsd, qp_out->v, nvd, qp_out->t, 2*nbd+2*ngd);
blasfeo_daxpy(2*nsd, -1.0, qp_in->d, 2*nbd+2*ngd, qp_out->t, 2*nbd+2*ngd, qp_out->t,
2*nbd+2*ngd);

}

void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res,
dense_qp_res_ws *res_ws)
{
dense_qp_info *info = (dense_qp_info *) qp_out->misc;

if (info->t_computed == 0)
{
dense_qp_compute_t(qp_in, qp_out);
info->t_computed = 1;
}

// compute residuals
d_compute_res_dense_qp(qp_in, qp_out, qp_res, res_ws);
Expand All @@ -255,3 +268,274 @@ void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4])
}



void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out)
{
out->nv = in->nv + 2 * in->ns;
out->ne = in->ne;
out->nb = in->nb - in->nsb + 2 * in->ns;
out->ng = in->ns > 0 ? in->ng + in->nsb : in->ng;
out->ns = 0;
out->nsb = 0;
out->nsg = 0;
}



void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out)
{
int nv = in->dim->nv;
int ne = in->dim->ne;
int nb = in->dim->nb;
int ng = in->dim->ng;
int ns = in->dim->ns;
int nsb = in->dim->nsb;
int nsg = in->dim->nsg;
int *idxs = in->idxs;
int *idxb = in->idxb;

int nv2 = out->dim->nv;
int ne2 = out->dim->ne;
int nb2 = out->dim->nb;
int ng2 = out->dim->ng;

assert(nv2 == nv+2*ns && "Dimensions are wrong!");
assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!");
assert(ng2 == ng+nsb && "Dimensions are wrong!");

// set matrices to 0.0
blasfeo_dgese(nv2, nv2, 0.0, out->Hv, 0, 0);
blasfeo_dgese(ne2, nv2, 0.0, out->A, 0, 0);
blasfeo_dgese(nv2, ng2, 0.0, out->Ct, 0, 0);

// copy in->Hv to upper left corner of out->Hv, out->Hv = [in->Hv 0; 0 0]
blasfeo_dgecp(nv, nv, in->Hv, 0, 0, out->Hv, 0, 0);

// copy in->Z to main diagonal of out->Hv, out->Hv = [in->Hv 0; 0 Z]
blasfeo_ddiain(2 * ns, 1.0, in->Z, 0, out->Hv, nv, nv);

// copy in->gz to out->gz
blasfeo_dveccp(nv + 2 * ns, in->gz, 0, out->gz, 0);

if (ne > 0)
{
// copy in->A to out->A
blasfeo_dgecp(ne, nv, in->A, 0, 0, out->A, 0, 0);

// copy in->b to out->b
blasfeo_dveccp(ne, in->b, 0, out->b, 0);
}

// copy in->Ct to out->Ct, out->Ct = [in->Ct 0; 0 0]
blasfeo_dgecp(nv, ng, in->Ct, 0, 0, out->Ct, 0, 0);

if (ns > 0)
{
// set flags for non-softened box constraints
// use out->m temporarily for this
for (int ii = 0; ii < nb; ii++)
{
// TODO(bnovoselnik): pick up some workspace for this
BLASFEO_DVECEL(out->m, ii) = 1.0;
}

int col_b = ng;
for (int ii = 0; ii < ns; ii++)
{
int js = idxs[ii];

int idx_v_ls0 = nv+ii;
int idx_v_us0 = nv+ns+ii;
int idx_v_ls1 = nv+ii;
int idx_v_us1 = nv+ns+ii;

int idx_d_ls0 = js;
int idx_d_us0 = nb+ng+js;
int idx_d_ls1;
int idx_d_us1;

if (js < nb) // soft box constraint
{
// index of a soft box constraint
int jv = idxb[js];

idx_d_ls1 = nb2+col_b;
idx_d_us1 = 2*nb2+ng2+col_b;

// softened box constraint, set its flag to -1
BLASFEO_DVECEL(out->m, js) = -1.0;

// insert softened box constraint into out->Ct, lb_i <= x_i + sl_i - su_i <= ub_i
BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0;
BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_b) = +1.0;
BLASFEO_DMATEL(out->Ct, idx_v_us1, col_b) = -1.0;
BLASFEO_DVECEL(out->d, idx_d_ls1) = BLASFEO_DVECEL(in->d, idx_d_ls0);
BLASFEO_DVECEL(out->d, idx_d_us1) = -BLASFEO_DVECEL(in->d, idx_d_us0);

col_b++;
}
else // soft general constraint
{
// index of a soft general constraint
int col_g = js - nb;

// soft general constraint, lg_i <= C_i x + sl_i - su_i <= ug_i
BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_g) = +1.0;
BLASFEO_DMATEL(out->Ct, idx_v_us1, col_g) = -1.0;
}

// slack variables have box constraints
out->idxb[nb-nsb+ii] = ii + nv;
out->idxb[nb-nsb+ns+ii] = ii + nv + ns;
}

int k_nsb = 0;
for (int ii = 0; ii < nb; ii++)
{
if (BLASFEO_DVECEL(out->m, ii) > 0)
{
// copy nonsoftened box constraint bounds to out->d
BLASFEO_DVECEL(out->d, k_nsb) = BLASFEO_DVECEL(in->d, ii);
BLASFEO_DVECEL(out->d, nb2+ng2+k_nsb) = -BLASFEO_DVECEL(in->d, nb+ng+ii);
out->idxb[k_nsb] = ii;
k_nsb++;
}
}

assert(k_nsb == nb-nsb && "Dimensions are wrong!");

// copy ls and us to out->lb, jump over nonsoftened box constraints
blasfeo_dveccp(2*ns, in->d, 2*nb+2*ng, out->d, k_nsb);

// for slack variables out->ub is +INFTY
blasfeo_dvecse(2*ns, 1.0e6, out->d, nb2+ng2+k_nsb);

// copy in->lg to out->lg
blasfeo_dveccp(ng, in->d, nb, out->d, nb2);

// copy in->ug to out->ug
blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2);

// flip signs for ub and ug
blasfeo_dvecsc(nb2+ng2, -1.0, out->d, nb2+ng2);

// set out->m to 0.0
blasfeo_dvecse(2*nb2+2*ng2, 0.0, out->m, 0);
}
else
{
blasfeo_dveccp(2*nb+2*ng, in->d, 0, out->d, 0);
blasfeo_dveccp(2*nb+2*ng, in->m, 0, out->m, 0);
for (int ii = 0; ii < nb; ii++) out->idxb[ii] = in->idxb[ii];
}
}



void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out)
{
int nv = qp_out->dim->nv;
int ne = qp_out->dim->ne;
int nb = qp_out->dim->nb;
int ng = qp_out->dim->ng;
int ns = qp_out->dim->ns;
int nsb = qp_out->dim->nsb;
int nsg = qp_out->dim->nsg;

int *idxs = qp_out->idxs;

int nv2 = in->dim->nv;
int ne2 = in->dim->ne;
int nb2 = in->dim->nb;
int ng2 = in->dim->ng;

assert(nv2 == nv+2*ns && "Dimensions are wrong!");
assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!");
assert(ng2 == 2*ng+2*nsb && "Dimensions are wrong!");

// inequality constraints multipliers and slacks
if (ns > 0)
{

// set flags for non-softened box constraints
// use out->v temporarily for this
// XXX assume that nb<=nv
assert(nv+2*ns >= nb);

for (int ii = 0; ii < nb; ii++)
{
// TODO(bnovoselnik): pick up some workspace for this
BLASFEO_DVECEL(out->v, ii) = 1.0;
}

int col_b = 2*ng;
for (int ii = 0; ii < ns; ii++)
{
int js = idxs[ii];

int idx_d_ls0 = js;
int idx_d_us0 = nb+ng+js;
int idx_d_ls1;
int idx_d_us1;

if (js < nb)
{
// softened box constraint, set its flag to -1
BLASFEO_DVECEL(out->v, js) = -1.0;

idx_d_ls1 = nb2+col_b;
idx_d_us1 = 2*nb2+ng2+col_b;

BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1);
BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1);

BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1);
BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1);

col_b++;
}
}

int k_nsb = 0; // number of non-softed bounds
for (int ii = 0; ii < nb; ii++)
{
int idx_d_ls0 = ii;
int idx_d_us0 = nb+ng+ii;
int idx_d_ls1;
int idx_d_us1;

if (BLASFEO_DVECEL(out->v, ii) > 0)
{
idx_d_ls1 = k_nsb;
idx_d_us1 = nb2+ng2+k_nsb;

BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1);
BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1);

BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1);
BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1);

k_nsb++;
}
}

assert(k_nsb == nb-nsb && "Dimensions are wrong!");

blasfeo_dveccp(2*ns, in->t, k_nsb, out->t, 2*nb+2*ng);
blasfeo_dveccp(ng, in->t, 2*nb2+ng2, out->t, 2*nb+ng);
blasfeo_dveccp(ng, in->t, nb2, out->t, nb);

blasfeo_dveccp(2*ns, in->lam, k_nsb, out->lam, 2*nb+2*ng);
blasfeo_dveccp(ng, in->lam, 2*nb2+ng2, out->lam, 2*nb+ng);
blasfeo_dveccp(ng, in->lam, nb2, out->lam, nb);

// variables
blasfeo_dveccp(nv+2*ns, in->v, 0, out->v, 0);

// equality constraints multipliers
blasfeo_dveccp(ne, in->pi, 0, out->pi, 0);

}

return;
}
8 changes: 8 additions & 0 deletions acados/dense_qp/dense_qp_common.h
View file
Expand Up @@ -61,6 +61,7 @@ typedef struct
double interface_time;
double total_time;
int num_iter;
int t_computed;
} dense_qp_info;

//
Expand Down Expand Up @@ -88,11 +89,18 @@ int dense_qp_res_workspace_calculate_size(dense_qp_dims *dims);
//
dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory);
//
void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out);
//
void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res,
dense_qp_res_ws *res_ws);
//
void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]);
//
void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out);
//
void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out);
//
void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out);

#ifdef __cplusplus
} /* extern "C" */
Expand Down
3 changes: 2 additions & 1 deletion acados/dense_qp/dense_qp_hpipm.c
View file
Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down

0 comments on commit a05f719

Please sign in to comment.