Skip to content

Commit

Permalink
merged from blessed master
Browse files Browse the repository at this point in the history
  • Loading branch information
@FreyJo
FreyJo committed Apr 27, 2018
2 parents ae2736d + 300ab4b commit 9e1630b
Show file tree
Hide file tree
Showing 30 changed files with 3,027 additions and 374 deletions.
230 changes: 118 additions & 112 deletions acados/dense_qp/dense_qp_qpoases.c
View file
Expand Up @@ -98,12 +98,12 @@ void dense_qp_qpoases_opts_initialize_default(void *config_, dense_qp_dims *dims
opts->max_cputime = 1000.0;
opts->warm_start = 0;
opts->max_nwsr = 1000;
opts->use_precomputed_cholesky = 0;
opts->hotstart = 0;
opts->use_precomputed_cholesky = 0;
opts->hotstart = 0;
opts->set_acado_opts = 1;
opts->compute_t = 1;
opts->compute_t = 1;

return;
return;
}


Expand All @@ -112,7 +112,7 @@ void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts
{
// dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *)opts_;

return;
return;
}


Expand All @@ -133,7 +133,7 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v
int size = sizeof(dense_qp_qpoases_memory);

size += 1 * nvd * nvd * sizeof(double); // H
size += 1 * nvd * nvd * sizeof(double); // R
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
Expand Down Expand Up @@ -203,8 +203,8 @@ void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *o

assert((char *)raw_memory + dense_qp_qpoases_memory_calculate_size(config_, dims, opts_) >= c_ptr);

// assign default values to fields stored in the memory
mem->first_it = 1; // only used if hotstart (only constant data matrices) is enabled
// assign default values to fields stored in the memory
mem->first_it = 1; // only used if hotstart (only constant data matrices) is enabled

