Skip to content
Permalink
Browse files

Merge branch 'master' into unify_build_systems

  • Loading branch information...
giaf committed Jun 11, 2019
2 parents ddfd52e + 49b5250 commit 4cd22d0223872f6326a2746539c9af5a04d312ea
Showing with 4,275 additions and 890 deletions.
  1. +1 −0 Makefile
  2. +2 −2 acados/dense_qp/dense_qp_common.h
  3. +83 −1 acados/dense_qp/dense_qp_hpipm.c
  4. +6 −0 acados/dense_qp/dense_qp_hpipm.h
  5. +78 −0 acados/dense_qp/dense_qp_ooqp.c
  6. +54 −0 acados/dense_qp/dense_qp_qore.c
  7. +50 −0 acados/dense_qp/dense_qp_qpoases.c
  8. +1 −0 acados/ocp_nlp/Makefile
  9. +36 −10 acados/ocp_nlp/ocp_nlp_common.c
  10. +28 −1 acados/ocp_nlp/ocp_nlp_constraints_bgh.c
  11. +17 −1 acados/ocp_nlp/ocp_nlp_constraints_bghp.c
  12. +1 −1 acados/ocp_nlp/ocp_nlp_constraints_common.h
  13. +1 −0 acados/ocp_nlp/ocp_nlp_cost_common.h
  14. +9 −0 acados/ocp_nlp/ocp_nlp_cost_external.c
  15. +2 −0 acados/ocp_nlp/ocp_nlp_cost_external.h
  16. +22 −0 acados/ocp_nlp/ocp_nlp_cost_ls.c
  17. +2 −0 acados/ocp_nlp/ocp_nlp_cost_ls.h
  18. +25 −0 acados/ocp_nlp/ocp_nlp_cost_nls.c
  19. +2 −0 acados/ocp_nlp/ocp_nlp_cost_nls.h
  20. +40 −1 acados/ocp_nlp/ocp_nlp_reg_common.c
  21. +6 −0 acados/ocp_nlp/ocp_nlp_reg_common.h
  22. +212 −19 acados/ocp_nlp/ocp_nlp_reg_convexify.c
  23. +7 −0 acados/ocp_nlp/ocp_nlp_reg_convexify.h
  24. +24 −0 acados/ocp_nlp/ocp_nlp_reg_mirror.c
  25. +24 −0 acados/ocp_nlp/ocp_nlp_reg_noreg.c
  26. +24 −0 acados/ocp_nlp/ocp_nlp_reg_project.c
  27. +547 −0 acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c
  28. +120 −0 acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h
  29. +227 −69 acados/ocp_nlp/ocp_nlp_sqp.c
  30. +20 −6 acados/ocp_nlp/ocp_nlp_sqp.h
  31. +163 −29 acados/ocp_nlp/ocp_nlp_sqp_rti.c
  32. +9 −0 acados/ocp_nlp/ocp_nlp_sqp_rti.h
  33. +38 −0 acados/ocp_qp/ocp_qp_common.c
  34. +8 −7 acados/ocp_qp/ocp_qp_common.h
  35. +76 −6 acados/ocp_qp/ocp_qp_full_condensing.c
  36. +11 −2 acados/ocp_qp/ocp_qp_full_condensing.h
  37. +54 −20 acados/ocp_qp/ocp_qp_full_condensing_solver.c
  38. +9 −2 acados/ocp_qp/ocp_qp_full_condensing_solver.h
  39. +96 −4 acados/ocp_qp/ocp_qp_hpipm.c
  40. +13 −0 acados/ocp_qp/ocp_qp_hpipm.h
  41. +47 −0 acados/ocp_qp/ocp_qp_hpmpc.c
  42. +108 −0 acados/ocp_qp/ocp_qp_ooqp.c
  43. +37 −0 acados/ocp_qp/ocp_qp_osqp.c
  44. +63 −1 acados/ocp_qp/ocp_qp_partial_condensing.c
  45. +11 −0 acados/ocp_qp/ocp_qp_partial_condensing.h
  46. +59 −28 acados/ocp_qp/ocp_qp_partial_condensing_solver.c
  47. +1 −2 acados/ocp_qp/ocp_qp_partial_condensing_solver.h
  48. +79 −0 acados/ocp_qp/ocp_qp_qpdunes.c
  49. +7 −0 docs/installation/index.md
  50. +5 −8 docs/interfaces/index.md
  51. +8 −8 examples/c/mass_spring_nmpc_example.c
  52. +2 −5 examples/c/mass_spring_offline_fcond_qpoases_split.c
  53. +4 −4 examples/c/no_interface_examples/nonlinear_chain_ocp_nlp_no_interface.c
  54. +11 −11 examples/c/nonlinear_chain_ocp_nlp.c
  55. +10 −10 examples/c/pendulum_scqp.cpp
  56. +4 −4 examples/c/simple_dae_example.c
  57. +13 −13 examples/c/wind_turbine_nmpc.c
  58. +3 −0 examples/matlab_mex/linear_mass_spring_model/env.sh
  59. +13 −2 examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m
  60. +47 −3 examples/matlab_mex/linear_mass_spring_model/example_ocp.m
  61. +10 −0 examples/matlab_mex/linear_mass_spring_model/example_sim.m
  62. +3 −0 examples/matlab_mex/masses_chain_model/env.sh
  63. +45 −13 examples/matlab_mex/masses_chain_model/example_closed_loop.m
  64. +85 −14 examples/matlab_mex/masses_chain_model/example_ocp.m
  65. +3 −0 examples/matlab_mex/pendulum_on_cart_model/env.sh
  66. +372 −0 examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m
  67. +95 −16 examples/matlab_mex/pendulum_on_cart_model/example_ocp.m
  68. +10 −0 examples/matlab_mex/pendulum_on_cart_model/example_sim.m
  69. +3 −0 examples/matlab_mex/wind_turbine_nx6/env.sh
  70. +77 −8 examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m
  71. +108 −4 examples/matlab_mex/wind_turbine_nx6/example_ocp.m
  72. +10 −0 examples/matlab_mex/wind_turbine_nx6/example_sim.m
  73. +1 −1 external/blasfeo
  74. +1 −1 external/hpipm
  75. +41 −5 interfaces/acados_c/ocp_nlp_interface.c
  76. +3 −3 interfaces/acados_c/ocp_nlp_interface.h
  77. +5 −5 interfaces/acados_c/options_interface.c
  78. +1 −1 interfaces/acados_matlab/acados_ocp.m
  79. +20 −238 interfaces/acados_matlab/acados_ocp_model.m
  80. +30 −49 interfaces/acados_matlab/acados_ocp_opts.m
  81. +2 −39 interfaces/acados_matlab/acados_sim_model.m
  82. +8 −32 interfaces/acados_matlab/acados_sim_opts.m
  83. +1 −1 interfaces/acados_matlab/generate_c_code_nonlinear_constr.m
  84. +14 −3 interfaces/acados_matlab/ocp_compile_mex.m
  85. +108 −7 interfaces/acados_matlab/ocp_create.c
  86. +51 −1 interfaces/acados_matlab/ocp_get.c
  87. +8 −0 interfaces/acados_matlab/ocp_set.c
  88. +174 −46 interfaces/acados_template/+acados_template/generate_solver_matlab.py
  89. +1 −1 interfaces/acados_template/acados_template/__init__.py
  90. +0 −5 interfaces/acados_template/acados_template/acados_layout.json
  91. +61 −34 interfaces/acados_template/acados_template/{ocp_nlp_render_arguments.py → acados_ocp_nlp.py}
  92. +1 −1 interfaces/acados_template/acados_template/c_templates/Makefile.in
  93. +11 −11 interfaces/acados_template/acados_template/c_templates/acados_solver.in.c
  94. +11 −9 interfaces/acados_template/acados_template/c_templates/acados_solver_sfun.in.c
  95. +8 −8 interfaces/acados_template/acados_template/c_templates_inja/acados_solver.in.c
  96. +1 −1 interfaces/acados_template/acados_template/c_templates_tera/Makefile.in
  97. +12 −12 interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c
  98. +26 −8 interfaces/acados_template/acados_template/generate_solver.py
  99. +5 −6 interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py
  100. +142 −0 interfaces/acados_template/examples/python/pendulum_example/generate_c_code_explicit_setters.py
  101. +4 −4 interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py
  102. +8 −8 test/ocp_nlp/test_chain.cpp
  103. +10 −10 test/ocp_nlp/test_wind_turbine.cpp
  104. +5 −5 test/ocp_qp/test_qpsolvers.cpp
