Skip to content

Commit

Permalink
added sqp_rti to c interface
Browse files Browse the repository at this point in the history
  • Loading branch information
@giaf
giaf committed Jun 7, 2018
1 parent 6ea4442 commit 1c028e8
Show file tree
Hide file tree
Showing 4 changed files with 166 additions and 106 deletions.
2 changes: 1 addition & 1 deletion acados/ocp_nlp/ocp_nlp_sqp_rti.c
View file
Expand Up @@ -184,7 +184,7 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *o



void ocp_nlp_sqp_opts_rti_update(void *config_, void *dims_, void *opts_)
void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_)
{
ocp_nlp_dims *dims = dims_;
ocp_nlp_solver_config *config = config_;
Expand Down
124 changes: 89 additions & 35 deletions examples/c/wind_turbine_nmpc.c
View file
Expand Up @@ -38,6 +38,7 @@
#include "acados/ocp_qp/ocp_qp_partial_condensing_solver.h"

#include "acados/ocp_nlp/ocp_nlp_sqp.h"
#include "acados/ocp_nlp/ocp_nlp_sqp_rti.h"
#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
#include "acados/ocp_nlp/ocp_nlp_cost_ls.h"
#include "acados/ocp_nlp/ocp_nlp_cost_nls.h"
Expand All @@ -51,7 +52,7 @@
#include "examples/c/wt_model_nx6/setup.c"
#define NN 40

#define MAX_SQP_ITERS 15
#define MAX_SQP_ITERS 1
#define NREP 1


Expand Down Expand Up @@ -499,7 +500,8 @@ int main()

ocp_nlp_solver_plan *plan = ocp_nlp_plan_create(NN);

plan->nlp_solver = SQP_GN;
// plan->nlp_solver = SQP_GN;
plan->nlp_solver = SQP_RTI;

for (int i = 0; i <= NN; i++)
plan->nlp_cost[i] = LINEAR_LS;
Expand Down Expand Up @@ -745,53 +747,85 @@ int main()
* sqp opts
************************************************/

// create opts
void *nlp_opts = ocp_nlp_opts_create(config, dims);
ocp_nlp_sqp_opts *sqp_opts = (ocp_nlp_sqp_opts *) nlp_opts;

for (int i = 0; i < NN; ++i)
// extract opts
sim_rk_opts *sim_opts[NN+1];
ocp_qp_partial_condensing_solver_opts *pcond_solver_opts;

// nlp opts
if (plan->nlp_solver == SQP_GN)
{

ocp_nlp_sqp_opts *sqp_opts = nlp_opts;

sqp_opts->maxIter = MAX_SQP_ITERS;
sqp_opts->min_res_g = 1e-6;
sqp_opts->min_res_b = 1e-8;
sqp_opts->min_res_d = 1e-8;
sqp_opts->min_res_m = 1e-8;

for (int i = 0; i < NN; ++i)
{
ocp_nlp_dynamics_cont_opts *dynamics_stage_opts = sqp_opts->dynamics[i];
sim_opts[i] = dynamics_stage_opts->sim_solver;
}

pcond_solver_opts = sqp_opts->qp_solver_opts;

}
else if (plan->nlp_solver == SQP_RTI)
{

ocp_nlp_sqp_rti_opts *sqp_rti_opts = nlp_opts;

for (int i = 0; i < NN; ++i)
{
ocp_nlp_dynamics_cont_opts *dynamics_stage_opts = sqp_rti_opts->dynamics[i];
sim_opts[i] = dynamics_stage_opts->sim_solver;
}

pcond_solver_opts = sqp_rti_opts->qp_solver_opts;

}

// sim opts
for (int i = 0; i < NN; ++i)
{
ocp_nlp_dynamics_cont_opts *dynamics_stage_opts = sqp_opts->dynamics[i];
sim_rk_opts *sim_opts = dynamics_stage_opts->sim_solver;

if (plan->sim_solver_plan[i].sim_solver == ERK)
{
sim_opts->ns = 4;
sim_opts->num_steps = 10;
sim_opts[i]->ns = 4;
sim_opts[i]->num_steps = 10;
}
else if (plan->sim_solver_plan[i].sim_solver == IRK)
{
sim_opts->ns = 4;
sim_opts->num_steps = 1;
sim_opts->jac_reuse = true;
sim_opts[i]->ns = 4;
sim_opts[i]->num_steps = 1;
sim_opts[i]->jac_reuse = true;
}
else if (plan->sim_solver_plan[i].sim_solver == NEW_LIFTED_IRK)
{
sim_opts->ns = 4;
sim_opts->num_steps = 1;
sim_opts[i]->ns = 4;
sim_opts[i]->num_steps = 1;
}
else if (plan->sim_solver_plan[i].sim_solver == GNSF)
{
sim_opts->ns = 4;
sim_opts->num_steps = 1;
sim_opts->newton_iter = 1;
sim_opts->jac_reuse = true;
sim_opts[i]->ns = 4;
sim_opts[i]->num_steps = 1;
sim_opts[i]->newton_iter = 1;
sim_opts[i]->jac_reuse = true;
}
}

sqp_opts->maxIter = MAX_SQP_ITERS;
sqp_opts->min_res_g = 1e-6;
sqp_opts->min_res_b = 1e-8;
sqp_opts->min_res_d = 1e-8;
sqp_opts->min_res_m = 1e-8;
}