return mem;
}
Expand All @@ -228,7 +228,7 @@ int dense_qp_qpoases_workspace_calculate_size(void *config_, dense_qp_dims *dims

int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_)
{
dense_qp_info *info = (dense_qp_info *) qp_out->misc;
dense_qp_info *info = (dense_qp_info *) qp_out->misc;
acados_timer tot_timer, qp_timer, interface_timer;

acados_tic(&tot_timer);
Expand Down Expand Up @@ -262,11 +262,11 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
int nbd = qp_in->dim->nb;
int nsd = qp_in->dim->ns;

if (nsd>0)
{
printf("\nqpOASES interface can not handle ns>0 yet: what about implementing it? :)\n");
return ACADOS_FAILURE;
}
if (nsd>0)
{
printf("\nqpOASES interface can not handle ns>0 yet: what about implementing it? :)\n");
return ACADOS_FAILURE;
}

// fill in the upper triangular of H in dense_qp
blasfeo_dtrtr_l(nvd, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0);
Expand Down Expand Up @@ -302,67 +302,73 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
int nwsr = opts->max_nwsr;
double cputime = opts->max_cputime;

int qpoases_status = 0;
if (opts->hotstart == 1) { // only to be used with fixed data matrices!
if (ngd > 0) { // QProblem
if (memory->first_it == 1) {
QProblemCON(QP, nvd, ngd, HST_POSDEF);
QProblem_setPrintLevel(QP, PL_MEDIUM);
// QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
QProblem_printProperties(QP);
// static Options options;

// Options_setToDefault( &options );
// options.initialStatusBounds = ST_INACTIVE;
// QProblem_setOptions( QP, options );
qpoases_status = QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime );
memory->first_it = 0;

QProblem_getPrimalSolution(QP, prim_sol);
QProblem_getDualSolution(QP, dual_sol);
} else {
qpoases_status = QProblem_hotstart(QP, g, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime);
}
} else {
if (memory->first_it == 1) {
QProblemBCON(QPB, nvd, HST_POSDEF);
QProblemB_setPrintLevel(QPB, PL_MEDIUM);
// QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
QProblemB_printProperties(QPB);
// static Options options;

// Options_setToDefault( &options );
// options.initialStatusBounds = ST_INACTIVE;
// QProblemB_setOptions( QPB, options );
QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
memory->first_it = 0;

QProblemB_getPrimalSolution(QPB, prim_sol);
QProblemB_getDualSolution(QPB, dual_sol);

} else {
QProblemB_hotstart(QPB, g, d_lb, d_ub, &nwsr, &cputime);
}
}
} else { // hotstart = 0
if (ngd > 0)
int qpoases_status = 0;
if (opts->hotstart == 1) { // only to be used with fixed data matrices!
if (ngd > 0) { // QProblem
if (memory->first_it == 1) {
QProblemCON(QP, nvd, ngd, HST_POSDEF);
QProblem_setPrintLevel(QP, PL_MEDIUM);
// QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
if (opts->set_acado_opts)
{
static Options options;
Options_setToMPC(&options);
QProblem_setOptions(QP, options);
}
qpoases_status = QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime );
memory->first_it = 0;

QProblem_getPrimalSolution(QP, prim_sol);
QProblem_getDualSolution(QP, dual_sol);
} else {
qpoases_status = QProblem_hotstart(QP, g, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime);

QProblem_getPrimalSolution(QP, prim_sol);
QProblem_getDualSolution(QP, dual_sol);
}
} else {
if (memory->first_it == 1) {
QProblemBCON(QPB, nvd, HST_POSDEF);
QProblemB_setPrintLevel(QPB, PL_MEDIUM);
// QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
if (opts->set_acado_opts)
{
static Options options;
Options_setToMPC(&options);
QProblem_setOptions(QP, options);
}
QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
memory->first_it = 0;

QProblemB_getPrimalSolution(QPB, prim_sol);
QProblemB_getDualSolution(QPB, dual_sol);

} else {
QProblemB_hotstart(QPB, g, d_lb, d_ub, &nwsr, &cputime);

QProblemB_getPrimalSolution(QPB, prim_sol);
QProblemB_getDualSolution(QPB, dual_sol);
}
}
} else { // hotstart = 0
if (ngd > 0)
{
QProblemCON(QP, nvd, ngd, HST_POSDEF);
QProblem_setPrintLevel(QP, PL_MEDIUM);
// QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
QProblemCON(QP, nvd, ngd, HST_POSDEF);
// QProblem_setPrintLevel(QP, PL_HIGH);
QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
QProblem_printProperties(QP);
if (opts->use_precomputed_cholesky == 1)
if (opts->use_precomputed_cholesky == 1)
{
// static Options options;
// Options_setToDefault( &options );
// 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,
/* primal_sol */ NULL, /* dual sol */ NULL,
/* guessed bounds */ NULL, /* guessed constraints */ NULL,
// static Options options;
// Options_setToDefault( &options );
// 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,
/* primal_sol */ NULL, /* dual sol */ NULL,
/* guessed bounds */ NULL, /* guessed constraints */ NULL,
/* R */ memory->R); // NOTE(dimitris): can pass either NULL or 0
} else
} else
{
if (opts->set_acado_opts)
{
Expand All @@ -376,28 +382,28 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
&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,
qpoases_status = QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg, d_ug,
&nwsr, &cputime);
}
}
QProblem_getPrimalSolution(QP, prim_sol);
QProblem_getDualSolution(QP, dual_sol);
} else
}
QProblem_getPrimalSolution(QP, prim_sol);
QProblem_getDualSolution(QP, dual_sol);
} else
{ // QProblemB
QProblemBCON(QPB, nvd, HST_POSDEF);
QProblemB_setPrintLevel(QPB, PL_MEDIUM);
// QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
QProblemBCON(QPB, nvd, HST_POSDEF);
// QProblemB_setPrintLevel(QPB, PL_MEDIUM);
QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
QProblemB_printProperties(QPB);
if (opts->use_precomputed_cholesky == 1)
if (opts->use_precomputed_cholesky == 1)
{
// static Options options;
// Options_setToDefault( &options );
// options.initialStatusBounds = ST_INACTIVE;
// QProblemB_setOptions( QPB, options );
qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
/* primal_sol */ NULL, /* dual sol */ NULL, /* guessed bounds */ NULL,
// static Options options;
// Options_setToDefault( &options );
// options.initialStatusBounds = ST_INACTIVE;
// QProblemB_setOptions( QPB, options );
qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
/* primal_sol */ NULL, /* dual sol */ NULL, /* guessed bounds */ NULL,
/* R */ memory->R);
} else
} else
{
if (opts->set_acado_opts)
{
Expand All @@ -407,18 +413,18 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
}
if (opts->warm_start)
{
qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
/* primal sol */ NULL, /* dual sol */ dual_sol, /* guessed bounds */ NULL,
/* R */ NULL);
} else
{
qpoases_status = QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
qpoases_status = QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
}
}
QProblemB_getPrimalSolution(QPB, prim_sol);
QProblemB_getDualSolution(QPB, dual_sol);
}
}
}
QProblemB_getPrimalSolution(QPB, prim_sol);
QProblemB_getDualSolution(QPB, dual_sol);
}
}
// save solution statistics to memory
memory->cputime = cputime;
memory->nwsr = nwsr;
Expand All @@ -445,14 +451,14 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
info->total_time = acados_toc(&tot_timer);
info->num_iter = nwsr;