@@ -32,6 +32,7 @@ OBJS += acados/ocp_nlp/ocp_nlp_reg_common.o
OBJS += acados/ocp_nlp/ocp_nlp_reg_convexify.o
OBJS += acados/ocp_nlp/ocp_nlp_reg_mirror.o
OBJS += acados/ocp_nlp/ocp_nlp_reg_project.o
OBJS += acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.o
OBJS += acados/ocp_nlp/ocp_nlp_reg_noreg.o

# dense qp
@@ -49,6 +49,7 @@ typedef struct
void *(*opts_assign)(void *config, void *dims, void *raw_memory);
void (*opts_initialize_default)(void *config, void *dims, void *args);
void (*opts_update)(void *config, void *dims, void *args);
void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
int (*memory_calculate_size)(void *config, void *dims, void *args);
void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory);
int (*workspace_calculate_size)(void *config, void *dims, void *args);
@@ -100,8 +101,7 @@ dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_me
//
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(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]);
//
@@ -18,7 +18,9 @@
*/

// external
#include <stdlib.h>
#include <assert.h>
#include <string.h>
// hpipm
#include "hpipm/include/hpipm_d_dense_qp.h"
#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
@@ -29,6 +31,8 @@
#include "acados/utils/mem.h"
#include "acados/utils/timing.h"



