Permalink
Browse files

perform timings

  • Loading branch information...
roversch committed May 28, 2018
1 parent 1515b5a commit 09e77d3c955d1e2a84de0afe6d7b866f34ddb9e6
@@ -89,7 +89,7 @@ void dense_qp_qpoases_opts_initialize_default(void *config_, dense_qp_dims *dims
dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *) opts_;
opts->max_cputime = 1000.0;
opts->warm_start = 0;
opts->warm_start = 1;
opts->max_nwsr = 1000;
opts->use_precomputed_cholesky = 0;
opts->hotstart = 0;
@@ -299,8 +299,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
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;
@@ -327,8 +325,6 @@ 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);
QProblemB_setPrintLevel(QPB, PL_MEDIUM);
// QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
if (opts->set_acado_opts)
{
static Options options;
@@ -355,9 +351,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
if (ngd > 0)
{
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)
{
// static Options options;
@@ -396,9 +389,6 @@ int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, vo
else
{ // QProblemB
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)
{
// static Options options;
@@ -434,6 +424,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;
@@ -86,6 +86,7 @@ typedef struct
struct blasfeo_dvec *t;
int sqp_iter;
int qp_iter;
double inf_norm_res;
double total_time;
} ocp_nlp_out;
@@ -878,6 +878,8 @@ int ocp_nlp_sqp(void *config_, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in, ocp_nlp_o
qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts,
mem->qp_solver_mem, work->qp_work);
nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter;
// printf("\n------- qp_out (sqp iter %d) ---------\n", sqp_iter);
// print_ocp_qp_out(work->qp_out);
// if(sqp_iter==1)
@@ -19,6 +19,7 @@
#include <assert.h>
#include <stdlib.h>
#include <string.h>
#include "acados/utils/external_function_generic.h"
#include "acados/utils/mem.h"
@@ -66,16 +67,15 @@ static void d_cvt_casadi_to_colmaj(double *in, int *sparsity_in, double *out)
if (dense)
{
for (ii = 0; ii < ncol * nrow; ii++) out[ii] = in[ii];
memcpy(out, in, nrow*ncol*sizeof(double));
}
else
{
double *ptr = in;
int *idxcol = sparsity_in + 2;
int *row = sparsity_in + ncol + 3;
// Fill with zeros
for (jj = 0; jj < ncol; jj++)
for (ii = 0; ii < nrow; ii++) out[ii + jj * nrow] = 0.0;
memset(out, 0, ncol*nrow*sizeof(double));
// Copy nonzeros
for (jj = 0; jj < ncol; jj++)
{
@@ -100,7 +100,7 @@ static void d_cvt_colmaj_to_casadi(double *in, double *out, int *sparsity_out)
if (dense)
{
for (ii = 0; ii < ncol * nrow; ii++) out[ii] = in[ii];
memcpy(out, in, ncol*nrow*sizeof(double));
}
else
{
@@ -40,22 +40,49 @@
#include "acados/ocp_qp/ocp_qp_qpdunes.h"
#endif
#include "acados/ocp_qp/ocp_qp_partial_condensing_solver.h"
#include "acados/ocp_nlp/ocp_nlp_sqp.h"
#include "acados/utils/strsep.h"
static bool is_qp_solver(const char *qp_solver_name)
{
return !strcmp(qp_solver_name, "hpipm")
|| !strcmp(qp_solver_name, "sparse_hpipm")
|| !strcmp(qp_solver_name, "condensing_hpipm")
|| !strcmp(qp_solver_name, "hpmpc")
|| !strcmp(qp_solver_name, "ooqp")
|| !strcmp(qp_solver_name, "qpdunes")
|| !strcmp(qp_solver_name, "qpoases")
|| !strcmp(qp_solver_name, "qore");
}
int get_option_int(const void *args_, const char *option) { return 0; }
bool set_option_int(void *args_, const char *option, const int value)
{
char *option_cpy, *token;
option_cpy = (char *) malloc(sizeof(char) * MAX_STR_LEN);
char *token;
char option_cpy[MAX_STR_LEN];
strcpy(option_cpy, option);
token = strsep_acados(&option_cpy, ".");
char *ptr_to_option_cpy = &option_cpy[0];
token = strsep_acados(&ptr_to_option_cpy, ".");
while (token)
{
// Linear search since the number of options is small.
if (!strcmp(token, "sparse_hpipm"))
if (!strcmp(token, "sqp"))
{
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_nlp_sqp_opts *args = (ocp_nlp_sqp_opts *) args_;
if (!strcmp(token, "max_iter")) {
args->maxIter = value;
}
else if (is_qp_solver(token))
{
token[strlen(token)] = '.'; // this effectively concatenates the strings again
return set_option_int(args->qp_solver_opts, token, value);
}
}
else if (!strcmp(token, "sparse_hpipm") || !strcmp(token, "hpipm"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_hpipm_opts *args = (ocp_qp_hpipm_opts *) sparse_args->qp_solver_opts;
@@ -72,7 +99,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "condensing_hpipm"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
dense_qp_hpipm_opts *args = (dense_qp_hpipm_opts *) sparse_args->qp_solver_opts;
@@ -86,7 +113,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "hpmpc"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_hpmpc_opts *args = (ocp_qp_hpmpc_opts *) sparse_args->qp_solver_opts;
@@ -112,7 +139,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "ooqp"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_ooqp_args *args = (ocp_qp_ooqp_args *) sparse_args->qp_solver_opts;
@@ -125,7 +152,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "qpdunes"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_qpdunes_opts *args = (ocp_qp_qpdunes_opts *) sparse_args->qp_solver_opts;
@@ -169,7 +196,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "qore"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_full_condensing_solver_opts *cond_opts =
(ocp_qp_full_condensing_solver_opts *) args_;
dense_qp_qore_opts *args = (dense_qp_qore_opts *) cond_opts->qp_solver_opts;
@@ -192,7 +219,7 @@ bool set_option_int(void *args_, const char *option, const int value)
}
else if (!strcmp(token, "qpoases"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_full_condensing_solver_opts *cond_opts =
(ocp_qp_full_condensing_solver_opts *) args_;
dense_qp_qpoases_opts *args = (dense_qp_qpoases_opts *) cond_opts->qp_solver_opts;
@@ -208,7 +235,7 @@ bool set_option_int(void *args_, const char *option, const int value)
{
return false;
}
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
}
return true;
}
@@ -226,15 +253,16 @@ double get_option_double(const void *args_, const char *option) { return 0; }
bool set_option_double(void *args_, const char *option, const double value)
{
char *option_cpy, *token;
option_cpy = (char *) malloc(sizeof(char) * MAX_STR_LEN);
char *token;
char option_cpy[MAX_STR_LEN];
strcpy(option_cpy, option);
while ((token = strsep_acados(&option_cpy, ".")))
char *ptr_to_option_cpy = &option_cpy[0];
while ((token = strsep_acados(&ptr_to_option_cpy, ".")))
{
// Linear search since the number of options is small.
if (!strcmp(token, "sparse_hpipm"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_hpipm_opts *args = (ocp_qp_hpipm_opts *) sparse_args->qp_solver_opts;
@@ -255,7 +283,7 @@ bool set_option_double(void *args_, const char *option, const double value)
}
else if (!strcmp(token, "condensing_hpipm"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
dense_qp_hpipm_opts *args = (dense_qp_hpipm_opts *) sparse_args->qp_solver_opts;
@@ -277,7 +305,7 @@ bool set_option_double(void *args_, const char *option, const double value)
}
else if (!strcmp(token, "hpmpc"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_hpmpc_opts *args = (ocp_qp_hpmpc_opts *) sparse_args->qp_solver_opts;
@@ -295,7 +323,7 @@ bool set_option_double(void *args_, const char *option, const double value)
}
else if (!strcmp(token, "qpdunes"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_partial_condensing_solver_opts *sparse_args =
(ocp_qp_partial_condensing_solver_opts *) args_;
ocp_qp_qpdunes_opts *args = (ocp_qp_qpdunes_opts *) sparse_args->qp_solver_opts;
@@ -308,7 +336,7 @@ bool set_option_double(void *args_, const char *option, const double value)
}
else if (!strcmp(token, "qpoases"))
{
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
ocp_qp_full_condensing_solver_opts *cond_opts =
(ocp_qp_full_condensing_solver_opts *) args_;
dense_qp_qpoases_opts *args = (dense_qp_qpoases_opts *) cond_opts->qp_solver_opts;
@@ -322,7 +350,7 @@ bool set_option_double(void *args_, const char *option, const double value)
{
return false;
}
token = strsep_acados(&option_cpy, ".");
token = strsep_acados(&ptr_to_option_cpy, ".");
}
return true;
}
@@ -60,7 +60,7 @@ casadi_module generate_ode_jacobian(const casadi::Function& model, string output
casadi::SX Sx = casadi::SX::sym("Sx", nx, nx);
casadi::SX Su = casadi::SX::sym("Su", nx, nu);
casadi::SX jac_x = casadi::SX::zeros(nx, nx) + casadi::SX::jacobian(rhs, x);
casadi::SX jac_x = casadi::SX::jacobian(rhs, x);
casadi::Function jac_fun(model.name() + "_expl_ode_jac", {x, Sx, Su, u}, {rhs, jac_x});
return casadi_module(jac_fun, output_folder);
@@ -154,7 +154,7 @@ void ocp_nlp::initialize_solver(std::string solver_name, std::map<std::string, o
else
qp_solver_name = "hpipm";
if (qp_solver_name == "hpipm")
if (qp_solver_name == "sparse_hpipm" || qp_solver_name == "hpipm")
{
ocp_qp_xcond_solver_config_initialize_default(PARTIAL_CONDENSING_HPIPM, config_->qp_solver);
plan_->ocp_qp_solver_plan.qp_solver = PARTIAL_CONDENSING_HPIPM;
@@ -164,6 +164,11 @@ void ocp_nlp::initialize_solver(std::string solver_name, std::map<std::string, o
ocp_qp_xcond_solver_config_initialize_default(FULL_CONDENSING_QPOASES, config_->qp_solver);
plan_->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES;
}
else if (qp_solver_name == "dense_hpipm")
{
ocp_qp_xcond_solver_config_initialize_default(FULL_CONDENSING_HPIPM, config_->qp_solver);
plan_->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM;
}
else
{
throw std::invalid_argument("QP solver name '" + qp_solver_name + "' not known.");
@@ -179,16 +184,17 @@ void ocp_nlp::initialize_solver(std::string solver_name, std::map<std::string, o
solver_options_.reset(ocp_nlp_opts_create(config_.get(), dims_.get()));
process_options(solver_name, options, solver_options_.get());
result_.reset(ocp_nlp_out_create(config_.get(), dims_.get()));
solver_.reset(ocp_nlp_create(config_.get(), dims_.get(), solver_options_.get()));
}
ocp_nlp_solution ocp_nlp::solve(vector<double> x_guess, vector<double> u_guess)
{
fill_bounds(cached_bounds);
solver_.reset(ocp_nlp_create(config_.get(), dims_.get(), solver_options_.get()));
if (!x_guess.empty() || !u_guess.empty())
{
for (int i = 0; i <= N; ++i)
@@ -104,7 +104,8 @@ ocp_nlp_info ocp_nlp_solution::info() {
nlp_out_->sqp_iter,
status_,
nlp_out_->inf_norm_res,
nlp_out_->total_time
nlp_out_->total_time,
nlp_out_->qp_iter
};
}
@@ -15,6 +15,7 @@ struct ocp_nlp_info
int status;
double inf_norm_residual;
double total_time;
int num_qp_iter;
};
class ocp_nlp_solution
Oops, something went wrong.

0 comments on commit 09e77d3

Please sign in to comment.