// 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);
}
// 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);
}

int acados_status = qpoases_status;
if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS;
Expand All @@ -465,17 +471,17 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
void dense_qp_qpoases_config_initialize_default(void *config_)
{

qp_solver_config *config = config_;
qp_solver_config *config = config_;

config->opts_calculate_size = ( int (*) (void *, void *)) &dense_qp_qpoases_opts_calculate_size;
config->opts_assign = ( void* (*) (void *, void *, void *)) &dense_qp_qpoases_opts_assign;
config->opts_initialize_default = ( void (*) (void *, void *, void *)) &dense_qp_qpoases_opts_initialize_default;
config->opts_update = ( void (*) (void *, void *, void *)) &dense_qp_qpoases_opts_update;
config->memory_calculate_size = ( int (*) (void *, void *, void *)) &dense_qp_qpoases_memory_calculate_size;
config->memory_assign = ( void* (*) (void *, void *, void *, void *)) &dense_qp_qpoases_memory_assign;
config->workspace_calculate_size = ( int (*) (void *, void *, void *)) &dense_qp_qpoases_workspace_calculate_size;
config->evaluate = ( int (*) (void *, void *, void *, void *, void *, void *)) &dense_qp_qpoases;
config->opts_calculate_size = ( int (*) (void *, void *)) &dense_qp_qpoases_opts_calculate_size;
config->opts_assign = ( void* (*) (void *, void *, void *)) &dense_qp_qpoases_opts_assign;
config->opts_initialize_default = ( void (*) (void *, void *, void *)) &dense_qp_qpoases_opts_initialize_default;
config->opts_update = ( void (*) (void *, void *, void *)) &dense_qp_qpoases_opts_update;
config->memory_calculate_size = ( int (*) (void *, void *, void *)) &dense_qp_qpoases_memory_calculate_size;
config->memory_assign = ( void* (*) (void *, void *, void *, void *)) &dense_qp_qpoases_memory_assign;
config->workspace_calculate_size = ( int (*) (void *, void *, void *)) &dense_qp_qpoases_workspace_calculate_size;
config->evaluate = ( int (*) (void *, void *, void *, void *, void *, void *)) &dense_qp_qpoases;

return;
return;

}
1 change: 1 addition & 0 deletions acados/sim/sim_common.h
View file
Expand Up @@ -47,6 +47,7 @@ typedef enum {
IMPL_ODE_JAC_U,
IMPL_ODE_FUN_JAC_X_XDOT,
IMPL_ODE_JAC_X_XDOT_U,
IMPL_ODE_FUN_JAC_X_XDOT_U,
IMPL_ODE_JAC_X_U,
// gnsf
PHI_FUN,
Expand Down

0 comments on commit 9e1630b

Please sign in to comment.