/************************************************
* opts
************************************************/
@@ -45,6 +49,8 @@ int dense_qp_hpipm_opts_calculate_size(void *config_, void *dims_)
return size;
}



void *dense_qp_hpipm_opts_assign(void *config_, void *dims_, void *raw_memory)
{
dense_qp_dims *dims = dims_;
@@ -68,6 +74,8 @@ void *dense_qp_hpipm_opts_assign(void *config_, void *dims_, void *raw_memory)
return (void *) opts;
}



void dense_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts_)
{
dense_qp_hpipm_opts *opts = opts_;
@@ -86,13 +94,67 @@ void dense_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *op
return;
}



void dense_qp_hpipm_opts_update(void *config_, void *dims_, void *opts_)
{
// dense_qp_hpipm_opts *opts = (dense_qp_hpipm_opts *)opts_;

return;
}



void dense_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value)
{
dense_qp_hpipm_opts *opts = opts_;

if (!strcmp(field, "iter_max"))
{
int* tmp_ptr = (int *) value;
d_set_dense_qp_ipm_arg_iter_max(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "mu0"))
{
double* tmp_ptr = (double *) value;
d_set_dense_qp_ipm_arg_mu0(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "tol_stat"))
{
double* tmp_ptr = (double *) value;
d_set_dense_qp_ipm_arg_tol_stat(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "tol_eq"))
{
double* tmp_ptr = (double *) value;
d_set_dense_qp_ipm_arg_tol_eq(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "tol_ineq"))
{
double* tmp_ptr = (double *) value;
d_set_dense_qp_ipm_arg_tol_ineq(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "tol_comp"))
{
double* tmp_ptr = (double *) value;
d_set_dense_qp_ipm_arg_tol_comp(*tmp_ptr, opts->hpipm_opts);
}
else if (!strcmp(field, "warm_start"))
{
int* tmp_ptr = (int *) value;
d_set_dense_qp_ipm_arg_warm_start(*tmp_ptr, opts->hpipm_opts);
}
else
{
printf("\nerror: dense_qp_hpipm_opts_set: wrong field: %s\n", field);
exit(1);
}

return;
}



/************************************************
* memory
************************************************/
@@ -111,6 +173,8 @@ int dense_qp_hpipm_memory_calculate_size(void *config_, void *dims_, void *opts_
return size;
}



void *dense_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void *raw_memory)
{
dense_qp_dims *dims = dims_;
@@ -139,7 +203,15 @@ void *dense_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void
return mem;
}

int dense_qp_hpipm_workspace_calculate_size(void *config_, void *dims_, void *opts_) { return 0; }


int dense_qp_hpipm_workspace_calculate_size(void *config_, void *dims_, void *opts_)
{
return 0;
}



int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_)
{
dense_qp_in *qp_in = qp_in_;
@@ -154,6 +226,13 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void
dense_qp_hpipm_opts *opts = opts_;
dense_qp_hpipm_memory *memory = mem_;

// zero primal solution
// TODO add a check if warm start of first SQP iteration is implemented !!!!!!
int ii;
int nv = qp_in->dim->nv;
int ns = qp_in->dim->ns;
blasfeo_dvecse(nv+2*ns, 0.0, qp_out->v, 0);

// solve ipm
acados_tic(&qp_timer);
int hpipm_status =
@@ -173,6 +252,8 @@ int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void
return acados_status;
}



void dense_qp_hpipm_config_initialize_default(void *config_)
{
qp_solver_config *config = config_;
@@ -182,6 +263,7 @@ void dense_qp_hpipm_config_initialize_default(void *config_)
config->opts_assign = &dense_qp_hpipm_opts_assign;
config->opts_initialize_default = &dense_qp_hpipm_opts_initialize_default;
config->opts_update = &dense_qp_hpipm_opts_update;
config->opts_set = &dense_qp_hpipm_opts_set;
config->memory_calculate_size = &dense_qp_hpipm_memory_calculate_size;
config->memory_assign = &dense_qp_hpipm_memory_assign;
config->workspace_calculate_size = &dense_qp_hpipm_workspace_calculate_size;
@@ -32,16 +32,22 @@ extern "C" {
#include "acados/dense_qp/dense_qp_common.h"
#include "acados/utils/types.h"



typedef struct dense_qp_hpipm_opts_
{
struct d_dense_qp_ipm_arg *hpipm_opts;
} dense_qp_hpipm_opts;



typedef struct dense_qp_hpipm_memory_
{
struct d_dense_qp_ipm_workspace *hpipm_workspace;
} dense_qp_hpipm_memory;



//
int dense_qp_hpipm_opts_calculate_size(void *config, void *dims);
//

0 comments on commit 4cd22d0

Please sign in to comment.
You can’t perform that action at this time.