// partial condensing
// partial condensing opts
if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM)
{
ocp_nlp_sqp_opts *sqp_opts = nlp_opts;
ocp_qp_partial_condensing_solver_opts *pcond_solver_opts = sqp_opts->qp_solver_opts;
pcond_solver_opts->pcond_opts->N2 = 10;
}

// update opts after manual changes
config->opts_update(config, dims, nlp_opts);

/************************************************
Expand All @@ -817,8 +851,8 @@ int main()
sim_gnsf_dims *gnsf_dims = (sim_gnsf_dims *) dyn_dims->sim;

// get sim opts
ocp_nlp_dynamics_cont_opts *dynamics_stage_opts = sqp_opts->dynamics[i];
sim_rk_opts *sim_opts = dynamics_stage_opts->sim_solver;
// ocp_nlp_dynamics_cont_opts *dynamics_stage_opts = sqp_opts->dynamics[i];
// sim_rk_opts *sim_opts = dynamics_stage_opts->sim_solver;

// import model matrices
sim_gnsf_import_matrices(gnsf_dims, model, get_model_matrices);
Expand All @@ -827,15 +861,28 @@ int main()
sim_solver_config *sim_sol_config = (sim_solver_config *) config->dynamics[i]->sim_solver;

// get sim_solver memory
ocp_nlp_sqp_memory *mem = solver->mem;
ocp_nlp_dynamics_cont_memory* dynamics_mem = (ocp_nlp_dynamics_cont_memory *) mem->dynamics[i];
char *mem_ptr = (char *) dynamics_mem;
mem_ptr += sizeof(ocp_nlp_dynamics_cont_memory); // mem_ptr now points to the memory of the integrator;
ocp_nlp_dynamics_cont_memory *dynamics_mem;

if (plan->nlp_solver == SQP_GN)
{
ocp_nlp_sqp_memory *sqp_mem = solver->mem;
dynamics_mem = sqp_mem->dynamics[i];
}
else if (plan->nlp_solver == SQP_RTI)
{
ocp_nlp_sqp_memory *sqp_rti_mem = solver->mem;
dynamics_mem = sqp_rti_mem->dynamics[i];
}

// XXX why this all ? dangerous !!!!!!!!!!!!!!!!!!!!!!!!!!!!
// ocp_nlp_sqp_memory *mem = solver->mem;
// ocp_nlp_dynamics_cont_memory* dynamics_mem = (ocp_nlp_dynamics_cont_memory *) mem->dynamics[i];
// char *mem_ptr = (char *) dynamics_mem;
// mem_ptr += sizeof(ocp_nlp_dynamics_cont_memory); // mem_ptr now points to the memory of the integrator;

// precompute
sim_gnsf_precompute(sim_sol_config, gnsf_dims, model, sim_opts, mem_ptr, solver->work, nlp_in->Ts[i]);
// sim_gnsf_precompute(sim_sol_config, gnsf_dims, model, sim_opts[i], mem_ptr, solver->work, nlp_in->Ts[i]);
sim_gnsf_precompute(sim_sol_config, gnsf_dims, model, sim_opts[i], dynamics_mem->sim_solver, solver->work, nlp_in->Ts[i]);
// NOTE; solver->work can be used, as it is for sure larger than the workspace
// needed to precompute, as the latter is part of the first.
}
Expand Down Expand Up @@ -919,7 +966,14 @@ int main()
// print info
if (true)
{
printf("\nproblem #%d, status %d, iters %d\n", idx, status, ((ocp_nlp_sqp_memory *)solver->mem)->sqp_iter);
if (plan->nlp_solver == SQP_GN)
{
printf("\nproblem #%d, status %d, iters %d\n", idx, status, ((ocp_nlp_sqp_memory *)solver->mem)->sqp_iter);
}
else if (plan->nlp_solver == SQP_RTI)
{
printf("\nproblem #%d, status %d\n", idx, status);
}
printf("xsim = \n");
blasfeo_print_tran_dvec(dims->nx[0], &nlp_out->ux[0], dims->nu[0]);
printf("electrical power = %f\n", 0.944*97/100*BLASFEO_DVECEL(&nlp_out->ux[0], 2)*BLASFEO_DVECEL(&nlp_out->ux[0], 7));
Expand Down
143 changes: 74 additions & 69 deletions interfaces/acados_c/ocp_nlp_interface.c
View file
Expand Up @@ -34,6 +34,7 @@
#include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h"
#include "acados/ocp_nlp/ocp_nlp_constraints_bghp.h"
#include "acados/ocp_nlp/ocp_nlp_sqp.h"
#include "acados/ocp_nlp/ocp_nlp_sqp_rti.h"
#include "acados/utils/mem.h"


Expand Down Expand Up @@ -126,80 +127,84 @@ ocp_nlp_solver_config *ocp_nlp_config_create(ocp_nlp_solver_plan plan, int N)
if (plan.nlp_solver == SQP_GN)
{
ocp_nlp_sqp_config_initialize_default(config);

// QP solver
config->qp_solver = ocp_qp_config_create(plan.ocp_qp_solver_plan);

// cost
for (int i = 0; i <= N; ++i)
{
switch (plan.nlp_cost[i])
{
case LINEAR_LS:
ocp_nlp_cost_ls_config_initialize_default(config->cost[i]);
break;
case NONLINEAR_LS:
ocp_nlp_cost_nls_config_initialize_default(config->cost[i]);
break;
case EXTERNALLY_PROVIDED:
ocp_nlp_cost_external_config_initialize_default(config->cost[i]);
break;
case 100:
printf("\nForgot to plan cost?\n\n");
exit(1);
default:
printf("Cost not available!\n");
exit(1);
}
}

// Dynamics
for (int i = 0; i < N; ++i)
{
switch (plan.nlp_dynamics[i])
{
case CONTINUOUS_MODEL:
ocp_nlp_dynamics_cont_config_initialize_default(config->dynamics[i]);
config->dynamics[i]->sim_solver = sim_config_create(plan.sim_solver_plan[i]);
break;
case DISCRETE_MODEL:
ocp_nlp_dynamics_disc_config_initialize_default(config->dynamics[i]);
break;
case 100:
printf("\nForgot to plan dynamics?\n\n");
exit(1);
default:
printf("Dynamics not available!\n");
exit(1);
}
}

// Constraints
for (int i = 0; i <= N; ++i)
{
switch (plan.nlp_constraints[i])
{
case BGH:
ocp_nlp_constraints_bgh_config_initialize_default(config->constraints[i]);
break;
case BGHP:
ocp_nlp_constraints_bghp_config_initialize_default(config->constraints[i]);
break;
case 100:
printf("\nForgot to plan constraints?\n\n");
exit(1);
default:
printf("\nConstraint not available!\n\n");
exit(1);
}
}

}
}
else if (plan.nlp_solver == SQP_RTI)
{
ocp_nlp_sqp_rti_config_initialize_default(config);
}
else
{
printf("Solver not available!\n");
exit(1);
}

// QP solver
config->qp_solver = ocp_qp_config_create(plan.ocp_qp_solver_plan);

// cost
for (int i = 0; i <= N; ++i)
{
switch (plan.nlp_cost[i])
{
case LINEAR_LS:
ocp_nlp_cost_ls_config_initialize_default(config->cost[i]);
break;
case NONLINEAR_LS:
ocp_nlp_cost_nls_config_initialize_default(config->cost[i]);
break;
case EXTERNALLY_PROVIDED:
ocp_nlp_cost_external_config_initialize_default(config->cost[i]);
break;
case 100:
printf("\nForgot to plan cost?\n\n");
exit(1);
default:
printf("Cost not available!\n");
exit(1);
}
}

// Dynamics
for (int i = 0; i < N; ++i)
{
switch (plan.nlp_dynamics[i])
{
case CONTINUOUS_MODEL:
ocp_nlp_dynamics_cont_config_initialize_default(config->dynamics[i]);
config->dynamics[i]->sim_solver = sim_config_create(plan.sim_solver_plan[i]);
break;
case DISCRETE_MODEL:
ocp_nlp_dynamics_disc_config_initialize_default(config->dynamics[i]);
break;
case 100:
printf("\nForgot to plan dynamics?\n\n");
exit(1);
default:
printf("Dynamics not available!\n");
exit(1);
}
}

// Constraints
for (int i = 0; i <= N; ++i)
{
switch (plan.nlp_constraints[i])
{
case BGH:
ocp_nlp_constraints_bgh_config_initialize_default(config->constraints[i]);
break;
case BGHP:
ocp_nlp_constraints_bghp_config_initialize_default(config->constraints[i]);
break;
case 100:
printf("\nForgot to plan constraints?\n\n");
exit(1);
default:
printf("\nConstraint not available!\n\n");
exit(1);
}
}

return config;
}

Expand Down
3 changes: 2 additions & 1 deletion interfaces/acados_c/ocp_nlp_interface.h
View file
Expand Up @@ -33,7 +33,8 @@ extern "C" {

typedef enum
{
SQP_GN,
SQP_GN, // why _GN ??? exact hessian would use the same scheme
SQP_RTI,
} ocp_nlp_solver_t;


Expand Down

0 comments on commit 1c028e8

Please sign in to comment.