From 3b1caf8744177b8b6901c62b79c0206adde889fc Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 19 May 2019 16:57:08 +0200 Subject: [PATCH 01/78] update hpipm --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index 073304b1b9..c1cfc9f172 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 073304b1b9763053b16106fa22d1067b929c911e +Subproject commit c1cfc9f1728479eef1b6f73e25d02b256e0f12a8 From ea5b6f8f857f97f8697e4c46cb9e0b4cfeb50ad6 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Mon, 20 May 2019 10:52:42 +0200 Subject: [PATCH 02/78] renamed ocp_nlp_render_arguments to ocp_nlp_acados --- interfaces/acados_template/acados_template/__init__.py | 2 +- .../{ocp_nlp_render_arguments.py => acados_ocp_nlp.py} | 2 +- interfaces/acados_template/acados_template/generate_solver.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) rename interfaces/acados_template/acados_template/{ocp_nlp_render_arguments.py => acados_ocp_nlp.py} (99%) diff --git a/interfaces/acados_template/acados_template/__init__.py b/interfaces/acados_template/acados_template/__init__.py index f361706290..34633b49fb 100644 --- a/interfaces/acados_template/acados_template/__init__.py +++ b/interfaces/acados_template/acados_template/__init__.py @@ -2,5 +2,5 @@ from .generate_c_code_explicit_ode import * from .generate_c_code_implicit_ode import * from .generate_c_code_constraint import * -from .ocp_nlp_render_arguments import * +from .acados_ocp_nlp import * from .generate_solver import * diff --git a/interfaces/acados_template/acados_template/ocp_nlp_render_arguments.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py similarity index 99% rename from interfaces/acados_template/acados_template/ocp_nlp_render_arguments.py rename to interfaces/acados_template/acados_template/acados_ocp_nlp.py index 13964fc413..f02a38cd13 100644 --- a/interfaces/acados_template/acados_template/ocp_nlp_render_arguments.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -954,7 +954,7 @@ def acados_ocp2json_layout(acados_ocp): Parameters ---------- acados_ocp : class - object of type ocp_nlp_render_arguments. + object of type acados_ocp_nlp. Returns ------ diff --git a/interfaces/acados_template/acados_template/generate_solver.py b/interfaces/acados_template/acados_template/generate_solver.py index b0f2423575..779855d429 100644 --- a/interfaces/acados_template/acados_template/generate_solver.py +++ b/interfaces/acados_template/acados_template/generate_solver.py @@ -2,7 +2,7 @@ from .generate_c_code_explicit_ode import * from .generate_c_code_implicit_ode import * from .generate_c_code_constraint import * -from .ocp_nlp_render_arguments import * +from .acados_ocp_nlp import * from ctypes import * def generate_solver(model, acados_ocp, con_h=None, con_hN=None, con_p=None, con_pN=None, json_file='acados_ocp_nlp.json'): From de88cb4a661b2a5dbce0018828877d5ba23ee96f Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Mon, 20 May 2019 11:31:16 +0200 Subject: [PATCH 03/78] fixing tera templates --- .../acados_template/c_templates_tera/acados_solver.in.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index 4d36f9a31a..7570453684 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -306,9 +306,9 @@ int acados_create() { double Vz[NY*NZ]; double yref_e[NYN]; - double WN[NYN*NYN]; + double W_e[NYN*NYN]; - double VxN[NYN*NX]; + double Vx_e[NYN*NX]; for (int ii = 0; ii < NU + NX; ii++) yref[ii] = 0.0; @@ -741,8 +741,8 @@ int acados_create() { {% if dims.nh_e > 0 %} // nonlinear constraints for stage N ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &h_constraint_e[i]); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lhN); - ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uhN); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); {% endif %} nlp_opts = ocp_nlp_opts_create(nlp_config, nlp_dims); From 5fcd0f899fc1c490b5095661ed06fee4d4e4c01e Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Tue, 21 May 2019 15:59:37 +0200 Subject: [PATCH 04/78] fix test on empty attributes --- .../acados_template/acados_ocp_nlp.py | 20 +++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index f02a38cd13..ee664c66f2 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -1080,19 +1080,23 @@ def json2dict_rec(ocp_nlp, ocp_nlp_dims, ocp_nlp_layout): if isinstance(v, int) or isinstance(v, float): v = np.array([v]) if v_type == 'ndarray' or v_type__ == 'list': + dims_l = [] + dims_names = [] + dim_keys = ocp_nlp_layout[k][1] + for item in dim_keys: + dims_l.append(ocp_nlp_dims[item]) + dims_names.append(item) + dims = tuple(dims_l) if v == []: # v = None - v = [] + try: + v = np.reshape(v, dims) + except: + raise Exception('acados -- mismatching dimensions for field {0}. Provided data has dimensions {1}, while associated dimensions {2} are {3}'.format(out_key, [], dims_names, dims)) + # v = [] else: v = np.array(v) v_dims = v.shape - dim_keys = ocp_nlp_layout[k][1] - dims_l = [] - dims_names = [] - for item in dim_keys: - dims_l.append(ocp_nlp_dims[item]) - dims_names.append(item) - dims = tuple(dims_l) try: v = np.reshape(v, dims) except: From 6c8d5e3d25f36113aee6a67b9b29f4706288d85c Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Tue, 21 May 2019 16:03:01 +0200 Subject: [PATCH 05/78] use Tera from MATLAB (WIP) --- .../generate_solver_matlab.py | 220 ++++++++++++++---- 1 file changed, 174 insertions(+), 46 deletions(-) diff --git a/interfaces/acados_template/+acados_template/generate_solver_matlab.py b/interfaces/acados_template/+acados_template/generate_solver_matlab.py index 35a0c0e0fd..c3931e5d76 100644 --- a/interfaces/acados_template/+acados_template/generate_solver_matlab.py +++ b/interfaces/acados_template/+acados_template/generate_solver_matlab.py @@ -6,6 +6,7 @@ import os def generate_solver_matlab(acados_ocp_nlp_json_file): + USE_TERA = 1 # EXPERIMENTAL: use Tera standalone parser instead of Jinja2 acados_path = os.path.dirname(acados_template.__file__) # load MATLAB JSON file instead @@ -29,77 +30,204 @@ def generate_solver_matlab(acados_ocp_nlp_json_file): acados_ocp = ra # setting up loader and environment - file_loader = FileSystemLoader(acados_path + '/c_templates') - env = Environment(loader = file_loader) + if USE_TERA == 0: + file_loader = FileSystemLoader(acados_path + '/c_templates') + env = Environment(loader = file_loader) + else: + template_glob = acados_path + '/c_templates_tera/*' + acados_template_path = acados_path + '/c_templates_tera' # render source template - template = env.get_template('main.in.c') - output = template.render(ocp=acados_ocp) + if USE_TERA == 0: + # render source template + template = env.get_template('main.in.c') + output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/main_' + model_name + '.c', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code') + # render source template + template_file = 'main.in.c' + out_file = 'main_' + model_name + '.c' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" + + os.system(os_cmd) + os.chdir('..') - # output file - out_file = open('./c_generated_code/main_' + model_name + '.c', 'w+') - out_file.write(output) # render source template - template = env.get_template('acados_solver.in.c') - output = template.render(ocp=acados_ocp) - # output file - out_file = open('./c_generated_code/acados_solver_' + model_name + '.c', 'w+') - out_file.write(output) + if USE_TERA == 0: + # render source template + template = env.get_template('acados_solver.in.c') + output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/acados_solver_' + model_name + '.c', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code') + # render source template + template_file = 'acados_solver.in.c' + out_file = 'acados_solver_' + model_name + '.c' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" + + os.system(os_cmd) + os.chdir('..') # render source template - template = env.get_template('acados_solver.in.h') - output = template.render(ocp=acados_ocp) - # output file - out_file = open('./c_generated_code/acados_solver_' + model_name + '.h', 'w+') - out_file.write(output) + if USE_TERA == 0: + # render source template + template = env.get_template('acados_solver.in.h') + output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/acados_solver_' + model_name + '.h', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code') + # render source template + template_file = 'acados_solver.in.h' + out_file = 'acados_solver_' + model_name + '.h' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" - # render header templates - template = env.get_template('model.in.h') - output = template.render(ocp=acados_ocp) - # output file - out_file = open('./c_generated_code/' + model_name + '_model/' + model_name + '_model.h', 'w+') - out_file.write(output) + os.system(os_cmd) + os.chdir('..') - if ra.dims.npd > 0: + # render header templates + if USE_TERA == 0: # render header templates - template = env.get_template('p_constraint.in.h') + template = env.get_template('model.in.h') output = template.render(ocp=acados_ocp) # output file - out_file = open('./c_generated_code/' + ra.con_p_name + '_p_constraint/' + ra.con_p_name + '_p_constraint.h', 'w+') + out_file = open('./c_generated_code/' + model.name + '_model/' + model_name + '_model.h', 'w+') out_file.write(output) + else: + os.chdir('c_generated_code/' + model_name + '_model/') + # render source template + template_file = 'model.in.h' + out_file = model.name + '_model.h' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" - if ra.dims.nh > 0: + os.system(os_cmd) + os.chdir('../..') + + if ra.dims.npd > 0: # render header templates - template = env.get_template('h_constraint.in.h') + if USE_TERA == 0: + # render header templates + template = env.get_template('p_constraint.in.h') + output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/' + acados_ocp.con_p_name + '_p_constraint/' + acados_ocp.con_p_name + '_p_constraint.h', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code/' + acados_ocp.con_p_name + '_p_constraint/') + # render source template + template_file = 'p_constraint.in.h' + out_file = acados_ocp.con_p_name + '_p_constraint.h' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" + + os.system(os_cmd) + os.chdir('../..') + + if ra.dims.nh > 0: + if USE_TERA == 0: + # render header templates + template = env.get_template('h_constraint.in.h') + output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/' + acados_ocp.con_h_name + '_h_constraint/' + acados_ocp.con_h_name + '_h_constraint.h', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code/' + acados_ocp.con_h_name + '_h_constraint/') + # render source template + template_file = 'h_constraint.in.h' + out_file = acados_ocp.con_h_name + '_h_constraint.h' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" + + os.system(os_cmd) + os.chdir('../..') + + # render Makefile template + if USE_TERA == 0: + # render Makefile template + template = env.get_template('Makefile.in') output = template.render(ocp=acados_ocp) + # output file - out_file = open('./c_generated_code/' + ra.con_h_name + '_h_constraint/' + ra.con_h_name + '_h_constraint.h', 'w+') + out_file = open('./c_generated_code/Makefile', 'w+') out_file.write(output) + else: + os.chdir('c_generated_code/') + # render source template + template_file = 'Makefile.in' + out_file = 'Makefile' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" - # render Makefile template - template = env.get_template('Makefile.in') - output = template.render(ocp=acados_ocp) + os.system(os_cmd) + os.chdir('..') - # output file - out_file = open('./c_generated_code/Makefile', 'w+') - out_file.write(output) + if USE_TERA == 0: + # render S-Function template + template = env.get_template('acados_solver_sfun.in.c') + output = template.render(ocp=acados_ocp) - # render S-Function template - template = env.get_template('acados_solver_sfun.in.c') - output = template.render(ocp=acados_ocp) + # output file + out_file = open('./c_generated_code/acados_solver_sfunction_' + model_name + '.c', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code/') + # render source template + template_file = 'acados_solver_sfun.in.c' + out_file = 'acados_solver_sfunction_' + model_name + '.c' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" - # output file - out_file = open('./c_generated_code/acados_solver_sfunction_' + model_name + '.c', 'w+') - out_file.write(output) + os.system(os_cmd) # render MATLAB make script - template = env.get_template('make_sfun.in.m') - output = template.render(ocp=acados_ocp) + if USE_TERA == 0: + # render MATLAB make script + template = env.get_template('make_sfun.in.m') + output = template.render(ocp=acados_ocp) + + # output file + out_file = open('./c_generated_code/make_sfun.m', 'w+') + out_file.write(output) + else: + os.chdir('c_generated_code/') + # render source template + template_file = 'make_sfun.in.m' + out_file = 'acados_solver_sfun.in.c' + # output file + os_cmd = 't_renderer ' + "\"" + template_glob + "\"" + ' ' + "\"" \ + + template_file + "\"" + ' ' + "\"" + '../' + json_file + \ + "\"" + ' ' + "\"" + out_file + "\"" - # output file - out_file = open('./c_generated_code/make_sfun.m', 'w+') - out_file.write(output) + os.system(os_cmd) + os.chdir('..') print('Successfully generated acados solver!\n') From 8806a28da009dbed5c02e0394fae0909eccad2eb Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 21 May 2019 16:33:12 +0200 Subject: [PATCH 06/78] add opts_set to ocp_qp_hpipm --- acados/ocp_qp/ocp_qp_common.h | 12 ++- acados/ocp_qp/ocp_qp_full_condensing_solver.c | 3 +- acados/ocp_qp/ocp_qp_full_condensing_solver.h | 3 +- acados/ocp_qp/ocp_qp_hpipm.c | 81 ++++++++++++++++++- acados/ocp_qp/ocp_qp_hpipm.h | 11 +++ .../ocp_qp/ocp_qp_partial_condensing_solver.c | 7 +- .../ocp_qp/ocp_qp_partial_condensing_solver.h | 3 +- 7 files changed, 102 insertions(+), 18 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index d444eff427..646e5ba3da 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -50,7 +50,7 @@ typedef struct void *(*opts_assign)(void *config, void *dims, void *raw_memory); void (*opts_initialize_default)(void *config, void *dims, void *opts); void (*opts_update)(void *config, void *dims, void *opts); - void (*opts_set)(void *config_, void *opts_, const char *field, const void* value); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); int (*memory_calculate_size)(void *config, void *dims, void *opts); void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); int (*workspace_calculate_size)(void *config, void *dims, void *opts); @@ -66,7 +66,7 @@ typedef struct void *(*opts_assign)(ocp_qp_dims *dims, void *raw_memory); void (*opts_initialize_default)(ocp_qp_dims *dims, void *opts); void (*opts_update)(ocp_qp_dims *dims, void *opts); - void (*opts_set)(void *config_, void *opts_, const char *field, const void* value); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); int (*memory_calculate_size)(ocp_qp_dims *dims, void *opts); void *(*memory_assign)(ocp_qp_dims *dims, void *opts, void *raw_memory); int (*workspace_calculate_size)(ocp_qp_dims *dims, void *opts); @@ -77,13 +77,12 @@ typedef struct typedef struct { void (*dims_set)(void *config_, void *dims_, int stage, const char *field, const int* value); - int (*evaluate)(void *config, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, - void *work); + int (*evaluate)(void *config, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); int (*opts_calculate_size)(void *config, ocp_qp_dims *dims); void *(*opts_assign)(void *config, ocp_qp_dims *dims, void *raw_memory); void (*opts_initialize_default)(void *config, ocp_qp_dims *dims, void *opts); void (*opts_update)(void *config, ocp_qp_dims *dims, void *opts); - void (*opts_set)(void *config_, void *opts_, const char *field, const void* value); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); int (*memory_calculate_size)(void *config, ocp_qp_dims *dims, void *opts); void *(*memory_assign)(void *config, ocp_qp_dims *dims, void *opts, void *raw_memory); int (*workspace_calculate_size)(void *config, ocp_qp_dims *dims, void *opts); @@ -142,8 +141,7 @@ int ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims); // ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory); // -void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, - ocp_qp_res_ws *res_ws); +void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws); // void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]); // diff --git a/acados/ocp_qp/ocp_qp_full_condensing_solver.c b/acados/ocp_qp/ocp_qp_full_condensing_solver.c index 1a74f49457..c57640dddb 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_full_condensing_solver.c @@ -112,8 +112,7 @@ void ocp_qp_full_condensing_solver_opts_update(void *config_, ocp_qp_dims *dims, qp_solver->opts_update(qp_solver, NULL, opts->qp_solver_opts); // TODO(all): pass dense_qp_dims } -void ocp_qp_full_condensing_solver_opts_set(void *config_, void *opts_, - const char *field, const void* value) +void ocp_qp_full_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value) { ocp_qp_full_condensing_solver_opts *opts = (ocp_qp_full_condensing_solver_opts *) opts_; // ocp_qp_xcond_solver_config *config = config_; diff --git a/acados/ocp_qp/ocp_qp_full_condensing_solver.h b/acados/ocp_qp/ocp_qp_full_condensing_solver.h index b0c65491f4..3c73b2d02c 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing_solver.h +++ b/acados/ocp_qp/ocp_qp_full_condensing_solver.h @@ -63,8 +63,7 @@ void ocp_qp_full_condensing_solver_opts_initialize_default(void *config, ocp_qp_ // void ocp_qp_full_condensing_solver_opts_update(void *config, ocp_qp_dims *dims, void *opts_); // -void ocp_qp_full_condensing_solver_opts_set(void *config_, void *opts_, - const char *field, const void* value); +void ocp_qp_full_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value); // int ocp_qp_full_condensing_solver_memory_calculate_size(void *config, ocp_qp_dims *dims, void *opts_); diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index 71fab644c4..fb0c8ad834 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -18,7 +18,9 @@ */ // external +#include #include +#include // hpipm #include "hpipm/include/hpipm_d_ocp_qp.h" #include "hpipm/include/hpipm_d_ocp_qp_ipm.h" @@ -31,6 +33,8 @@ #include "acados/utils/timing.h" #include "acados/utils/types.h" + + /************************************************ * opts ************************************************/ @@ -48,6 +52,8 @@ int ocp_qp_hpipm_opts_calculate_size(void *config_, void *dims_) return size; } + + void *ocp_qp_hpipm_opts_assign(void *config_, void *dims_, void *raw_memory) { ocp_qp_dims *dims = dims_; @@ -72,6 +78,8 @@ void *ocp_qp_hpipm_opts_assign(void *config_, void *dims_, void *raw_memory) return (void *) opts; } + + void ocp_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts_) { // ocp_qp_dims *dims = dims_; @@ -91,6 +99,8 @@ void ocp_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts return; } + + void ocp_qp_hpipm_opts_update(void *config_, void *dims_, void *opts_) { // ocp_qp_hpipm_opts *opts = (ocp_qp_hpipm_opts *)opts_; @@ -98,6 +108,63 @@ void ocp_qp_hpipm_opts_update(void *config_, void *dims_, void *opts_) return; } + + +void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_hpipm_opts *opts = opts_; + + if (!strcmp(field, "iter_max")) + { + int* tmp_ptr = (int *) value; + d_set_ocp_qp_ipm_arg_iter_max(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "mu0")) + { + double* tmp_ptr = (double *) value; + d_set_ocp_qp_ipm_arg_mu0(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "tol_stat")) + { + double* tmp_ptr = (double *) value; + d_set_ocp_qp_ipm_arg_tol_stat(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "tol_eq")) + { + double* tmp_ptr = (double *) value; + d_set_ocp_qp_ipm_arg_tol_eq(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "tol_ineq")) + { + double* tmp_ptr = (double *) value; + d_set_ocp_qp_ipm_arg_tol_ineq(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "tol_comp")) + { + double* tmp_ptr = (double *) value; + d_set_ocp_qp_ipm_arg_tol_comp(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "warm_start")) + { + int* tmp_ptr = (int *) value; + d_set_ocp_qp_ipm_arg_warm_start(*tmp_ptr, opts->hpipm_opts); + } + else if (!strcmp(field, "ric_alg")) + { + int* tmp_ptr = (int *) value; + d_set_ocp_qp_ipm_arg_ric_alg(*tmp_ptr, opts->hpipm_opts); + } + else + { + printf("\nerror: ocp_qp_hpipm_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -118,6 +185,8 @@ int ocp_qp_hpipm_memory_calculate_size(void *config_, void *dims_, void *opts_) return size; } + + void *ocp_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void *raw_memory) { ocp_qp_dims *dims = dims_; @@ -147,11 +216,18 @@ void *ocp_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void * return mem; } + + /************************************************ * workspace ************************************************/ -int ocp_qp_hpipm_workspace_calculate_size(void *config_, void *dims_, void *opts_) { return 0; } +int ocp_qp_hpipm_workspace_calculate_size(void *config_, void *dims_, void *opts_) +{ + return 0; +} + + /************************************************ * functions @@ -189,6 +265,8 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void * return acados_status; } + + void ocp_qp_hpipm_config_initialize_default(void *config_) { qp_solver_config *config = config_; @@ -198,6 +276,7 @@ void ocp_qp_hpipm_config_initialize_default(void *config_) config->opts_assign = &ocp_qp_hpipm_opts_assign; config->opts_initialize_default = &ocp_qp_hpipm_opts_initialize_default; config->opts_update = &ocp_qp_hpipm_opts_update; + config->opts_set = &ocp_qp_hpipm_opts_set; config->memory_calculate_size = &ocp_qp_hpipm_memory_calculate_size; config->memory_assign = &ocp_qp_hpipm_memory_assign; config->workspace_calculate_size = &ocp_qp_hpipm_workspace_calculate_size; diff --git a/acados/ocp_qp/ocp_qp_hpipm.h b/acados/ocp_qp/ocp_qp_hpipm.h index ddfdd4e23d..b6d8b8879c 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.h +++ b/acados/ocp_qp/ocp_qp_hpipm.h @@ -30,6 +30,8 @@ extern "C" { #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/utils/types.h" + + // struct of arguments to the solver // TODO(roversch): why not make this a typedef of the underlying struct? typedef struct ocp_qp_hpipm_opts_ @@ -44,6 +46,8 @@ typedef struct ocp_qp_hpipm_memory_ struct d_ocp_qp_ipm_workspace *hpipm_workspace; } ocp_qp_hpipm_memory; + + // int ocp_qp_hpipm_opts_calculate_size(void *config, void *dims); // @@ -52,17 +56,24 @@ void *ocp_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory); void ocp_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_); // void ocp_qp_hpipm_opts_update(void *config, void *dims, void *opts_); +// +void ocp_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value); + // int ocp_qp_hpipm_memory_calculate_size(void *config, void *dims, void *opts_); // void *ocp_qp_hpipm_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); + // int ocp_qp_hpipm_workspace_calculate_size(void *config, void *dims, void *opts_); // int ocp_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); + // void ocp_qp_hpipm_config_initialize_default(void *config); + + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 5e8577d7ad..148b09ffb1 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -108,8 +108,7 @@ void ocp_qp_partial_condensing_solver_opts_update(void *config_, ocp_qp_dims *di } -void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, - const char *field, const void* value) +void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value) { ocp_qp_partial_condensing_solver_opts *opts = (ocp_qp_partial_condensing_solver_opts *) opts_; // ocp_qp_xcond_solver_config *config = config_; @@ -126,8 +125,8 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, } else { - printf("\nerror: option type %s not available in ocp_qp_partial_condense solver module\n", - field); + // TODO pass options to qp solver + printf("\nerror: option type %s not available in ocp_qp_partial_condense solver module\n", field); exit(1); } } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.h b/acados/ocp_qp/ocp_qp_partial_condensing_solver.h index 9f39cf8ce1..77fbce102e 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.h @@ -63,8 +63,7 @@ void ocp_qp_partial_condensing_solver_opts_initialize_default(void *config, ocp_ // void ocp_qp_partial_condensing_solver_opts_update(void *config, ocp_qp_dims *dims, void *opts_); // -void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, - const char *field, const void* value); +void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value); // int ocp_qp_partial_condensing_solver_calculate_memory_size(void *config, ocp_qp_dims *dims, void *opts_); From 00548becae66d37505584807e2b8ce5c6ff088f2 Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 21 May 2019 16:34:13 +0200 Subject: [PATCH 07/78] forgot addition --- acados/dense_qp/dense_qp_common.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/acados/dense_qp/dense_qp_common.h b/acados/dense_qp/dense_qp_common.h index 4c6c4d8c06..3dc657929d 100644 --- a/acados/dense_qp/dense_qp_common.h +++ b/acados/dense_qp/dense_qp_common.h @@ -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]); // From fd1cccff4302e4ecf7f65bed428e2ceb4b7971fd Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 22 May 2019 09:56:56 +0200 Subject: [PATCH 08/78] add ocp_nlp_qp_opts_set --- acados/ocp_nlp/ocp_nlp_common.h | 7 +++--- acados/ocp_nlp/ocp_nlp_sqp.c | 22 ++++++++++++++-- acados/ocp_nlp/ocp_nlp_sqp.h | 3 +-- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 25 +++++++++++++++++-- examples/c/wind_turbine_nmpc.c | 2 +- .../pendulum_on_cart_model/example_ocp.m | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 15 +++++++++-- interfaces/acados_c/ocp_nlp_interface.h | 12 +++++++-- interfaces/acados_matlab/ocp_create.c | 2 +- test/ocp_nlp/test_wind_turbine.cpp | 2 +- 10 files changed, 75 insertions(+), 17 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 1cf80066e5..3e101d4b59 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -70,9 +70,10 @@ typedef struct void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory); int (*workspace_calculate_size)(void *config, void *dims, void *opts_); void (*opts_set)(void *config_, void *opts_, const char *field, void* value); - void (*dynamics_opts_set)(void *config, void *opts, int stage, const char *field, void *value); - void (*cost_opts_set)(void *config, void *opts, int stage, const char *field, void *value); - void (*constraints_opts_set)(void *config, void *opts, int stage, const char *field, void *value); + void (*qp_opts_set)(void *config, void *opts, const char *field, void *value); // TODO rename !!! + void (*dynamics_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! + void (*cost_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! + void (*constraints_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! // evaluate solver int (*evaluate)(void *config, void *dims, void *qp_in, void *qp_out, void *opts_, void *mem, void *work); // prepare memory diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 6233317ece..d687f50415 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -313,13 +313,28 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v } else { - // TODO extract prefix 'qp_solver_' from filed ??? - config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + printf("\nerror: ocp_nlp_sqp_opts_set: wrong field: %s\n", field); + exit(1); } } +// TODO rename ... opts_set_qp !!! +void ocp_nlp_sqp_qp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_nlp_config *config = config_; + ocp_nlp_sqp_opts *opts = opts_; + + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + + return; + +} + + + +// TODO rename ... opts_set_dynamics !!! void ocp_nlp_sqp_dynamics_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -335,6 +350,7 @@ void ocp_nlp_sqp_dynamics_opts_set(void *config_, void *opts_, int stage, +// TODO rename ... opts_set_cost !!! void ocp_nlp_sqp_cost_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -350,6 +366,7 @@ void ocp_nlp_sqp_cost_opts_set(void *config_, void *opts_, int stage, +// TODO rename ... opts_set_constraints !!! void ocp_nlp_sqp_constraints_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -1412,6 +1429,7 @@ void ocp_nlp_sqp_config_initialize_default(void *config_) config->opts_initialize_default = &ocp_nlp_sqp_opts_initialize_default; config->opts_update = &ocp_nlp_sqp_opts_update; config->opts_set = &ocp_nlp_sqp_opts_set; + config->qp_opts_set = &ocp_nlp_sqp_qp_opts_set; config->dynamics_opts_set = &ocp_nlp_sqp_dynamics_opts_set; config->cost_opts_set = &ocp_nlp_sqp_cost_opts_set; config->constraints_opts_set = &ocp_nlp_sqp_constraints_opts_set; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index f6a2dde13b..c4de24fe3a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -70,8 +70,7 @@ void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts); // void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value); // -void ocp_nlp_sqp_dyanimcs_opts_set(void *config, void *opts, int stage, - const char *field, void *value); +void ocp_nlp_sqp_dyanimcs_opts_set(void *config, void *opts, int stage, const char *field, void *value); /************************************************ * memory diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index e26e3cbc23..ca6ca333e9 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -283,13 +283,31 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi } else { - // TODO extract prefix 'qp_solver_' from filed ??? - config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + printf("\nerror: ocp_nlp_sqp_rti_opts_set: wrong field: %s\n", field); + exit(1); } + + return; + +} + + + +// TODO rename ... opts_set_qp !!! +void ocp_nlp_sqp_rti_qp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_nlp_config *config = config_; + ocp_nlp_sqp_rti_opts *opts = opts_; + + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + + return; + } +// TODO rename ... opts_set_dynamics !!! void ocp_nlp_sqp_rti_dynamics_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -305,6 +323,7 @@ void ocp_nlp_sqp_rti_dynamics_opts_set(void *config_, void *opts_, int stage, +// TODO rename ... opts_set_cost !!! void ocp_nlp_sqp_rti_cost_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -320,6 +339,7 @@ void ocp_nlp_sqp_rti_cost_opts_set(void *config_, void *opts_, int stage, +// TODO rename ... opts_set_constraints !!! void ocp_nlp_sqp_rti_constraints_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -1288,6 +1308,7 @@ void ocp_nlp_sqp_rti_config_initialize_default(void *config_) config->opts_initialize_default = &ocp_nlp_sqp_rti_opts_initialize_default; config->opts_update = &ocp_nlp_sqp_rti_opts_update; config->opts_set = &ocp_nlp_sqp_rti_opts_set; + config->qp_opts_set = &ocp_nlp_sqp_rti_qp_opts_set; config->dynamics_opts_set = &ocp_nlp_sqp_rti_dynamics_opts_set; config->cost_opts_set = &ocp_nlp_sqp_rti_cost_opts_set; config->constraints_opts_set = &ocp_nlp_sqp_rti_constraints_opts_set; diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index 88c4165d91..b1bf9c0bfe 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -874,7 +874,7 @@ int main() if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { int pcond_N2 = 5; - ocp_nlp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); + ocp_nlp_qp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); } // update opts after manual changes diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 2751ce90d9..4a622d7bd5 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -20,7 +20,7 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_N_pcond = N; %5; +qp_solver_N_pcond = 5; %sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 1fe7bb4eee..2e2f26d780 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -520,14 +520,23 @@ void *ocp_nlp_opts_create(ocp_nlp_config *config, ocp_nlp_dims *dims) -void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, - const char *field, void *value) +void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void *value) { config->opts_set(config, opts_, field, value); } +// TODO rename !!! +void ocp_nlp_qp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void *value) +{ + config->qp_opts_set(config, opts_, field, value); + return; +} + + + +// TODO rename !!! void ocp_nlp_dynamics_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { @@ -537,6 +546,7 @@ void ocp_nlp_dynamics_opts_set(ocp_nlp_config *config, void *opts_, int stage, +// TODO rename !!! void ocp_nlp_cost_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { @@ -546,6 +556,7 @@ void ocp_nlp_cost_opts_set(ocp_nlp_config *config, void *opts_, int stage, +// TODO rename !!! void ocp_nlp_constraints_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 33e9f238d5..3d95dfc93a 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -270,8 +270,16 @@ void ocp_nlp_opts_destroy(void *opts); /// \param opts_ The options struct. /// \param field Name of the option. /// \param value Value of the option. -void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, - const char *field, void* value); +void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value); + +/// TBC +/// Set the option for the QP solver. +/// +/// \param config The configuration struct. +/// \param opts_ The options struct. +/// \param field Name of the option. +/// \param value Value of the option. +void ocp_nlp_qp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void *value); /// TBC /// Set the option for the dynamics in a given stage. diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 335b0cfb3d..5baa5dcc0d 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -1049,7 +1049,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // qp_solver_N_pcond if(set_qp_solver_N_pcond) { - ocp_nlp_opts_set(config, opts, "pcond_N2", &qp_solver_N_pcond); + ocp_nlp_qp_opts_set(config, opts, "pcond_N2", &qp_solver_N_pcond); } // sim_method_num_stages if(set_sim_method_num_stages) diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index d6453c608a..3ad757f65c 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -946,7 +946,7 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { int pcond_N2 = 10; - ocp_nlp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); + ocp_nlp_qp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); } config->opts_update(config, dims, nlp_opts); From 0216c09d5ccf230a87552f5cee553979da9d9188 Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 22 May 2019 10:44:29 +0200 Subject: [PATCH 09/78] replace ocp_nlp_qp_opts_set with module-from-string extraction in ocp_nlp_opts_set --- acados/ocp_nlp/ocp_nlp_common.h | 1 - acados/ocp_nlp/ocp_nlp_sqp.c | 145 +++++++++++++----------- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 84 +++++++------- examples/c/wind_turbine_nmpc.c | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 9 -- interfaces/acados_c/ocp_nlp_interface.h | 9 -- interfaces/acados_matlab/ocp_create.c | 2 +- test/ocp_nlp/test_wind_turbine.cpp | 2 +- 8 files changed, 127 insertions(+), 127 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index 3e101d4b59..fa509b8c4c 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -70,7 +70,6 @@ typedef struct void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory); int (*workspace_calculate_size)(void *config, void *dims, void *opts_); void (*opts_set)(void *config_, void *opts_, const char *field, void* value); - void (*qp_opts_set)(void *config, void *opts, const char *field, void *value); // TODO rename !!! void (*dynamics_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! void (*cost_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! void (*constraints_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index d687f50415..2d9a80790d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -263,72 +263,84 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v int ii; - if (!strcmp(field, "max_iter")) - { - int* max_iter = (int *) value; - opts->max_iter = *max_iter; - } - else if (!strcmp(field, "reuse_workspace")) - { - int* reuse_workspace = (int *) value; - opts->reuse_workspace = *reuse_workspace; - } - else if (!strcmp(field, "num_threads")) - { - int* num_threads = (int *) value; - opts->num_threads = *num_threads; - } - else if (!strcmp(field, "min_res_g")) - { - double* min_res_g = (double *) value; - opts->min_res_g = *min_res_g; - } - else if (!strcmp(field, "min_res_b")) - { - double* min_res_b = (double *) value; - opts->min_res_b = *min_res_b; - } - else if (!strcmp(field, "min_res_d")) - { - double* min_res_d = (double *) value; - opts->min_res_d = *min_res_d; - } - else if (!strcmp(field, "min_res_m")) - { - double* min_res_m = (double *) value; - opts->min_res_m = *min_res_m; - } - else if (!strcmp(field, "exact_hess")) - { - int N = config->N; - // cost - for (ii=0; ii<=N; ii++) - config->cost[ii]->opts_set(config->cost[ii], opts->cost[ii], "exact_hess", value); - // dynamics - for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); - // constraints - for (ii=0; ii<=N; ii++) - config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); - } - else - { - printf("\nerror: ocp_nlp_sqp_opts_set: wrong field: %s\n", field); - exit(1); - } -} - - - -// TODO rename ... opts_set_qp !!! -void ocp_nlp_sqp_qp_opts_set(void *config_, void *opts_, const char *field, void *value) -{ - ocp_nlp_config *config = config_; - ocp_nlp_sqp_opts *opts = opts_; - - config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); - - return; + char module[MAX_STR_LEN]; + char *ptr_module = NULL; + int module_length = 0; + + // extract module name + char *char_ = strchr(field, '_'); + if(char_!=NULL) + { + module_length = char_-field; + for(ii=0; iiqp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field+module_length+1, value); + } + else // nlp opts + { + if (!strcmp(field, "max_iter")) + { + int* max_iter = (int *) value; + opts->max_iter = *max_iter; + } + else if (!strcmp(field, "reuse_workspace")) + { + int* reuse_workspace = (int *) value; + opts->reuse_workspace = *reuse_workspace; + } + else if (!strcmp(field, "num_threads")) + { + int* num_threads = (int *) value; + opts->num_threads = *num_threads; + } + else if (!strcmp(field, "min_res_g")) + { + double* min_res_g = (double *) value; + opts->min_res_g = *min_res_g; + } + else if (!strcmp(field, "min_res_b")) + { + double* min_res_b = (double *) value; + opts->min_res_b = *min_res_b; + } + else if (!strcmp(field, "min_res_d")) + { + double* min_res_d = (double *) value; + opts->min_res_d = *min_res_d; + } + else if (!strcmp(field, "min_res_m")) + { + double* min_res_m = (double *) value; + opts->min_res_m = *min_res_m; + } + else if (!strcmp(field, "exact_hess")) + { + int N = config->N; + // cost + for (ii=0; ii<=N; ii++) + config->cost[ii]->opts_set(config->cost[ii], opts->cost[ii], "exact_hess", value); + // dynamics + for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); + // constraints + for (ii=0; ii<=N; ii++) + config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); + } + else + { + printf("\nerror: ocp_nlp_sqp_opts_set: wrong field: %s\n", field); + exit(1); + } + } + + return; } @@ -1429,7 +1441,6 @@ void ocp_nlp_sqp_config_initialize_default(void *config_) config->opts_initialize_default = &ocp_nlp_sqp_opts_initialize_default; config->opts_update = &ocp_nlp_sqp_opts_update; config->opts_set = &ocp_nlp_sqp_opts_set; - config->qp_opts_set = &ocp_nlp_sqp_qp_opts_set; config->dynamics_opts_set = &ocp_nlp_sqp_dynamics_opts_set; config->cost_opts_set = &ocp_nlp_sqp_cost_opts_set; config->constraints_opts_set = &ocp_nlp_sqp_constraints_opts_set; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index ca6ca333e9..1ce1b37dd7 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -263,29 +263,52 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi int ii; - if (!strcmp(field, "num_threads")) - { - int* num_threads = (int *) value; - opts->num_threads = *num_threads; - } - else if (!strcmp(field, "exact_hess")) - { - int N = config->N; - // cost - for (ii=0; ii<=N; ii++) - config->cost[ii]->opts_set(config->cost[ii], opts->cost[ii], "exact_hess", value); - // dynamics - for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); - // constraints - for (ii=0; ii<=N; ii++) - config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); - } - else - { - printf("\nerror: ocp_nlp_sqp_rti_opts_set: wrong field: %s\n", field); - exit(1); - } + char module[MAX_STR_LEN]; + char *ptr_module = NULL; + int module_length = 0; + + // extract module name + char *char_ = strchr(field, '_'); + if(char_!=NULL) + { + module_length = char_-field; + for(ii=0; iiqp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field+module_length+1, value); + } + else // nlp opts + { + if (!strcmp(field, "num_threads")) + { + int* num_threads = (int *) value; + opts->num_threads = *num_threads; + } + else if (!strcmp(field, "exact_hess")) + { + int N = config->N; + // cost + for (ii=0; ii<=N; ii++) + config->cost[ii]->opts_set(config->cost[ii], opts->cost[ii], "exact_hess", value); + // dynamics + for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); + // constraints + for (ii=0; ii<=N; ii++) + config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); + } + else + { + printf("\nerror: ocp_nlp_sqp_rti_opts_set: wrong field: %s\n", field); + exit(1); + } + } return; @@ -293,20 +316,6 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi -// TODO rename ... opts_set_qp !!! -void ocp_nlp_sqp_rti_qp_opts_set(void *config_, void *opts_, const char *field, void *value) -{ - ocp_nlp_config *config = config_; - ocp_nlp_sqp_rti_opts *opts = opts_; - - config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); - - return; - -} - - - // TODO rename ... opts_set_dynamics !!! void ocp_nlp_sqp_rti_dynamics_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) @@ -1308,7 +1317,6 @@ void ocp_nlp_sqp_rti_config_initialize_default(void *config_) config->opts_initialize_default = &ocp_nlp_sqp_rti_opts_initialize_default; config->opts_update = &ocp_nlp_sqp_rti_opts_update; config->opts_set = &ocp_nlp_sqp_rti_opts_set; - config->qp_opts_set = &ocp_nlp_sqp_rti_qp_opts_set; config->dynamics_opts_set = &ocp_nlp_sqp_rti_dynamics_opts_set; config->cost_opts_set = &ocp_nlp_sqp_rti_cost_opts_set; config->constraints_opts_set = &ocp_nlp_sqp_rti_constraints_opts_set; diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index b1bf9c0bfe..a6fd9e7940 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -874,7 +874,7 @@ int main() if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { int pcond_N2 = 5; - ocp_nlp_qp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); + ocp_nlp_opts_set(config, nlp_opts, "qp_pcond_N2", &pcond_N2); } // update opts after manual changes diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 2e2f26d780..42eb716d06 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -527,15 +527,6 @@ void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, vo -// TODO rename !!! -void ocp_nlp_qp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void *value) -{ - config->qp_opts_set(config, opts_, field, value); - return; -} - - - // TODO rename !!! void ocp_nlp_dynamics_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index 3d95dfc93a..ea21be4480 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -272,15 +272,6 @@ void ocp_nlp_opts_destroy(void *opts); /// \param value Value of the option. void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value); -/// TBC -/// Set the option for the QP solver. -/// -/// \param config The configuration struct. -/// \param opts_ The options struct. -/// \param field Name of the option. -/// \param value Value of the option. -void ocp_nlp_qp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void *value); - /// TBC /// Set the option for the dynamics in a given stage. /// diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 5baa5dcc0d..22a579678e 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -1049,7 +1049,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // qp_solver_N_pcond if(set_qp_solver_N_pcond) { - ocp_nlp_qp_opts_set(config, opts, "pcond_N2", &qp_solver_N_pcond); + ocp_nlp_opts_set(config, opts, "qp_pcond_N2", &qp_solver_N_pcond); } // sim_method_num_stages if(set_sim_method_num_stages) diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index 3ad757f65c..c91036f82b 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -946,7 +946,7 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { int pcond_N2 = 10; - ocp_nlp_qp_opts_set(config, nlp_opts, "pcond_N2", &pcond_N2); + ocp_nlp_opts_set(config, nlp_opts, "qp_pcond_N2", &pcond_N2); } config->opts_update(config, dims, nlp_opts); From 6f73a960d079b51378aa23a709a8bd2c42c02002 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 10:54:50 +0200 Subject: [PATCH 10/78] remove constant from ocp (for now), fix bug when npd = 0 --- .../acados_template/acados_template/acados_layout.json | 5 ----- interfaces/acados_template/acados_template/acados_ocp_nlp.py | 2 +- .../acados_template/acados_template/c_templates/Makefile.in | 2 +- .../acados_template/c_templates_tera/Makefile.in | 2 +- .../examples/python/pendulum_example/generate_c_code.py | 2 +- .../python/rel_sync_machine_example/generate_c_code.py | 2 +- 6 files changed, 5 insertions(+), 10 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_layout.json b/interfaces/acados_template/acados_template/acados_layout.json index e02f4df0f4..1bb1b1aaf8 100644 --- a/interfaces/acados_template/acados_template/acados_layout.json +++ b/interfaces/acados_template/acados_template/acados_layout.json @@ -17,11 +17,6 @@ "con_p_name": [ "NoneType" ], - "constants": { - "PI": [ - "float" - ] - }, "constraints": { "lbx": [ "ndarray", diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index ee664c66f2..1d238cdb19 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -913,7 +913,7 @@ def __init__(self): self.con_p_e_name = None self.con_h_name = None self.con_h_e_name = None - self.constants = {} + # self.constants = {} self.acados_include_path = [] self.acados_lib_path = [] diff --git a/interfaces/acados_template/acados_template/c_templates/Makefile.in b/interfaces/acados_template/acados_template/c_templates/Makefile.in index 50a3812ab9..976ab377b5 100644 --- a/interfaces/acados_template/acados_template/c_templates/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates/Makefile.in @@ -51,7 +51,7 @@ CASADI_CON_H_SOURCE+= {{ ocp.con_h_name }}_h_constraint.c casadi_fun: ( cd {{ ocp.model_name }}_model; gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE)) - {% if ocp.dims.np > 0 %} + {% if ocp.dims.npd > 0 %} ( cd {{ ocp.con_p_name }}_p_constraint; gcc $(ACADOS_FLAGS) -c $(CASADI_CON_P_SOURCE)) {% endif %} {% if ocp.dims.nh > 0 %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in index 5f36296703..1d402de218 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in +++ b/interfaces/acados_template/acados_template/c_templates_tera/Makefile.in @@ -51,7 +51,7 @@ CASADI_CON_H_SOURCE+= {{ con_h_name }}_h_constraint.c casadi_fun: ( cd {{ model_name }}_model; gcc $(ACADOS_FLAGS) -c $(CASADI_MODEL_SOURCE)) - {% if dims.np > 0 %} + {% if dims.npd > 0 %} ( cd {{ con_p_name }}_p_constraint; gcc $(ACADOS_FLAGS) -c $(CASADI_CON_P_SOURCE)) {% endif %} {% if dims.nh > 0 %} diff --git a/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py index 3877da771a..126e0c0f3f 100644 --- a/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py +++ b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py @@ -78,7 +78,7 @@ nlp_con.idxbu = np.array([0]) # set constants -ra.constants['PI'] = 3.1415926535897932 +# ra.constants['PI'] = 3.1415926535897932 # set QP solver # ra.solver_config.qp_solver = 'PARTIAL_CONDENSING_HPIPM' diff --git a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py index e73803a656..bf0e577ab2 100644 --- a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py +++ b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py @@ -366,7 +366,7 @@ def get_general_constraints_DC(u_max): nlp_con.p = nmp.array([w_val, 0.0, 0.0]) # set constants -ra.constants = [] +# ra.constants = [] # set QP solver ra.solver_config.qp_solver = 'PARTIAL_CONDENSING_HPIPM' From d823f98693c495d096b88d3f8592afcf7a92ac10 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 15:27:26 +0200 Subject: [PATCH 11/78] fix circular constraint radius --- .../python/rel_sync_machine_example/generate_c_code.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py index bf0e577ab2..b99c63dd3e 100644 --- a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py +++ b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py @@ -347,7 +347,7 @@ def get_general_constraints_DC(u_max): if FORMULATION > 0: nlp_con.lh = nmp.array([-1.0e8]) - nlp_con.uh = nmp.array([u_max**2]) + nlp_con.uh = nmp.array([(u_max*sqrt(3)/2)**2]) nlp_con.x0 = nmp.array([0.0, -0.0]) @@ -496,6 +496,6 @@ def get_general_constraints_DC(u_max): ax = plt.gca() ax.set_xlim([-1.5*u_max, 1.5*u_max]) ax.set_ylim([-1.5*u_max, 1.5*u_max]) -circle = plt.Circle((0, 0), u_max, color='red', fill=False) +circle = plt.Circle((0, 0), u_max*nmp.sqrt(3)/2, color='red', fill=False) ax.add_artist(circle) plt.show() From 00119224427e610f608ccbbcba15c7cb7205095d Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 16:42:36 +0200 Subject: [PATCH 12/78] update docs --- docs/installation/index.md | 7 +++ docs/interfaces/index.md | 6 +- .../acados_template/acados_ocp_nlp.py | 62 +++++++++---------- .../c_templates/acados_solver.in.c | 6 +- 4 files changed, 42 insertions(+), 39 deletions(-) diff --git a/docs/installation/index.md b/docs/installation/index.md index 046c5b1070..68eb7eea42 100644 --- a/docs/installation/index.md +++ b/docs/installation/index.md @@ -7,6 +7,11 @@ To install CasADi, you can follow the installation instructions [here](https://g ## Installation Both a CMake and a Makefile based build system are supported at the moment. +1. Clone acados + ``` + git clone https://github.com/acados/acados.git + ``` + 1. Initialize all submodules ``` git submodule update --recursive --init @@ -63,3 +68,5 @@ export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/lib make examples_c make run_examples_c ``` +### Interfaces installation +For the installation of Python/MATLAB interfaces, please refer to the [Interfaces](../interfaces/index.md) page. diff --git a/docs/interfaces/index.md b/docs/interfaces/index.md index 77678eaa0f..a90f580a40 100644 --- a/docs/interfaces/index.md +++ b/docs/interfaces/index.md @@ -57,10 +57,6 @@ The currently supported formulations reads as x(\cdot),\,u(\cdot), \, z(\cdot) \end{subarray}}{\min} &&\int_0^T l(x(\tau), u(\tau), z(\tau), p)\mathrm{d}\tau + m(x(T), z(T), p)\\ &\,\,\,\quad \text{s.t.} &&x(0) - \bar{x}_0 = 0, &&\\ - & &&\underline{h} \leq h(x(t), u(t), p) \leq \bar{h}, &&\quad t \in [0,\,T),\\ - & &&\underline{u}_0 \leq \Pi_{u_0}u(0) \leq \bar{u}_0, && \\ - & &&\underline{c}_0 \leq D_0u(0)\leq \bar{c}_0, && \\ - & && && \\[-1em] & &&F(x(t), \dot{x}(t), u(t), z(t), p) = 0, &&\quad t \in [0,\,T),\\ & &&\underline{h} \leq h(x(t), u(t), p) \leq \bar{h}, &&\quad t \in [0,\,T),\\ & &&\underline{x} \leq \Pi_{x}x(t) \leq \bar{x}, &&\quad t \in (0,\,T),\\ @@ -116,7 +112,7 @@ For more information contact `@zanellia` :undoc-members: ``` ``` eval_rst -.. automodule:: acados_template.ocp_nlp_render_arguments +.. automodule:: acados_template.acados_ocp_nlp :members: :private-members: :exclude-members: acados_ocp2json_layout, cast_ocp_nlp, dict2json_layout, dict2json_layout_rec, check_ra, json2dict_rec diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index 1d238cdb19..fd01b652a8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -472,44 +472,44 @@ class containing the description of the constraints """ def __init__(self): # bounds on x and u - self.__lbx = [] #: lower bounds on x - self.__lbu = [] #: lower bounds on u - self.__ubx = [] #: upper bounds on x - self.__ubu = [] #: upper bounds on u - self.__idxbx = [] #: indexes of bounds on x - self.__idxbu = [] #: indexes of bounds on u + self.__lbx = [] #: :math:`\underline{x}` - lower bounds on x + self.__lbu = [] #: :math:`\underline{u}` - lower bounds on u + self.__ubx = [] #: :math:`\bar{x}` - upper bounds on x + self.__ubu = [] #: :math:`\bar{u}` - upper bounds on u + self.__idxbx = [] #: indexes of bounds on x (defines :math: `\Pi_x`) + self.__idxbu = [] #: indexes of bounds on u (defines :math: `\Pi_u`) # bounds on x at t=T - self.__lbx_e = [] #: lower bounds on x at t=T - self.__ubx_e = [] #: upper bounds on x at t=T - self.__idxbx_e = [] #: indexes for bounds on x at t=T + self.__lbx_e = [] #: :math:`\underline{x}^e` - lower bounds on x at t=T + self.__ubx_e = [] #: :math:`\bar{x}^e` - upper bounds on x at t=T + self.__idxbx_e = [] #: indexes for bounds on x at t=T (defines :math: `\Pi_x^e`) # soft bounds on x and u - self.__lsbx = [] #: soft lower bounds on x - self.__lsbu = [] #: soft lower bounds on u - self.__usbx = [] #: soft upper bounds on x - self.__usbu = [] #: soft upper bounds on u - self.__idxsbx = [] #: indexes of soft bounds on x - self.__idxsbu = [] #: indexes of soft bounds on u + self.__lsbx = [] #: :math:`` - soft lower bounds on x + self.__lsbu = [] #: :math:`` - soft lower bounds on u + self.__usbx = [] #: :math:`` - soft upper bounds on x + self.__usbu = [] #: :math:`` - soft upper bounds on u + self.__idxsbx = [] #: :math:`` - indexes of soft bounds on x + self.__idxsbu = [] #: :math:`` - indexes of soft bounds on u # soft bounds on x and u at t=T - self.__lsbx_e = [] #: soft lower bounds on x at t=T - self.__usbx_e = [] #: soft upper bounds on x at t=T - self.__idxsbx_e= [] #: indexes of soft bounds on x at t=T + self.__lsbx_e = [] #: :math:`` - soft lower bounds on x at t=T + self.__usbx_e = [] #: :math:`` - soft upper bounds on x at t=T + self.__idxsbx_e= [] #: :math:`` - indexes of soft bounds on x at t=T # polytopic constraints - self.__lg = [] #: lower bound for general inequalities - self.__ug = [] #: upper bound for general inequalities - self.__D = [] #: D matrix in lg <= D * u + C * x <= ug - self.__C = [] #: C matrix in lg <= D * u + C * x <= ug + self.__lg = [] #: :math:`\underline{c}` - lower bound for general inequalities + self.__ug = [] #: :math:`\bar{c}` - upper bound for general inequalities + self.__D = [] #: :math:`D` - D matrix in lg <= D * u + C * x <= ug + self.__C = [] #: :math:`C` - C matrix in lg <= D * u + C * x <= ug # polytopic constraints at t=T - self.__C_e = [] #: C matrix at t=T - self.__lg_e = [] #: lower bound on general inequalities at t=T - self.__ug_e = [] #: upper bound on general inequalities at t=T + self.__C_e = [] #: :math:`C^e` - C matrix at t=T + self.__lg_e = [] #: :math:`\underline{c}^e` - lower bound on general inequalities at t=T + self.__ug_e = [] #: :math:`\bar{c}^e` - upper bound on general inequalities at t=T # nonlinear constraints - self.__lh = [] #: lower bound for nonlinear inequalities - self.__uh = [] #: upper bound for nonlinear inequalities + self.__lh = [] #: :math:`\underline{h}` - lower bound for nonlinear inequalities + self.__uh = [] #: :math:`\bar{h}` - upper bound for nonlinear inequalities # nonlinear constraints at t=T - self.__uh_e = [] #: upper bound on nonlinear inequalities at t=T - self.__lh_e = [] #: lower bound on nonlinear inequalities at t=T - self.__x0 = [] #: initial state - self.__p = [] #: parameters + self.__uh_e = [] #: :math:`\bar{h}^e` - upper bound on nonlinear inequalities at t=T + self.__lh_e = [] #: :math:`\underline{h}^e` - lower bound on nonlinear inequalities at t=T + self.__x0 = [] #: :math:`\bar{x}_0` - initial state + self.__p = [] #: :math:`p` - parameters @property def lbx(self): diff --git a/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c index 736e3cf0ce..74348bc1be 100644 --- a/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c @@ -853,9 +853,9 @@ int acados_create() { ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbu", lbu); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubu", ubu); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); - // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbx", idxsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lsbx", lsbx); + ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); {%- if ocp.dims.nsbu > 0 %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxsbu", idxsbu); From caa0ec82a01e8b61ec98eb487773ac16563a5a04 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 16:44:15 +0200 Subject: [PATCH 13/78] remove soft bounds from docs for now --- .../acados_template/acados_ocp_nlp.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index fd01b652a8..e1edd7a480 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -483,16 +483,16 @@ def __init__(self): self.__ubx_e = [] #: :math:`\bar{x}^e` - upper bounds on x at t=T self.__idxbx_e = [] #: indexes for bounds on x at t=T (defines :math: `\Pi_x^e`) # soft bounds on x and u - self.__lsbx = [] #: :math:`` - soft lower bounds on x - self.__lsbu = [] #: :math:`` - soft lower bounds on u - self.__usbx = [] #: :math:`` - soft upper bounds on x - self.__usbu = [] #: :math:`` - soft upper bounds on u - self.__idxsbx = [] #: :math:`` - indexes of soft bounds on x - self.__idxsbu = [] #: :math:`` - indexes of soft bounds on u + self.__lsbx = [] #: soft lower bounds on x + self.__lsbu = [] #: soft lower bounds on u + self.__usbx = [] #: soft upper bounds on x + self.__usbu = [] #: soft upper bounds on u + self.__idxsbx = [] #: indexes of soft bounds on x + self.__idxsbu = [] #: indexes of soft bounds on u # soft bounds on x and u at t=T - self.__lsbx_e = [] #: :math:`` - soft lower bounds on x at t=T - self.__usbx_e = [] #: :math:`` - soft upper bounds on x at t=T - self.__idxsbx_e= [] #: :math:`` - indexes of soft bounds on x at t=T + self.__lsbx_e = [] #: soft lower bounds on x at t=T + self.__usbx_e = [] #: soft upper bounds on x at t=T + self.__idxsbx_e= [] #: indexes of soft bounds on x at t=T # polytopic constraints self.__lg = [] #: :math:`\underline{c}` - lower bound for general inequalities self.__ug = [] #: :math:`\bar{c}` - upper bound for general inequalities From 28d7ee5791f46625136f671e1b00dca67ebc58d3 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 17:06:26 +0200 Subject: [PATCH 14/78] fix some typos --- docs/interfaces/index.md | 7 ++++--- .../acados_template/acados_template/acados_ocp_nlp.py | 6 +++--- 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/docs/interfaces/index.md b/docs/interfaces/index.md index a90f580a40..3642eeb02a 100644 --- a/docs/interfaces/index.md +++ b/docs/interfaces/index.md @@ -59,9 +59,9 @@ The currently supported formulations reads as &\,\,\,\quad \text{s.t.} &&x(0) - \bar{x}_0 = 0, &&\\ & &&F(x(t), \dot{x}(t), u(t), z(t), p) = 0, &&\quad t \in [0,\,T),\\ & &&\underline{h} \leq h(x(t), u(t), p) \leq \bar{h}, &&\quad t \in [0,\,T),\\ - & &&\underline{x} \leq \Pi_{x}x(t) \leq \bar{x}, &&\quad t \in (0,\,T),\\ - & &&\underline{u} \leq \Pi_{u}u(t) \leq \bar{u}, &&\quad t \in (0,\,T),\\ - & &&\underline{c} \leq Cx(t) + Du(t)\leq \bar{c}, &&\quad t \in (0,\,T), \\ + & &&\underline{x} \leq \Pi_{x}x(t) \leq \bar{x}, &&\quad t \in [0,\,T),\\ + & &&\underline{u} \leq \Pi_{u}u(t) \leq \bar{u}, &&\quad t \in [0,\,T),\\ + & &&\underline{c} \leq Cx(t) + Du(t)\leq \bar{c}, &&\quad t \in [0,\,T), \\ & && && \\[-1em] & &&\underline{h}^e \leq h^e(x(T), p) \leq \bar{h}^e, &&\\ & &&\underline{x}^e \leq \Pi_{x}^e x(T) \leq \bar{x}^{e}, &&\\ @@ -79,6 +79,7 @@ Where: * :math:`h: \mathbb{R}^{n_x}\times\mathbb{R}^{n_u}\times\mathbb{R}^{n_z}\times\mathbb{R}^{n_p} \rightarrow \mathbb{R}^{n_h}` and :math:`h^e: \mathbb{R}^{n_x}\times\mathbb{R}^{n_z}\times\mathbb{R}^{n_p} \rightarrow \mathbb{R}^{n_{h_e}}` are general nonlinear functions. +* :math:`C,\,D,\,C^e,\,\Pi_x,\,\Pi_u,\,\Pi_x^e` are matrices of appropriate dimensions defining the polytopic and box constraints. Currently not yet implemented features: diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index e1edd7a480..c5273a8aa8 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -476,12 +476,12 @@ def __init__(self): self.__lbu = [] #: :math:`\underline{u}` - lower bounds on u self.__ubx = [] #: :math:`\bar{x}` - upper bounds on x self.__ubu = [] #: :math:`\bar{u}` - upper bounds on u - self.__idxbx = [] #: indexes of bounds on x (defines :math: `\Pi_x`) - self.__idxbu = [] #: indexes of bounds on u (defines :math: `\Pi_u`) + self.__idxbx = [] #: indexes of bounds on x (defines :math:`\Pi_x`) + self.__idxbu = [] #: indexes of bounds on u (defines :math:`\Pi_u`) # bounds on x at t=T self.__lbx_e = [] #: :math:`\underline{x}^e` - lower bounds on x at t=T self.__ubx_e = [] #: :math:`\bar{x}^e` - upper bounds on x at t=T - self.__idxbx_e = [] #: indexes for bounds on x at t=T (defines :math: `\Pi_x^e`) + self.__idxbx_e = [] #: indexes for bounds on x at t=T (defines :math:`\Pi_x^e`) # soft bounds on x and u self.__lsbx = [] #: soft lower bounds on x self.__lsbu = [] #: soft lower bounds on u From bafa38e642fcb4be3d26fbe7006c433b6b34592b Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 22 May 2019 17:17:31 +0200 Subject: [PATCH 15/78] small rename of pcond_N in matlab interface --- acados/ocp_nlp/ocp_nlp_common.h | 6 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 6 + .../ocp_qp/ocp_qp_partial_condensing_solver.c | 31 ++- .../example_closed_loop.m | 4 +- .../linear_mass_spring_model/example_ocp.m | 4 +- .../masses_chain_model/example_closed_loop.m | 4 +- .../masses_chain_model/example_ocp.m | 4 +- .../pendulum_on_cart_model/example_ocp.m | 6 +- .../wind_turbine_nx6/example_closed_loop.m | 4 +- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 4 +- interfaces/acados_c/ocp_nlp_interface.c | 3 - interfaces/acados_matlab/acados_ocp_model.m | 258 ++---------------- interfaces/acados_matlab/acados_ocp_opts.m | 65 ++--- interfaces/acados_matlab/ocp_create.c | 26 +- 14 files changed, 104 insertions(+), 321 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.h b/acados/ocp_nlp/ocp_nlp_common.h index fa509b8c4c..1cf80066e5 100644 --- a/acados/ocp_nlp/ocp_nlp_common.h +++ b/acados/ocp_nlp/ocp_nlp_common.h @@ -70,9 +70,9 @@ typedef struct void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory); int (*workspace_calculate_size)(void *config, void *dims, void *opts_); void (*opts_set)(void *config_, void *opts_, const char *field, void* value); - void (*dynamics_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! - void (*cost_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! - void (*constraints_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // TODO rename !!! + void (*dynamics_opts_set)(void *config, void *opts, int stage, const char *field, void *value); + void (*cost_opts_set)(void *config, void *opts, int stage, const char *field, void *value); + void (*constraints_opts_set)(void *config, void *opts, int stage, const char *field, void *value); // evaluate solver int (*evaluate)(void *config, void *dims, void *qp_in, void *qp_out, void *opts_, void *mem, void *work); // prepare memory diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index ae76b60f81..eb033c65a9 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -58,6 +58,8 @@ int ocp_qp_partial_condensing_opts_calculate_size(ocp_qp_dims *dims) return size; } + + void *ocp_qp_partial_condensing_opts_assign(ocp_qp_dims *dims, void *raw_memory) { int N = dims->N; @@ -92,6 +94,8 @@ void *ocp_qp_partial_condensing_opts_assign(ocp_qp_dims *dims, void *raw_memory) return opts; } + + void ocp_qp_partial_condensing_opts_initialize_default(ocp_qp_dims *dims, void *opts_) { ocp_qp_partial_condensing_opts *opts = opts_; @@ -106,6 +110,8 @@ void ocp_qp_partial_condensing_opts_initialize_default(ocp_qp_dims *dims, void * d_set_default_cond_qp_ocp2ocp_arg(opts->N2, opts->hpipm_opts); } + + void ocp_qp_partial_condensing_opts_update(ocp_qp_dims *dims, void *opts_) { ocp_qp_partial_condensing_opts *opts = opts_; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 148b09ffb1..86ffc1391d 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -30,6 +30,8 @@ #include "acados/utils/timing.h" #include "acados/utils/types.h" + + /************************************************ * opts ************************************************/ @@ -48,6 +50,8 @@ int ocp_qp_partial_condensing_solver_opts_calculate_size(void *config_, ocp_qp_d return size; } + + void *ocp_qp_partial_condensing_solver_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory) { @@ -76,6 +80,8 @@ void *ocp_qp_partial_condensing_solver_opts_assign(void *config_, ocp_qp_dims *d return (void *) opts; } + + void ocp_qp_partial_condensing_solver_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_) { @@ -92,6 +98,8 @@ void ocp_qp_partial_condensing_solver_opts_initialize_default(void *config_, ocp qp_solver->opts_initialize_default(qp_solver, dims, opts->qp_solver_opts); } + + void ocp_qp_partial_condensing_solver_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) { ocp_qp_xcond_solver_config *config = config_; @@ -108,10 +116,13 @@ void ocp_qp_partial_condensing_solver_opts_update(void *config_, ocp_qp_dims *di } + void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value) { ocp_qp_partial_condensing_solver_opts *opts = (ocp_qp_partial_condensing_solver_opts *) opts_; - // ocp_qp_xcond_solver_config *config = config_; + ocp_qp_xcond_solver_config *config = config_; + + // TODO extract module name 'pcond' for partial condensing optsion if (!strcmp(field, "N2") || !strcmp(field, "pcond_N2")) { @@ -123,15 +134,21 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const int* N2_bkp = (int *) value; opts->pcond_opts->N2_bkp = *N2_bkp; } - else - { - // TODO pass options to qp solver - printf("\nerror: option type %s not available in ocp_qp_partial_condense solver module\n", field); - exit(1); - } + else //if(!strcmp(field, "ric_alg")) + { + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + } +// else +// { +// // TODO pass options to qp solver +// printf("\nerror: option type %s not available in ocp_qp_partial_condense solver module\n", field); +// exit(1); +// } + return; } + /************************************************ * memory ************************************************/ diff --git a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m index be4587af52..7332f2a8ee 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m @@ -16,7 +16,7 @@ %ocp_nlp_solver = 'sqp_rti'; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_N_pcond = 5; +ocp_qp_solver_pcond_N = 5; ocp_sim_method = 'erk'; %ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 2; @@ -128,7 +128,7 @@ ocp_opts.set('nlp_solver', ocp_nlp_solver); ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', ocp_qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index afc5fe30d3..7409d4254e 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -20,7 +20,7 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_N_pcond = 5; +qp_solver_pcond_N = 5; %dyn_type = 'explicit'; %dyn_type = 'implicit'; dyn_type = 'discrete'; @@ -201,7 +201,7 @@ ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); end if (strcmp(dyn_type, 'explicit')) ocp_opts.set('sim_method', 'erk'); diff --git a/examples/matlab_mex/masses_chain_model/example_closed_loop.m b/examples/matlab_mex/masses_chain_model/example_closed_loop.m index 017aafd207..4c95bcfbf0 100644 --- a/examples/matlab_mex/masses_chain_model/example_closed_loop.m +++ b/examples/matlab_mex/masses_chain_model/example_closed_loop.m @@ -24,7 +24,7 @@ nlp_solver_max_iter = 100; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_N_pcond = 5; +ocp_qp_solver_pcond_N = 5; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4; @@ -160,7 +160,7 @@ ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', ocp_qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 2c95f64a8d..1aa658f2d7 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -20,7 +20,7 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_N_pcond = 5; +qp_solver_pcond_N = 5; %dyn_type = 'explicit'; dyn_type = 'implicit'; %dyn_type = 'discrete'; @@ -170,7 +170,7 @@ ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); end if (strcmp(dyn_type, 'explicit')) ocp_opts.set('sim_method', 'erk'); diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 4a622d7bd5..7343a80027 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -20,7 +20,8 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_N_pcond = 5; +qp_solver_pcond_N = 5; +qp_solver_ric_alg = 0; %sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; @@ -171,7 +172,8 @@ ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); end ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); diff --git a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m index 150cb71410..9203408e64 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m @@ -18,7 +18,7 @@ ocp_nlp_solver = 'sqp_rti'; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_N_pcond = 5; +ocp_qp_solver_pcond_N = 5; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4; @@ -215,7 +215,7 @@ ocp_opts.set('nlp_solver', ocp_nlp_solver); ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', ocp_qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 17a300b6ce..cc50f2f06f 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -12,7 +12,7 @@ %nlp_solver = 'sqp_rti'; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_N_pcond = 5; +qp_solver_pcond_N = 5; % sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; @@ -209,7 +209,7 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_N_pcond', qp_solver_N_pcond); + ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); end ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 42eb716d06..f7eb0af7f6 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -527,7 +527,6 @@ void ocp_nlp_opts_set(ocp_nlp_config *config, void *opts_, const char *field, vo -// TODO rename !!! void ocp_nlp_dynamics_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { @@ -537,7 +536,6 @@ void ocp_nlp_dynamics_opts_set(ocp_nlp_config *config, void *opts_, int stage, -// TODO rename !!! void ocp_nlp_cost_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { @@ -547,7 +545,6 @@ void ocp_nlp_cost_opts_set(ocp_nlp_config *config, void *opts_, int stage, -// TODO rename !!! void ocp_nlp_constraints_opts_set(ocp_nlp_config *config, void *opts_, int stage, const char *field, void *value) { diff --git a/interfaces/acados_matlab/acados_ocp_model.m b/interfaces/acados_matlab/acados_ocp_model.m index 04d67d7f80..28fafe620f 100644 --- a/interfaces/acados_matlab/acados_ocp_model.m +++ b/interfaces/acados_matlab/acados_ocp_model.m @@ -3,115 +3,6 @@ properties - name - % dims - T - dim_nx - dim_nu - dim_nz - dim_ny - dim_ny_e - dim_nbx - dim_nbu - dim_ng - dim_ng_e - dim_nh - dim_nh_e - dim_ns - dim_ns_e - dim_nsbu - dim_nsbx - dim_nsg - dim_nsg_e - dim_nsh - dim_nsh_e - dim_np -% dim_np_e - % symbolics - sym_x - sym_u - sym_xdot - sym_z - sym_p - % cost - cost_type - cost_type_e - cost_expr_y - cost_param_y - cost_expr_y_e - cost_param_y_e - cost_expr_ext_cost - cost_param_ext_cost - cost_expr_ext_cost_e - cost_param_ext_cost_e - cost_Vu - cost_Vx - cost_Vx_e - cost_W - cost_W_e - cost_yr - cost_yr_e - cost_Z - cost_Z_e - cost_Zl - cost_Zl_e - cost_Zu - cost_Zu_e - cost_z - cost_z_e - cost_zl - cost_zl_e - cost_zu - cost_zu_e - % dynamics - dyn_type - dyn_expr_f - dyn_param_f - dyn_expr_phi - dyn_param_phi - % constraints - constr_type - constr_x0 - constr_Jbx - constr_lbx - constr_ubx - constr_Jbu - constr_lbu - constr_ubu - constr_C - constr_D - constr_lg - constr_ug - constr_C_e - constr_lg_e - constr_ug_e - constr_expr_h - constr_param_h - constr_lh - constr_uh - constr_expr_h_e - constr_param_h_e - constr_lh_e - constr_uh_e - constr_Jsbu -% constr_lsbu -% constr_usbu - constr_Jsbx -% constr_lsbx -% constr_usbx - constr_Jsg -% constr_lsg -% constr_usg - constr_Jsg_e -% constr_lsg_e -% constr_usg_e - constr_Jsh -% constr_lsh -% constr_ush - constr_Jsh_e -% constr_lsh_e -% constr_ush_e - % structure model_struct end %properties @@ -121,36 +12,22 @@ function obj = acados_ocp_model() - % default values - obj.name = 'ocp_model'; - obj.cost_type = 'linear_ls'; - obj.cost_type_e = 'linear_ls'; - obj.cost_param_y = 'false'; - obj.cost_param_y_e = 'false'; - obj.cost_param_ext_cost = 'false'; - obj.cost_param_ext_cost_e = 'false'; - obj.dyn_type = 'implicit'; - obj.dyn_param_f = 'false'; - obj.dyn_param_phi = 'false'; - obj.constr_type = 'bgh'; - obj.constr_param_h = 'false'; - obj.constr_param_h_e = 'false'; % model structure obj.model_struct = struct; - % initialize model struct - obj.model_struct.name = obj.name; - obj.model_struct.cost_type = obj.cost_type; - obj.model_struct.cost_type_e = obj.cost_type_e; - obj.model_struct.cost_param_y = obj.cost_param_y; - obj.model_struct.cost_param_y_e = obj.cost_param_y_e; - obj.model_struct.cost_param_ext_cost = obj.cost_param_ext_cost; - obj.model_struct.cost_param_ext_cost_e = obj.cost_param_ext_cost_e; - obj.model_struct.dyn_type = obj.dyn_type; - obj.model_struct.dyn_param_f = obj.dyn_param_f; - obj.model_struct.dyn_param_phi = obj.dyn_param_phi; - obj.model_struct.constr_type = obj.constr_type; - obj.model_struct.constr_param_h = obj.constr_param_h; - obj.model_struct.constr_param_h_e = obj.constr_param_h_e; + % default values + obj.model_struct.name = 'ocp_model'; + obj.model_struct.cost_type = 'linear_ls'; + obj.model_struct.cost_type_e = 'linear_ls'; + obj.model_struct.cost_param_y = 'false'; + obj.model_struct.cost_param_y_e = 'false'; + obj.model_struct.cost_param_ext_cost = 'false'; + obj.model_struct.cost_param_ext_cost_e = 'false'; + obj.model_struct.dyn_type = 'implicit'; + obj.model_struct.dyn_param_f = 'false'; + obj.model_struct.dyn_param_phi = 'false'; + obj.model_struct.constr_type = 'bgh'; + obj.model_struct.constr_param_h = 'false'; + obj.model_struct.constr_param_h_e = 'false'; end @@ -164,346 +41,251 @@ if (strcmp(tokens{1}, 'sym')) if (strcmp(field, 'sym_x')) - obj.sym_x = value; obj.model_struct.sym_x = value; elseif (strcmp(field, 'sym_xdot')) - obj.sym_xdot = value; obj.model_struct.sym_xdot = value; elseif (strcmp(field, 'sym_u')) - obj.sym_u = value; obj.model_struct.sym_u = value; elseif (strcmp(field, 'sym_z')) - obj.sym_z = value; obj.model_struct.sym_z = value; elseif (strcmp(field, 'sym_p')) - obj.sym_p = value; obj.model_struct.sym_p = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end % cost elseif (strcmp(tokens{1}, 'cost')) if (strcmp(field, 'cost_type')) - obj.cost_type = value; obj.model_struct.cost_type = value; elseif (strcmp(field, 'cost_type_e')) - obj.cost_type_e = value; obj.model_struct.cost_type_e = value; elseif (strcmp(field, 'cost_expr_y')) - obj.cost_expr_y = value; obj.model_struct.cost_expr_y = value; elseif (strcmp(field, 'cost_param_y')) - obj.cost_param_y = value; obj.model_struct.cost_param_y = value; elseif (strcmp(field, 'cost_expr_y_e')) - obj.cost_expr_y_e = value; obj.model_struct.cost_expr_y_e = value; elseif (strcmp(field, 'cost_param_y_e')) - obj.cost_param_y_e = value; obj.model_struct.cost_param_y_e = value; elseif (strcmp(field, 'cost_expr_ext_cost')) - obj.cost_expr_ext_cost = value; obj.model_struct.cost_expr_ext_cost = value; elseif (strcmp(field, 'cost_param_ext_cost')) - obj.cost_param_ext_cost = value; obj.model_struct.cost_param_ext_cost = value; elseif (strcmp(field, 'cost_expr_ext_cost_e')) - obj.cost_expr_ext_cost_e = value; obj.model_struct.cost_expr_ext_cost_e = value; elseif (strcmp(field, 'cost_param_ext_cost_e')) - obj.cost_param_ext_cost_e = value; obj.model_struct.cost_param_ext_cost_e = value; elseif (strcmp(field, 'cost_Vu')) - obj.cost_Vu = value; obj.model_struct.cost_Vu = value; elseif (strcmp(field, 'cost_Vx')) - obj.cost_Vx = value; obj.model_struct.cost_Vx = value; elseif (strcmp(field, 'cost_Vx_e')) - obj.cost_Vx_e = value; obj.model_struct.cost_Vx_e = value; elseif (strcmp(field, 'cost_W')) - obj.cost_W = value; obj.model_struct.cost_W = value; elseif (strcmp(field, 'cost_W_e')) - obj.cost_W_e = value; obj.model_struct.cost_W_e = value; elseif (strcmp(field, 'cost_yr')) - obj.cost_yr = value; obj.model_struct.cost_yr = value; elseif (strcmp(field, 'cost_yr_e')) - obj.cost_yr_e = value; obj.model_struct.cost_yr_e = value; elseif (strcmp(field, 'cost_Z')) - obj.cost_Z = value; obj.model_struct.cost_Z = value; elseif (strcmp(field, 'cost_Z_e')) - obj.cost_Z_e = value; obj.model_struct.cost_Z_e = value; elseif (strcmp(field, 'cost_Zl')) - obj.cost_Zl = value; obj.model_struct.cost_Zl = value; elseif (strcmp(field, 'cost_Zl_e')) - obj.cost_Zl_e = value; obj.model_struct.cost_Zl_e = value; elseif (strcmp(field, 'cost_Zu')) - obj.cost_Zu = value; obj.model_struct.cost_Zu = value; elseif (strcmp(field, 'cost_Zu_e')) - obj.cost_Zu_e = value; obj.model_struct.cost_Zu_e = value; elseif (strcmp(field, 'cost_zl')) - obj.cost_zl = value; obj.model_struct.cost_zl = value; elseif (strcmp(field, 'cost_zl_e')) - obj.cost_zl_e = value; obj.model_struct.cost_zl_e = value; elseif (strcmp(field, 'cost_z')) - obj.cost_z = value; obj.model_struct.cost_z = value; elseif (strcmp(field, 'cost_z_e')) - obj.cost_z_e = value; obj.model_struct.cost_z_e = value; elseif (strcmp(field, 'cost_zu')) - obj.cost_zu = value; obj.model_struct.cost_zu = value; elseif (strcmp(field, 'cost_zu_e')) - obj.cost_zu_e = value; obj.model_struct.cost_zu_e = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end % dynamics elseif (strcmp(tokens{1}, 'dyn')) if (strcmp(field, 'dyn_type')) - obj.dyn_type = value; obj.model_struct.dyn_type = value; elseif (strcmp(field, 'dyn_expr_f')) - obj.dyn_expr_f = value; obj.model_struct.dyn_expr_f = value; elseif (strcmp(field, 'dyn_param_f')) - obj.dyn_param_f = value; obj.model_struct.dyn_param_f = value; elseif (strcmp(field, 'dyn_expr_phi')) - obj.dyn_expr_phi = value; obj.model_struct.dyn_expr_phi = value; elseif (strcmp(field, 'dyn_param_phi')) - obj.dyn_param_phi = value; obj.model_struct.dyn_param_phi = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end % constraints elseif (strcmp(tokens{1}, 'constr')) if (strcmp(field, 'constr_type')) - obj.constr_type = value; obj.model_struct.constr_type = value; elseif (strcmp(field, 'constr_x0')) - obj.constr_x0 = value; obj.model_struct.constr_x0 = value; elseif (strcmp(field, 'constr_Jbx')) - obj.constr_Jbx = value; obj.model_struct.constr_Jbx = value; elseif (strcmp(field, 'constr_lbx')) - obj.constr_lbx = value; obj.model_struct.constr_lbx = value; elseif (strcmp(field, 'constr_ubx')) - obj.constr_ubx = value; obj.model_struct.constr_ubx = value; elseif (strcmp(field, 'constr_Jbu')) - obj.constr_Jbu = value; obj.model_struct.constr_Jbu = value; elseif (strcmp(field, 'constr_lbu')) - obj.constr_lbu = value; obj.model_struct.constr_lbu = value; elseif (strcmp(field, 'constr_ubu')) - obj.constr_ubu = value; obj.model_struct.constr_ubu = value; elseif (strcmp(field, 'constr_C')) - obj.constr_C = value; obj.model_struct.constr_C = value; elseif (strcmp(field, 'constr_D')) - obj.constr_D = value; obj.model_struct.constr_D = value; elseif (strcmp(field, 'constr_lg')) - obj.constr_lg = value; obj.model_struct.constr_lg = value; elseif (strcmp(field, 'constr_ug')) - obj.constr_ug = value; obj.model_struct.constr_ug = value; elseif (strcmp(field, 'constr_C_e')) - obj.constr_C_e = value; obj.model_struct.constr_C_e = value; elseif (strcmp(field, 'constr_lg_e')) - obj.constr_lg_e = value; obj.model_struct.constr_lg_e = value; elseif (strcmp(field, 'constr_ug_e')) - obj.constr_ug_e = value; obj.model_struct.constr_ug_e = value; elseif (strcmp(field, 'constr_expr_h')) - obj.constr_expr_h = value; obj.model_struct.constr_expr_h = value; elseif (strcmp(field, 'constr_param_h')) - obj.constr_param_h = value; obj.model_struct.constr_param_h = value; elseif (strcmp(field, 'constr_lh')) - obj.constr_lh = value; obj.model_struct.constr_lh = value; elseif (strcmp(field, 'constr_uh')) - obj.constr_uh = value; obj.model_struct.constr_uh = value; elseif (strcmp(field, 'constr_expr_h_e')) - obj.constr_expr_h_e = value; obj.model_struct.constr_expr_h_e = value; elseif (strcmp(field, 'constr_param_h_e')) - obj.constr_param_h_e = value; obj.model_struct.constr_param_h_e = value; elseif (strcmp(field, 'constr_lh_e')) - obj.constr_lh_e = value; obj.model_struct.constr_lh_e = value; elseif (strcmp(field, 'constr_uh_e')) - obj.constr_uh_e = value; obj.model_struct.constr_uh_e = value; elseif (strcmp(field, 'constr_Jsbu')) - obj.constr_Jsbu = value; obj.model_struct.constr_Jsbu = value; % elseif (strcmp(field, 'constr_lsbu')) - % obj.constr_lsbu = value; % obj.model_struct.constr_lsbu = value; % elseif (strcmp(field, 'constr_usbu')) - % obj.constr_usbu = value; % obj.model_struct.constr_usbu = value; elseif (strcmp(field, 'constr_Jsbx')) - obj.constr_Jsbx = value; obj.model_struct.constr_Jsbx = value; % elseif (strcmp(field, 'constr_lsbx')) - % obj.constr_lsbx = value; % obj.model_struct.constr_lsbx = value; % elseif (strcmp(field, 'constr_usbx')) - % obj.constr_usbx = value; % obj.model_struct.constr_usbx = value; elseif (strcmp(field, 'constr_Jsg')) - obj.constr_Jsg = value; obj.model_struct.constr_Jsg = value; % elseif (strcmp(field, 'constr_lsg')) - % obj.constr_lsg = value; % obj.model_struct.constr_lsg = value; % elseif (strcmp(field, 'constr_usg')) - % obj.constr_usg = value; % obj.model_struct.constr_usg = value; elseif (strcmp(field, 'constr_Jsg_e')) - obj.constr_Jsg_e = value; obj.model_struct.constr_Jsg_e = value; % elseif (strcmp(field, 'constr_lsg_e')) - % obj.constr_lsg_e = value; % obj.model_struct.constr_lsg_e = value; % elseif (strcmp(field, 'constr_usg_e')) - % obj.constr_usg_e = value; % obj.model_struct.constr_usg_e = value; elseif (strcmp(field, 'constr_Jsh')) - obj.constr_Jsh = value; obj.model_struct.constr_Jsh = value; % elseif (strcmp(field, 'constr_lsh')) - % obj.constr_lsh = value; % obj.model_struct.constr_lsh = value; % elseif (strcmp(field, 'constr_ush')) - % obj.constr_ush = value; % obj.model_struct.constr_ush = value; elseif (strcmp(field, 'constr_Jsh_e')) - obj.constr_Jsh_e = value; obj.model_struct.constr_Jsh_e = value; % elseif (strcmp(field, 'constr_lsh_e')) - % obj.constr_lsh_e = value; % obj.model_struct.constr_lsh_e = value; % elseif (strcmp(field, 'constr_ush_e')) - % obj.constr_ush_e = value; % obj.model_struct.constr_ush_e = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end % dims elseif (strcmp(tokens{1}, 'dim')) if (strcmp(field, 'dim_nx')) - obj.dim_nx = value; obj.model_struct.dim_nx = value; elseif (strcmp(field, 'dim_nu')) - obj.dim_nu = value; obj.model_struct.dim_nu = value; elseif (strcmp(field, 'dim_nz')) - obj.dim_nz = value; obj.model_struct.dim_nz = value; elseif (strcmp(field, 'dim_ny')) - obj.dim_ny = value; obj.model_struct.dim_ny = value; elseif (strcmp(field, 'dim_ny_e')) - obj.dim_ny_e = value; obj.model_struct.dim_ny_e = value; elseif (strcmp(field, 'dim_nbx')) - obj.dim_nbx = value; obj.model_struct.dim_nbx = value; elseif (strcmp(field, 'dim_nbu')) - obj.dim_nbu = value; obj.model_struct.dim_nbu = value; elseif (strcmp(field, 'dim_ng')) - obj.dim_ng = value; obj.model_struct.dim_ng = value; elseif (strcmp(field, 'dim_ng_e')) - obj.dim_ng_e = value; obj.model_struct.dim_ng_e = value; elseif (strcmp(field, 'dim_nh')) - obj.dim_nh = value; obj.model_struct.dim_nh = value; elseif (strcmp(field, 'dim_nh_e')) - obj.dim_nh_e = value; obj.model_struct.dim_nh_e = value; elseif (strcmp(field, 'dim_ns')) - obj.dim_ns = value; obj.model_struct.dim_ns = value; elseif (strcmp(field, 'dim_ns_e')) - obj.dim_ns_e = value; obj.model_struct.dim_ns_e = value; elseif (strcmp(field, 'dim_nsbu')) - obj.dim_nsbu = value; obj.model_struct.dim_nsbu = value; elseif (strcmp(field, 'dim_nsbx')) - obj.dim_nsbx = value; obj.model_struct.dim_nsbx = value; elseif (strcmp(field, 'dim_nsg')) - obj.dim_nsg = value; obj.model_struct.dim_nsg = value; elseif (strcmp(field, 'dim_nsg_e')) - obj.dim_nsg_e = value; obj.model_struct.dim_nsg_e = value; elseif (strcmp(field, 'dim_nsh')) - obj.dim_nsh = value; obj.model_struct.dim_nsh = value; elseif (strcmp(field, 'dim_nsh_e')) - obj.dim_nsh_e = value; obj.model_struct.dim_nsh_e = value; elseif (strcmp(field, 'dim_np')) - obj.dim_np = value; obj.model_struct.dim_np = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end % others else if (strcmp(field, 'T')) - obj.T = value; obj.model_struct.T = value; else disp(['acados_ocp_model: set: wrong field: ', field]); + keyboard; end end end diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 6245773864..76a6c152bf 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -3,19 +3,6 @@ properties - compile_mex - codgen_model - param_scheme - param_scheme_N - nlp_solver - nlp_solver_exact_hessian - nlp_solver_max_iter - qp_solver - qp_solver_N_pcond - sim_method - sim_method_num_stages - sim_method_num_steps - regularize_method opts_struct end % properties @@ -25,73 +12,53 @@ function obj = acados_ocp_opts() - % default values - obj.compile_mex = 'true'; - obj.codgen_model = 'true'; - obj.param_scheme = 'multiple_shooting_unif_grid'; - obj.param_scheme_N = 10; - obj.nlp_solver = 'sqp'; - obj.nlp_solver_exact_hessian = 'false'; - obj.qp_solver = 'qp_solver'; - obj.sim_method = 'irk'; - obj.regularize_method = 'no_regularize'; % model stuct obj.opts_struct = struct; - % initialize model stuct - obj.opts_struct.compile_mex = obj.compile_mex; - obj.opts_struct.codgen_model = obj.codgen_model; - obj.opts_struct.param_scheme = obj.param_scheme; - obj.opts_struct.param_scheme_N = obj.param_scheme_N; - obj.opts_struct.qp_solver = obj.qp_solver; - obj.opts_struct.nlp_solver = obj.nlp_solver; - obj.opts_struct.nlp_solver_exact_hessian = obj.nlp_solver; - obj.opts_struct.sim_method = obj.sim_method; - obj.opts_struct.regularize_method = obj.regularize_method; + % default values + obj.opts_struct.compile_mex = 'true'; + obj.opts_struct.codgen_model = 'true'; + obj.opts_struct.param_scheme = 'multiple_shooting_unif_grid'; + obj.opts_struct.param_scheme_N = 10; + obj.opts_struct.nlp_solver = 'sqp'; + obj.opts_struct.nlp_solver_exact_hessian = 'false'; + obj.opts_struct.qp_solver = 'partial_condensing_hpipm'; + obj.opts_struct.sim_method = 'irk'; + obj.opts_struct.regularize_method = 'no_regularize'; end function obj = set(obj, field, value) if (strcmp(field, 'compile_mex')) - obj.compile_mex = value; obj.opts_struct.compile_mex = value; elseif (strcmp(field, 'codgen_model')) - obj.codgen_model = value; obj.opts_struct.codgen_model = value; elseif (strcmp(field, 'param_scheme')) - obj.param_scheme = value; obj.opts_struct.param_scheme = value; elseif (strcmp(field, 'param_scheme_N')) - obj.param_scheme_N = value; obj.opts_struct.param_scheme_N = value; elseif (strcmp(field, 'nlp_solver')) - obj.nlp_solver = value; obj.opts_struct.nlp_solver = value; elseif (strcmp(field, 'nlp_solver_exact_hessian')) - obj.nlp_solver_exact_hessian = value; obj.opts_struct.nlp_solver_exact_hessian = value; elseif (strcmp(field, 'nlp_solver_max_iter')) - obj.nlp_solver_max_iter = value; obj.opts_struct.nlp_solver_max_iter = value; elseif (strcmp(field, 'qp_solver')) - obj.qp_solver = value; obj.opts_struct.qp_solver = value; - elseif (strcmp(field, 'qp_solver_N_pcond')) - obj.qp_solver_N_pcond = value; - obj.opts_struct.qp_solver_N_pcond = value; + elseif (strcmp(field, 'qp_solver_pcond_N')) + obj.opts_struct.qp_solver_pcond_N = value; + elseif (strcmp(field, 'qp_solver_ric_alg')) + obj.opts_struct.qp_solver_ric_alg = value; elseif (strcmp(field, 'sim_method')) - obj.sim_method = value; obj.opts_struct.sim_method = value; elseif (strcmp(field, 'sim_method_num_stages')) - obj.sim_method_num_stages = value; obj.opts_struct.sim_method_num_stages = value; elseif (strcmp(field, 'sim_method_num_steps')) - obj.sim_method_num_steps = value; obj.opts_struct.sim_method_num_steps = value; elseif (strcmp(field, 'regularize_method')) - obj.regularize_method = value; obj.opts_struct.regularize_method = value; else - disp('acados_ocp_opts: set: wrong field'); + disp(['acados_ocp_opts: set: wrong field: ', field]); + keyboard; end end diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 22a579678e..54c38210a4 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -620,7 +620,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) bool nlp_solver_exact_hessian; int nlp_solver_max_iter; bool set_nlp_solver_max_iter = false; char *qp_solver; - int qp_solver_N_pcond; bool set_qp_solver_N_pcond = false; + int qp_solver_pcond_N; bool set_qp_solver_pcond_N = false; + int qp_solver_ric_alg; bool set_qp_solver_ric_alg = false; char *sim_method; int sim_method_num_stages; bool set_sim_method_num_stages = false; int sim_method_num_steps; bool set_sim_method_num_steps = false; @@ -657,10 +658,16 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // TODO check qp_solver = mxArrayToString( mxGetField( prhs[1], 0, "qp_solver" ) ); // N_part_cond - if(mxGetField( prhs[1], 0, "qp_solver_N_pcond" )!=NULL) + if(mxGetField( prhs[1], 0, "qp_solver_pcond_N" )!=NULL) { - set_qp_solver_N_pcond = true; - qp_solver_N_pcond = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_N_pcond" ) ); + set_qp_solver_pcond_N = true; + qp_solver_pcond_N = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_pcond_N" ) ); + } + // hpipm: riccati algorithm + if(mxGetField( prhs[1], 0, "qp_solver_ric_alg" )!=NULL) + { + set_qp_solver_ric_alg = true; + qp_solver_ric_alg = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_ric_alg" ) ); } // sim_method // TODO check @@ -1046,10 +1053,15 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "max_iter", &nlp_solver_max_iter); } - // qp_solver_N_pcond - if(set_qp_solver_N_pcond) + // qp_solver_pcond_N + if(set_qp_solver_pcond_N) + { + ocp_nlp_opts_set(config, opts, "qp_pcond_N2", &qp_solver_pcond_N); + } + // qp_solver_ric_alg + if(set_qp_solver_ric_alg) { - ocp_nlp_opts_set(config, opts, "qp_pcond_N2", &qp_solver_N_pcond); + ocp_nlp_opts_set(config, opts, "qp_ric_alg", &qp_solver_ric_alg); } // sim_method_num_stages if(set_sim_method_num_stages) From c2eec25e47a433e1309048968bf6263e255f5550 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 22 May 2019 17:39:08 +0200 Subject: [PATCH 16/78] general constraints -> general polytopic constraints --- .../acados_template/acados_ocp_nlp.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index c5273a8aa8..bdb79d2695 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -26,8 +26,8 @@ def __init__(self): self.__nsbu = 0 #: :math:`n_{{sb}_u}` - number of soft input bounds self.__ns = 0 #: :math:`n_{s}` - total number of slacks self.__ns_e = 0 #: :math:`n_{s}^e` - total number of slacks at t=T - self.__ng = 0 #: :math:`n_{g}` - number of general constraints - self.__ng_e = 0 #: :math:`n_{g}^e` - number of general constraints at t=T + self.__ng = 0 #: :math:`n_{g}` - number of general polytopic constraints + self.__ng_e = 0 #: :math:`n_{g}^e` - number of general polytopic constraints at t=T self.__N = None #: :math:`N` - prediction horizon @property @@ -494,14 +494,14 @@ def __init__(self): self.__usbx_e = [] #: soft upper bounds on x at t=T self.__idxsbx_e= [] #: indexes of soft bounds on x at t=T # polytopic constraints - self.__lg = [] #: :math:`\underline{c}` - lower bound for general inequalities - self.__ug = [] #: :math:`\bar{c}` - upper bound for general inequalities + self.__lg = [] #: :math:`\underline{c}` - lower bound for general polytopic inequalities + self.__ug = [] #: :math:`\bar{c}` - upper bound for general polytopic inequalities self.__D = [] #: :math:`D` - D matrix in lg <= D * u + C * x <= ug self.__C = [] #: :math:`C` - C matrix in lg <= D * u + C * x <= ug # polytopic constraints at t=T self.__C_e = [] #: :math:`C^e` - C matrix at t=T - self.__lg_e = [] #: :math:`\underline{c}^e` - lower bound on general inequalities at t=T - self.__ug_e = [] #: :math:`\bar{c}^e` - upper bound on general inequalities at t=T + self.__lg_e = [] #: :math:`\underline{c}^e` - lower bound on general polytopic inequalities at t=T + self.__ug_e = [] #: :math:`\bar{c}^e` - upper bound on general polytopic inequalities at t=T # nonlinear constraints self.__lh = [] #: :math:`\underline{h}` - lower bound for nonlinear inequalities self.__uh = [] #: :math:`\bar{h}` - upper bound for nonlinear inequalities From 7138f5c25e0422fab535c96bd3de7d42c92c4435 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 23 May 2019 17:57:44 +0200 Subject: [PATCH 17/78] add ocp_qp_partial_condensing_opts_set --- acados/ocp_qp/ocp_qp_common.h | 5 +- acados/ocp_qp/ocp_qp_partial_condensing.c | 57 ++++++++++++++++++- acados/ocp_qp/ocp_qp_partial_condensing.h | 10 ++++ .../ocp_qp/ocp_qp_partial_condensing_solver.c | 42 ++++++++------ .../pendulum_on_cart_model/example_ocp.m | 2 + interfaces/acados_c/options_interface.c | 10 ++-- interfaces/acados_matlab/acados_ocp_opts.m | 2 + interfaces/acados_matlab/ocp_create.c | 12 ++++ 8 files changed, 116 insertions(+), 24 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_common.h b/acados/ocp_qp/ocp_qp_common.h index 646e5ba3da..fd10090171 100644 --- a/acados/ocp_qp/ocp_qp_common.h +++ b/acados/ocp_qp/ocp_qp_common.h @@ -66,7 +66,7 @@ typedef struct void *(*opts_assign)(ocp_qp_dims *dims, void *raw_memory); void (*opts_initialize_default)(ocp_qp_dims *dims, void *opts); void (*opts_update)(ocp_qp_dims *dims, void *opts); - void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + void (*opts_set)(void *opts_, const char *field, void* value); int (*memory_calculate_size)(ocp_qp_dims *dims, void *opts); void *(*memory_assign)(ocp_qp_dims *dims, void *opts, void *raw_memory); int (*workspace_calculate_size)(ocp_qp_dims *dims, void *opts); @@ -87,8 +87,10 @@ typedef struct void *(*memory_assign)(void *config, ocp_qp_dims *dims, void *opts, void *raw_memory); int (*workspace_calculate_size)(void *config, ocp_qp_dims *dims, void *opts); qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver + // TODO condensing config } ocp_qp_xcond_solver_config; // pcond - partial condensing or fcond - full condensing + /// Struct containing metrics of the qp solver. typedef struct { @@ -100,6 +102,7 @@ typedef struct int t_computed; } ocp_qp_info; + /* config */ // int ocp_qp_solver_config_calculate_size(); diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index eb033c65a9..86cd469380 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -18,7 +18,9 @@ */ // external +#include #include +#include // acados #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/ocp_qp/ocp_qp_partial_condensing.h" @@ -108,6 +110,8 @@ void ocp_qp_partial_condensing_opts_initialize_default(ocp_qp_dims *dims, void * opts->pcond_dims->N = opts->N2; // hpipm_opts d_set_default_cond_qp_ocp2ocp_arg(opts->N2, opts->hpipm_opts); + + return; } @@ -120,8 +124,44 @@ void ocp_qp_partial_condensing_opts_update(ocp_qp_dims *dims, void *opts_) opts->N2_bkp = opts->N2; // hpipm_opts d_set_default_cond_qp_ocp2ocp_arg(opts->N2, opts->hpipm_opts); + + return; } + + +void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value) +{ + + ocp_qp_partial_condensing_opts *opts = opts_; + + if(!strcmp(field, "N2")) + { + int *tmp_ptr = value; + opts->N2 = *tmp_ptr; + } + else if(!strcmp(field, "N2_bkp")) + { + int *tmp_ptr = value; + opts->N2_bkp = *tmp_ptr; + } + else if(!strcmp(field, "ric_alg")) + { + int *tmp_ptr = value; + d_set_cond_qp_ocp2ocp_arg_ric_alg(*tmp_ptr, opts->N2, opts->hpipm_opts); + } + else + { + printf("\nerror: field %s not available in ocp_qp_partial_condensing_opts_set\n", field); + exit(1); + } + + return; + +} + + + /************************************************ * memory ************************************************/ @@ -148,6 +188,8 @@ int ocp_qp_partial_condensing_memory_calculate_size(ocp_qp_dims *dims, void *opt return size; } + + void *ocp_qp_partial_condensing_memory_assign(ocp_qp_dims *dims, void *opts_, void *raw_memory) { ocp_qp_partial_condensing_opts *opts = opts_; @@ -177,11 +219,19 @@ void *ocp_qp_partial_condensing_memory_assign(ocp_qp_dims *dims, void *opts_, vo return mem; } + + /************************************************ * memory ************************************************/ -int ocp_qp_partial_condensing_workspace_calculate_size(ocp_qp_dims *dims, void *opts_) { return 0; } +int ocp_qp_partial_condensing_workspace_calculate_size(ocp_qp_dims *dims, void *opts_) +{ + return 0; +} + + + /************************************************ * functions ************************************************/ @@ -199,6 +249,8 @@ void ocp_qp_partial_condensing(ocp_qp_in *in, ocp_qp_in *out, ocp_qp_partial_con d_cond_qp_ocp2ocp(in, out, opts->hpipm_opts, mem->hpipm_workspace); } + + void ocp_qp_partial_expansion(ocp_qp_out *in, ocp_qp_out *out, ocp_qp_partial_condensing_opts *opts, ocp_qp_partial_condensing_memory *mem, void *work) { @@ -208,6 +260,8 @@ void ocp_qp_partial_expansion(ocp_qp_out *in, ocp_qp_out *out, ocp_qp_partial_co mem->hpipm_workspace); } + + void ocp_qp_partial_condensing_config_initialize_default(void *config_) { ocp_qp_condensing_config *config = config_; @@ -216,6 +270,7 @@ void ocp_qp_partial_condensing_config_initialize_default(void *config_) config->opts_assign = &ocp_qp_partial_condensing_opts_assign; config->opts_initialize_default = &ocp_qp_partial_condensing_opts_initialize_default; config->opts_update = &ocp_qp_partial_condensing_opts_update; + config->opts_set = &ocp_qp_partial_condensing_opts_set; config->memory_calculate_size = &ocp_qp_partial_condensing_memory_calculate_size; config->memory_assign = &ocp_qp_partial_condensing_memory_assign; config->workspace_calculate_size = &ocp_qp_partial_condensing_workspace_calculate_size; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.h b/acados/ocp_qp/ocp_qp_partial_condensing.h index 182562c108..51fc06ff82 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -27,6 +27,8 @@ extern "C" { // acados #include "acados/ocp_qp/ocp_qp_common.h" + + typedef struct ocp_qp_partial_condensing_opts_ { struct d_cond_qp_ocp2ocp_arg *hpipm_opts; @@ -36,6 +38,8 @@ typedef struct ocp_qp_partial_condensing_opts_ int N2_bkp; } ocp_qp_partial_condensing_opts; + + typedef struct ocp_qp_partial_condensing_memory_ { struct d_cond_qp_ocp2ocp_workspace *hpipm_workspace; @@ -43,6 +47,8 @@ typedef struct ocp_qp_partial_condensing_memory_ ocp_qp_in *pcond_qp_in; } ocp_qp_partial_condensing_memory; + + // int ocp_qp_partial_condensing_opts_calculate_size(ocp_qp_dims *dims); // @@ -52,6 +58,8 @@ void ocp_qp_partial_condensing_opts_initialize_default(ocp_qp_dims *dims, void * // void ocp_qp_partial_condensing_opts_update(ocp_qp_dims *dims, void *opts_); // +void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value); +// int ocp_qp_partial_condensing_memory_calculate_size(ocp_qp_dims *dims, void *opts_); // void *ocp_qp_partial_condensing_memory_assign(ocp_qp_dims *dims, void *opts, void *raw_memory); @@ -66,6 +74,8 @@ void ocp_qp_partial_expansion(ocp_qp_out *in, ocp_qp_out *out, ocp_qp_partial_co // void ocp_qp_partial_condensing_config_initialize_default(void *config_); + + #ifdef __cplusplus } /* extern "C" */ #endif diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 86ffc1391d..6a94295d4c 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -124,27 +124,35 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const // TODO extract module name 'pcond' for partial condensing optsion - if (!strcmp(field, "N2") || !strcmp(field, "pcond_N2")) - { - int* N2 = (int *) value; - opts->pcond_opts->N2 = *N2; - } - else if (!strcmp(field, "N2_bkp") || !strcmp(field, "pcond_N2_bkp")) - { - int* N2_bkp = (int *) value; - opts->pcond_opts->N2_bkp = *N2_bkp; - } - else //if(!strcmp(field, "ric_alg")) + int ii; + + char module[MAX_STR_LEN]; + char *ptr_module = NULL; + int module_length = 0; + + // extract module name + char *char_ = strchr(field, '_'); + if(char_!=NULL) + { + module_length = char_-field; + for(ii=0; iipcond_opts, field+module_length+1, value); + } + else // pass options to QP module { config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); } -// else -// { -// // TODO pass options to qp solver -// printf("\nerror: option type %s not available in ocp_qp_partial_condense solver module\n", field); -// exit(1); -// } + return; + } diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 7343a80027..6c6ba2d9db 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -21,6 +21,7 @@ qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_pcond_N = 5; +qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; %sim_method = 'erk'; sim_method = 'irk'; @@ -173,6 +174,7 @@ ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); end ocp_opts.set('sim_method', sim_method); diff --git a/interfaces/acados_c/options_interface.c b/interfaces/acados_c/options_interface.c index 56e51909dd..25c27de78f 100644 --- a/interfaces/acados_c/options_interface.c +++ b/interfaces/acados_c/options_interface.c @@ -107,7 +107,7 @@ bool set_option_int(void *args_, const char *option, const int value) args->hpipm_opts->iter_max = value; else if (!strcmp(token, "max_stat")) args->hpipm_opts->stat_max = value; - else if (!strcmp(token, "N2")) + else if (!strcmp(token, "pcond_N2")) pcond_opts->N2 = value; else return false; @@ -140,7 +140,7 @@ bool set_option_int(void *args_, const char *option, const int value) args->warm_start = value; // NOTE(dimitris): HPMPC partial condesing has a bug, using hpipm partial condensing // instead - else if (!strcmp(token, "N2")) + else if (!strcmp(token, "pcond_N2")) pcond_opts->N2 = value; // partial tightening else if (!strcmp(token, "N")) @@ -162,7 +162,7 @@ bool set_option_int(void *args_, const char *option, const int value) (ocp_qp_partial_condensing_opts *) sparse_args->pcond_opts; if (!strcmp(token, "print_level")) args->printLevel = value; - else if (!strcmp(token, "N2")) + else if (!strcmp(token, "pcond_N2")) pcond_opts->N2 = value; else return false; @@ -188,7 +188,7 @@ bool set_option_int(void *args_, const char *option, const int value) ocp_qp_osqp_opts *args = (ocp_qp_osqp_opts *) sparse_args->qp_solver_opts; ocp_qp_partial_condensing_opts *pcond_opts = (ocp_qp_partial_condensing_opts *) sparse_args->pcond_opts; - if (!strcmp(token, "N2")) + if (!strcmp(token, "pcond_N2")) pcond_opts->N2 = value; else return false; @@ -215,7 +215,7 @@ bool set_option_int(void *args_, const char *option, const int value) { args->options.maxIter = value; } - else if (!strcmp(token, "N2")) + else if (!strcmp(token, "pcond_N2")) { pcond_opts->N2 = value; } diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 76a6c152bf..8082f8ed14 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -46,6 +46,8 @@ obj.opts_struct.qp_solver = value; elseif (strcmp(field, 'qp_solver_pcond_N')) obj.opts_struct.qp_solver_pcond_N = value; + elseif (strcmp(field, 'qp_solver_pcond_ric_alg')) + obj.opts_struct.qp_solver_pcond_ric_alg = value; elseif (strcmp(field, 'qp_solver_ric_alg')) obj.opts_struct.qp_solver_ric_alg = value; elseif (strcmp(field, 'sim_method')) diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 54c38210a4..674c35f63c 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -621,6 +621,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) int nlp_solver_max_iter; bool set_nlp_solver_max_iter = false; char *qp_solver; int qp_solver_pcond_N; bool set_qp_solver_pcond_N = false; + int qp_solver_pcond_ric_alg; bool set_qp_solver_pcond_ric_alg = false; int qp_solver_ric_alg; bool set_qp_solver_ric_alg = false; char *sim_method; int sim_method_num_stages; bool set_sim_method_num_stages = false; @@ -663,6 +664,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) set_qp_solver_pcond_N = true; qp_solver_pcond_N = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_pcond_N" ) ); } + // pcond riccati-like algorithm + if(mxGetField( prhs[1], 0, "qp_solver_pcond_ric_alg" )!=NULL) + { + set_qp_solver_pcond_ric_alg = true; + qp_solver_pcond_ric_alg = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_pcond_ric_alg" ) ); + } // hpipm: riccati algorithm if(mxGetField( prhs[1], 0, "qp_solver_ric_alg" )!=NULL) { @@ -1058,6 +1065,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "qp_pcond_N2", &qp_solver_pcond_N); } + // qp_solver_pcond_ric alg + if(set_qp_solver_pcond_N) + { + ocp_nlp_opts_set(config, opts, "qp_pcond_ric_alg", &qp_solver_pcond_ric_alg); + } // qp_solver_ric_alg if(set_qp_solver_ric_alg) { From f5e1594766c269c20f8da226ebeb76fc00443fad Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 26 May 2019 12:18:45 +0200 Subject: [PATCH 18/78] working on project_red_hess --- Makefile | 1 + acados/ocp_nlp/Makefile | 1 + .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 585 ++++++++++++++++++ .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.h | 117 ++++ acados/ocp_nlp/ocp_nlp_sqp.c | 2 + .../pendulum_on_cart_model/example_ocp.m | 5 +- interfaces/acados_c/ocp_nlp_interface.c | 4 + interfaces/acados_c/ocp_nlp_interface.h | 1 + interfaces/acados_matlab/ocp_create.c | 4 + 9 files changed, 718 insertions(+), 2 deletions(-) create mode 100644 acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c create mode 100644 acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h diff --git a/Makefile b/Makefile index a50c8f59ff..34cac0093f 100644 --- a/Makefile +++ b/Makefile @@ -34,6 +34,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 diff --git a/acados/ocp_nlp/Makefile b/acados/ocp_nlp/Makefile index 89e4a9173c..399e34263d 100644 --- a/acados/ocp_nlp/Makefile +++ b/acados/ocp_nlp/Makefile @@ -21,6 +21,7 @@ OBJS += ocp_nlp_reg_common.o OBJS += ocp_nlp_reg_convexify.o OBJS += ocp_nlp_reg_mirror.o OBJS += ocp_nlp_reg_project.o +OBJS += ocp_nlp_reg_project_reduc_hess.o OBJS += ocp_nlp_reg_noreg.o obj: $(OBJS) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c new file mode 100644 index 0000000000..6f13d7dbaa --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -0,0 +1,585 @@ +/* + * This file is part of acados. + * + * acados is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * acados is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with acados; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +#include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h" + +#include +#include +#include +#include + +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/utils/math.h" +#include "acados/utils/mem.h" + +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_blas.h" + + + +/************************************************ + * opts + ************************************************/ + +int ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void) +{ + return sizeof(ocp_nlp_reg_project_reduc_hess_opts); +} + + + +void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory) +{ + return raw_memory; +} + + + +void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; + + opts->epsilon = 1e-4; + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value) +{ + + ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; + + if (!strcmp(field, "epsilon")) + { + double *d_ptr = value; + opts->epsilon = *d_ptr; + } + else + { + printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_opts_set\n", field); + exit(1); + } + + return; +} + + + +/************************************************ + * memory + ************************************************/ + +int ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + int nuM = nu[0]; + int nxM = nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nxM = nx[ii]>nxM ? nx[ii] : nxM; + } + + int size = 0; + + size += sizeof(ocp_nlp_reg_project_reduc_hess_memory); + + size += nuxM*nuxM*sizeof(double); // reg_hess + size += nuxM*nuxM*sizeof(double); // V + size += 2*nuxM*sizeof(double); // d e + size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + size += N*sizeof(struct blasfeo_dmat *); // BAbt + + size += 1 * 64; + + size += blasfeo_memsize_dmat(nuxM, nuxM); // L + size += blasfeo_memsize_dmat(nuxM, nuxM); // L2 + size += blasfeo_memsize_dmat(nuxM, nuxM); // L3 + size += blasfeo_memsize_dmat(nxM, nuM); // Ls + size += blasfeo_memsize_dmat(nxM, nxM); // P + size += blasfeo_memsize_dmat(nuxM, nxM); // AL + + return size; +} + + + +void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory) +{ + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + int ii; + + int nuxM = nu[0]+nx[0]; + int nuM = nu[0]; + int nxM = nx[0]; + for(ii=1; ii<=N; ii++) + { + nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nxM = nx[ii]>nxM ? nx[ii] : nxM; + } + + char *c_ptr = (char *) raw_memory; + + ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) c_ptr; + c_ptr += sizeof(ocp_nlp_reg_project_reduc_hess_memory); + + mem->reg_hess = (double *) c_ptr; + c_ptr += nuxM*nuxM*sizeof(double); // reg_hess + + mem->V = (double *) c_ptr; + c_ptr += nuxM*nuxM*sizeof(double); // V + + mem->d = (double *) c_ptr; + c_ptr += nuxM*sizeof(double); // d + + mem->e = (double *) c_ptr; + c_ptr += nuxM*sizeof(double); // e + + mem->RSQrq = (struct blasfeo_dmat **) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq + + mem->BAbt = (struct blasfeo_dmat **) c_ptr; + c_ptr += N*sizeof(struct blasfeo_dmat *); // BAbt + + align_char_to(64, &c_ptr); + + assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L, &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L2, &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L3, &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nxM, nuM, &mem->Ls, &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->P, &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nuxM, nxM, &mem->AL, &c_ptr); + + assert((char *) mem + ocp_nlp_reg_project_reduc_hess_memory_calculate_size(config_, dims, opts_) >= c_ptr); + + return mem; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_) +{ + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; ii<=N; ii++) + { + memory->RSQrq[ii] = RSQrq+ii; +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0); + } + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_) +{ +#if 0 + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; ii<=N; ii++) + { + memory->rq[ii] = rq+ii; +// blasfeo_print_dvec(nu[ii]+nx[ii], memory->rq[ii], 0); + } +#endif + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_) +{ + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; iiBAbt[ii] = BAbt+ii; +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nx[ii+1], memory->BAbt[ii], 0, 0); + } + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_) +{ +#if 0 + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; iib[ii] = b+ii; +// blasfeo_print_dvec(nx[ii=1], memory->b[ii], 0); + } +#endif + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) +{ +#if 0 + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; ii<=N; ii++) + { + memory->ux[ii] = ux+ii; +// blasfeo_print_dvec(nu[ii]+nx[ii], memory->ux[ii], 0); + } +#endif + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_) +{ +#if 0 + ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + + for(ii=0; iipi[ii] = pi+ii; +// blasfeo_print_dvec(nx[ii+1], memory->pi[ii], 0); + } +#endif + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) +{ + + if(!strcmp(field, "RSQrq_ptr")) + { + struct blasfeo_dmat *RSQrq = value; + ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(dims, RSQrq, memory_); + } +// else if(!strcmp(field, "rq_ptr")) +// { +// struct blasfeo_dvec *rq = value; +// ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr(dims, rq, memory_); +// } + else if(!strcmp(field, "BAbt_ptr")) + { + struct blasfeo_dmat *BAbt = value; + ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(dims, BAbt, memory_); + } +// else if(!strcmp(field, "b_ptr")) +// { +// struct blasfeo_dvec *b = value; +// ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr(dims, b, memory_); +// } +// else if(!strcmp(field, "ux_ptr")) +// { +// struct blasfeo_dvec *ux = value; +// ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(dims, ux, memory_); +// } +// else if(!strcmp(field, "pi_ptr")) +// { +// struct blasfeo_dvec *pi = value; +// ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr(dims, pi, memory_); +// } + else + { + printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_set\n", field); + exit(1); + } + + return; +} + + + +/************************************************ + * functions + ************************************************/ + +void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) mem_; + ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; + + int ii, jj, kk, ll, ss; + +//printf("\nhola\n"); + int *nx = dims->nx; + int *nu = dims->nu; + int N = dims->N; + + struct blasfeo_dmat *L = &mem->L; + struct blasfeo_dmat *L2 = &mem->L2; + struct blasfeo_dmat *L3 = &mem->L3; + struct blasfeo_dmat *Ls = &mem->Ls; + struct blasfeo_dmat *P = &mem->P; + struct blasfeo_dmat *AL = &mem->AL; + + int do_reg = 0; + double pivot, tmp_el; + +// for(ii=0; ii<=N; ii++) +// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); +// exit(1); + +//printf("\nin project\n"); + // last stage + ss = N; + blasfeo_dtrtr_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, mem->RSQrq[ss], 0, 0); // necessary ??? + // TODO !!!!!!!!!!!!!!!!!! +// blasfeo_unpack_dmat(nu[ss], nu[ss], mem->RSQrq[ss], 0, 0, mem->reg_hess, nu[ss]); +// acados_project(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon); +// blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], mem->RSQrq[ss], 0, 0); + blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], mem->RSQrq[ss], 0, 0, L, 0, 0); +//printf("\nii = %d\n", ss); +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); + blasfeo_dgecp(nx[ss], nu[ss], L, nu[ss], 0, Ls, 0, 0); + blasfeo_dsyrk_ln_mn(nx[ss], nx[ss], nu[ss], -1.0, Ls, 0, 0, Ls, 0, 0, 1.0, mem->RSQrq[ss], nu[ss], nu[ss], P, 0, 0); + blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0); + + // middle stages + for(ii=0; iiBAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm + blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0); + blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ??? + + // backup L in L3 + blasfeo_dgese(nu[ss]+nx[ss], nu[ss]+nx[ss], 0.0, L3, 0, 0); + blasfeo_dgecp(nu[ss]+nx[ss], nu[ss], L, 0, 0, L3, 0, 0); + + // project L_R + blasfeo_unpack_dmat(nu[ss], nu[ss], L, 0, 0, mem->reg_hess, nu[ss]); + acados_eigen_decomposition(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e); + do_reg = 0; + for(jj=0; jjd[jj]epsilon) + { + mem->e[jj] = opts->epsilon - mem->d[jj]; + do_reg = 1; + } + else + { + mem->e[jj] = 0.0; + } + } + if(do_reg) + { + acados_reconstruct_A(nu[ss], mem->reg_hess, mem->V, mem->e); + blasfeo_dgese(nu[ss]+nx[ss], nu[ss]+nx[ss], 0.0, L2, 0, 0); + blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], L2, 0, 0); + + // apply reg to R + blasfeo_dgead(nu[ss], nu[ss], 1.0, L2, 0, 0, mem->RSQrq[ss], 0, 0); + // apply reg to L + blasfeo_dgead(nu[ss], nu[ss], 1.0, L2, 0, 0, L, 0, 0); + } + + // compute reg_schur + blasfeo_dgecp(nu[ss]+nx[ss], nu[ss], L, 0, 0, L2, 0, 0); + blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], L2, 0, 0, L2, 0, 0); + blasfeo_dgecp(nx[ss], nu[ss], L2, nu[ss], 0, Ls, 0, 0); + blasfeo_dsyrk_ln_mn(nx[ss], nx[ss], nu[ss], -1.0, Ls, 0, 0, Ls, 0, 0, 0.0, L2, nu[ss], nu[ss], L2, nu[ss], nu[ss]); +//printf("\nL2\n"); +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L2, 0, 0); + + // compute true_schur + if(do_reg) + { + for(jj=0; jjRSQrq[ss], nu[ss], nu[ss]); + blasfeo_dgead(nx[ss], nx[ss], 1.0, L3, nu[ss], nu[ss], mem->RSQrq[ss], nu[ss], nu[ss]); + } + else + { + // P + blasfeo_dgead(nx[ss], nx[ss], 1.0, L2, nu[ss], nu[ss], P, 0, 0); + } + blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0); +//blasfeo_print_dmat(nx[ss], nx[ss], P, 0, 0); + + } + + // first stage: factorize P in L too + ss = 0; +//printf("\nss %d\n", ss); + blasfeo_dgemm_nt(nu[ss]+nx[ss], nx[ss+1], nx[ss+1], 1.0, mem->BAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm + blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0); + blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ??? + // TODO regularize RSQ (also SQ !!!) +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); + blasfeo_unpack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0, mem->reg_hess, nu[ss]+nx[ss]); + acados_eigen_decomposition(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->d, mem->e); + for(jj=0; jjd[jj]epsilon) + mem->e[jj] = opts->epsilon - mem->d[jj]; + else + mem->e[jj] = 0.0; + } +//d_print_mat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->V, nu[ss]+nx[ss]); +//d_print_mat(1, nu[ss]+nx[ss], mem->d, 1); +//d_print_mat(1, nu[ss]+nx[ss], mem->e, 1); + acados_reconstruct_A(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->e); +//d_print_mat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->reg_hess, nu[ss]+nx[ss]); + blasfeo_pack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->reg_hess, nu[ss]+nx[ss], L2, 0, 0); + blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, mem->RSQrq[ss], 0, 0); +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0); + // TODO till here +//blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, L, 0, 0); +//blasfeo_dpotrf_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); + +// blasfeo_print_dmat(nx[ii], nx[ii], P, 0, 0); + +// exit(1); + +// printf("\nhessian after\n"); +// for(ii=0; ii<=N; ii++) +// { +// printf("\nii = %d\n", ii); +// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); +// blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); +// acados_eigen_decomposition(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e); +// d_print_mat(1, nu[ii]+nx[ii], mem->d, 1); +// } +// exit(1); + + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_) +{ + return; +} + + + +void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config) +{ + // dims + config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size; + config->dims_assign = &ocp_nlp_reg_dims_assign; + config->dims_set = &ocp_nlp_reg_dims_set; + // opts + config->opts_calculate_size = &ocp_nlp_reg_project_reduc_hess_opts_calculate_size; + config->opts_assign = &ocp_nlp_reg_project_reduc_hess_opts_assign; + config->opts_initialize_default = &ocp_nlp_reg_project_reduc_hess_opts_initialize_default; + config->opts_set = &ocp_nlp_reg_project_reduc_hess_opts_set; + // memory + config->memory_calculate_size = &ocp_nlp_reg_project_reduc_hess_memory_calculate_size; + config->memory_assign = &ocp_nlp_reg_project_reduc_hess_memory_assign; + config->memory_set = &ocp_nlp_reg_project_reduc_hess_memory_set; + config->memory_set_RSQrq_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr; + config->memory_set_rq_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr; + config->memory_set_BAbt_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr; + config->memory_set_b_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr; + config->memory_set_ux_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr; + config->memory_set_pi_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr; + // functions + config->regularize_hessian = &ocp_nlp_reg_project_reduc_hess_regularize_hessian; + config->correct_dual_sol = &ocp_nlp_reg_project_reduc_hess_correct_dual_sol; +} + diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h new file mode 100644 index 0000000000..5d932fd61e --- /dev/null +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -0,0 +1,117 @@ +/* + * This file is part of acados. + * + * acados is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 3 of the License, or (at your option) any later version. + * + * acados is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with acados; if not, write to the Free Software Foundation, + * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + + // TODO ??? + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_project_reduc_hess_opts; + +// +int ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void); +// +void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + double *reg_hess; // TODO move to workspace + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + + // giaf's + struct blasfeo_dmat L; // TODO move to workspace + struct blasfeo_dmat L2; // TODO move to workspace + struct blasfeo_dmat L3; // TODO move to workspace + struct blasfeo_dmat Ls; // TODO move to workspace + struct blasfeo_dmat P; // TODO move to workspace + struct blasfeo_dmat AL; // TODO move to workspace + + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat **BAbt; // pointer to RSQrq in qp_in +} ocp_nlp_reg_project_reduc_hess_memory; + +// +int ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_ +/// @} +/// @} diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 2d9a80790d..55dc8d5180 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -1216,6 +1216,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->nlp_res->inf_norm_res_m : nlp_out->inf_norm_res; +printf("\niter = %d, res = %e %e %e %e\n", sqp_iter, mem->nlp_res->inf_norm_res_g, mem->nlp_res->inf_norm_res_b, mem->nlp_res->inf_norm_res_d, mem->nlp_res->inf_norm_res_m); + // exit conditions on residuals if ((mem->nlp_res->inf_norm_res_g < opts->min_res_g) & (mem->nlp_res->inf_norm_res_b < opts->min_res_b) & diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 6c6ba2d9db..769a4fc5bb 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -14,13 +14,14 @@ %nlp_solver_exact_hessian = 'false' nlp_solver_exact_hessian = 'true' %regularize_method = 'no_regularize'; -regularize_method = 'project'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; +qp_solver_pcond_N = N; %5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; %sim_method = 'erk'; diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index f7eb0af7f6..88b8f872b7 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -37,6 +37,7 @@ #include "acados/ocp_nlp/ocp_nlp_reg_convexify.h" #include "acados/ocp_nlp/ocp_nlp_reg_mirror.h" #include "acados/ocp_nlp/ocp_nlp_reg_project.h" +#include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h" #include "acados/ocp_nlp/ocp_nlp_reg_noreg.h" #include "acados/ocp_nlp/ocp_nlp_sqp.h" #include "acados/ocp_nlp/ocp_nlp_sqp_rti.h" @@ -198,6 +199,9 @@ ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan plan) case PROJECT: ocp_nlp_reg_project_config_initialize_default(config->regularize); break; + case PROJECT_REDUC_HESS: + ocp_nlp_reg_project_reduc_hess_config_initialize_default(config->regularize); + break; case CONVEXIFY: ocp_nlp_reg_convexify_config_initialize_default(config->regularize); break; diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index ea21be4480..be7c1b47ce 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -85,6 +85,7 @@ typedef enum NO_REGULARIZE, MIRROR, PROJECT, + PROJECT_REDUC_HESS, CONVEXIFY, INVALID_REGULARIZE, } ocp_nlp_reg_t; diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 674c35f63c..a8287d704f 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -891,6 +891,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { plan->regularization = PROJECT; } + else if(!strcmp(regularize_method, "project_reduc_hess")) + { + plan->regularization = PROJECT_REDUC_HESS; + } else if(!strcmp(regularize_method, "convexify")) { plan->regularization = CONVEXIFY; From 0d2b84981dd641521829c44b5f825e1d2f2ca7b0 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 26 May 2019 17:55:07 +0200 Subject: [PATCH 19/78] matlab interface: fix to generate_c_code_nonlinear_constr --- interfaces/acados_matlab/generate_c_code_nonlinear_constr.m | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_matlab/generate_c_code_nonlinear_constr.m b/interfaces/acados_matlab/generate_c_code_nonlinear_constr.m index 2fc648fcd0..a7e2785aaa 100644 --- a/interfaces/acados_matlab/generate_c_code_nonlinear_constr.m +++ b/interfaces/acados_matlab/generate_c_code_nonlinear_constr.m @@ -105,7 +105,7 @@ function generate_c_code_nonlinear_constr( model, opts ) h_e_fun_jac_ut_xt_hess = Function([model_name,'_constr_h_e_fun_jac_ut_xt_hess'], {x, lam_h_e, p}, {h_e, jac_x_e', hess_ux_e}); else h_e_fun_jac_ut_xt = Function([model_name,'_constr_h_e_fun_jac_ut_xt'], {x}, {h_e, jac_x_e'}); - h_e_fun_jac_ut_xt_hess = Function([model_name,'_constr_h_e_fun_jac_ut_xt_hess'], {x, lam_h}, {h_e, jac_x_e', hess_ux_e}); + h_e_fun_jac_ut_xt_hess = Function([model_name,'_constr_h_e_fun_jac_ut_xt_hess'], {x, lam_h_e}, {h_e, jac_x_e', hess_ux_e}); end % generate C code h_e_fun_jac_ut_xt.generate([model_name,'_constr_h_e_fun_jac_ut_xt'], casadi_opts); From f0971250befa6a7954179f070838ec2b8eff4499 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 26 May 2019 18:41:01 +0200 Subject: [PATCH 20/78] fix partial_cond_opts_set for ric_alg --- acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 2 ++ acados/ocp_qp/ocp_qp_partial_condensing.c | 3 ++- acados/ocp_qp/ocp_qp_partial_condensing.h | 1 + examples/matlab_mex/pendulum_on_cart_model/example_ocp.m | 2 +- 4 files changed, 6 insertions(+), 2 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index 6f13d7dbaa..70ef2f4767 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -496,6 +496,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg blasfeo_dgead(nx[ss], nx[ss], 1.0, L2, nu[ss], nu[ss], P, 0, 0); } blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0); +//printf("\nP\n"); //blasfeo_print_dmat(nx[ss], nx[ss], P, 0, 0); } @@ -528,6 +529,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg // TODO till here //blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, L, 0, 0); //blasfeo_dpotrf_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); +//printf("\nL0\n"); //blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); // blasfeo_print_dmat(nx[ii], nx[ii], P, 0, 0); diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index 86cd469380..41b5fadf54 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -124,6 +124,7 @@ void ocp_qp_partial_condensing_opts_update(ocp_qp_dims *dims, void *opts_) opts->N2_bkp = opts->N2; // hpipm_opts d_set_default_cond_qp_ocp2ocp_arg(opts->N2, opts->hpipm_opts); + d_set_cond_qp_ocp2ocp_arg_ric_alg(opts->ric_alg, opts->N2, opts->hpipm_opts); return; } @@ -148,7 +149,7 @@ void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* va else if(!strcmp(field, "ric_alg")) { int *tmp_ptr = value; - d_set_cond_qp_ocp2ocp_arg_ric_alg(*tmp_ptr, opts->N2, opts->hpipm_opts); + opts->ric_alg = *tmp_ptr; } else { diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.h b/acados/ocp_qp/ocp_qp_partial_condensing.h index 51fc06ff82..327d749170 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.h +++ b/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -36,6 +36,7 @@ typedef struct ocp_qp_partial_condensing_opts_ int *block_size; int N2; int N2_bkp; + int ric_alg; } ocp_qp_partial_condensing_opts; diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 769a4fc5bb..a3c94d47c6 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -21,7 +21,7 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = N; %5; +qp_solver_pcond_N = 5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; %sim_method = 'erk'; From cc5e6fdea7a8c2adb75ccb439999198889edbbe9 Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 28 May 2019 16:26:07 +0200 Subject: [PATCH 21/78] add stat to sqp memory --- acados/ocp_nlp/ocp_nlp_sqp.c | 40 ++++++++++++++++++- acados/ocp_nlp/ocp_nlp_sqp.h | 4 ++ acados/ocp_nlp/ocp_nlp_sqp_rti.c | 38 ++++++++++++++++++ acados/ocp_nlp/ocp_nlp_sqp_rti.h | 4 ++ .../pendulum_on_cart_model/example_ocp.m | 29 +++++++++----- interfaces/acados_matlab/ocp_get.c | 22 +++++++++- 6 files changed, 123 insertions(+), 14 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 55dc8d5180..03e0ece345 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -453,6 +453,9 @@ int ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void *opts_) // nlp mem size += ocp_nlp_memory_calculate_size(config, dims); + // stat + size += 5*opts->max_iter*sizeof(double); + size += 8; // initial align // make_int_multiple_of(64, &size); @@ -534,6 +537,12 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *r opts->constraints[ii]); } + // stat + mem->stat = (double *) c_ptr; + mem->stat_m = opts->max_iter; + mem->stat_n = 5; + c_ptr += mem->stat_m*mem->stat_n*sizeof(double); + mem->status = ACADOS_READY; assert((char *) raw_memory + ocp_nlp_sqp_memory_calculate_size(config, dims, opts) >= c_ptr); @@ -1103,6 +1112,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int ii; + int qp_iter = 0; + #if defined(ACADOS_WITH_OPENMP) // backup number of threads int num_threads_bkp = omp_get_num_threads(); @@ -1216,7 +1227,15 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->nlp_res->inf_norm_res_m : nlp_out->inf_norm_res; -printf("\niter = %d, res = %e %e %e %e\n", sqp_iter, mem->nlp_res->inf_norm_res_g, mem->nlp_res->inf_norm_res_b, mem->nlp_res->inf_norm_res_d, mem->nlp_res->inf_norm_res_m); + // save statistics + if (sqp_iter < mem->stat_m) + { + mem->stat[mem->stat_n*sqp_iter+0] = mem->nlp_res->inf_norm_res_g; + mem->stat[mem->stat_n*sqp_iter+1] = mem->nlp_res->inf_norm_res_b; + mem->stat[mem->stat_n*sqp_iter+2] = mem->nlp_res->inf_norm_res_d; + mem->stat[mem->stat_n*sqp_iter+3] = mem->nlp_res->inf_norm_res_m; + mem->stat[mem->stat_n*sqp_iter+4] = qp_iter; + } // exit conditions on residuals if ((mem->nlp_res->inf_norm_res_g < opts->min_res_g) & @@ -1266,7 +1285,9 @@ printf("\niter = %d, res = %e %e %e %e\n", sqp_iter, mem->nlp_res->inf_norm_res_ // stop timer mem->time_qp_sol += acados_toc(&timer1); + // TODO move into QP solver memory ??? nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; + 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); @@ -1424,9 +1445,24 @@ void ocp_nlp_sqp_get(void *config_, void *mem_, const char *field, void *return_ ocp_nlp_res **value = return_value_; *value = mem->nlp_res; } + else if (!strcmp("stat", field)) + { + double **value = return_value_; + *value = mem->stat; + } + else if (!strcmp("stat_m", field)) + { + int *value = return_value_; + *value = mem->stat_m; + } + else if (!strcmp("stat_n", field)) + { + int *value = return_value_; + *value = mem->stat_n; + } else { - printf("\nerror: output type %s not available in ocp_nlp_sqp module\n", field); + printf("\nerror: field %s not available in ocp_nlp_sqp_get\n", field); exit(1); } diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index c4de24fe3a..dd53e7c4e7 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -100,6 +100,10 @@ typedef struct double time_qp_sol; double time_lin; double time_tot; + + double *stat; + int stat_m; + int stat_n; } ocp_nlp_sqp_memory; // diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 1ce1b37dd7..2a30391472 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -418,6 +418,9 @@ int ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts // nlp mem size += ocp_nlp_memory_calculate_size(config, dims); + // stat + size += 1*2*sizeof(double); + size += 8; // initial align // make_int_multiple_of(64, &size); @@ -495,6 +498,14 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, voi opts->constraints[ii]); } + // stat + mem->stat = (double *) c_ptr; + mem->stat_m = 2; + mem->stat_n = 1; + c_ptr += mem->stat_m*mem->stat_n*sizeof(double); + + mem->status = ACADOS_READY; + assert((char *) raw_memory+ocp_nlp_sqp_rti_memory_calculate_size(config, dims, opts) >= c_ptr); return mem; @@ -1060,6 +1071,8 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int ii; + int qp_iter = 0; + #if defined(ACADOS_WITH_OPENMP) // backup number of threads int num_threads_bkp = omp_get_num_threads(); @@ -1157,6 +1170,9 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // update QP rhs for SQP (step prim var, abs dual var) sqp_update_qp_vectors(config, dims, nlp_in, nlp_out, opts, mem, work); + // save statistics + mem->stat[mem->stat_n*1+0] = qp_iter; + // regularize Hessian config->regularize->regularize_hessian(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); @@ -1177,11 +1193,18 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // compute correct dual solution in case of Hessian regularization config->regularize->correct_dual_sol(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); + // TODO move into QP solver memory ??? + nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; + 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) // exit(1); + // save statistics + mem->stat[mem->stat_n*1+0] = qp_iter; + if (qp_status != 0) { // print_ocp_qp_in(work->qp_in); @@ -1300,6 +1323,21 @@ void ocp_nlp_sqp_rti_get(void *config_, void *mem_, const char *field, void *ret double *value = return_value_; *value = mem->time_lin; } + else if (!strcmp("stat", field)) + { + double **value = return_value_; + *value = mem->stat; + } + else if (!strcmp("stat_m", field)) + { + int *value = return_value_; + *value = mem->stat_m; + } + else if (!strcmp("stat_n", field)) + { + int *value = return_value_; + *value = mem->stat_n; + } else { printf("\nerror: output type %s not available in ocp_nlp_sqp_rti module\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index ef25465513..1003d041f0 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -90,6 +90,10 @@ typedef struct double time_qp_sol; double time_lin; double time_tot; + + double *stat; + int stat_m; + int stat_n; } ocp_nlp_sqp_rti_memory; // diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index a3c94d47c6..8f072afbb6 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -171,7 +171,9 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +if (strcmp(nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); @@ -232,6 +234,21 @@ +% statistics + +status = ocp.get('status'); +sqp_iter = ocp.get('sqp_iter'); +time_tot = ocp.get('time_tot'); +time_lin = ocp.get('time_lin'); +time_qp_sol = ocp.get('time_qp_sol'); + +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); + +stat = ocp.get('stat') + + +% figures + for ii=1:N+1 x_cur = x(:,ii); visualize; @@ -249,16 +266,6 @@ -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_qp_sol = ocp.get('time_qp_sol'); - -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); - - - if status==0 fprintf('\nsuccess!\n\n'); else diff --git a/interfaces/acados_matlab/ocp_get.c b/interfaces/acados_matlab/ocp_get.c index 420a03e17b..6ccfc478f2 100644 --- a/interfaces/acados_matlab/ocp_get.c +++ b/interfaces/acados_matlab/ocp_get.c @@ -16,7 +16,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) long long *ptr; - int ii; + int ii, jj; /* RHS */ @@ -129,6 +129,26 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *mat_ptr = mxGetPr( plhs[0] ); ocp_nlp_get(config, solver, "time_qp_sol", mat_ptr); } + else if(!strcmp(field, "stat")) + { + int sqp_iter; + int stat_m, stat_n; + double *stat; + ocp_nlp_get(config, solver, "sqp_iter", &sqp_iter); + ocp_nlp_get(config, solver, "stat_m", &stat_m); + ocp_nlp_get(config, solver, "stat_n", &stat_n); + ocp_nlp_get(config, solver, "stat", &stat); + plhs[0] = mxCreateNumericMatrix(sqp_iter+1, stat_n+1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + if(sqp_iter+1>stat_m) + sqp_iter = stat_m-1; + for(ii=0; ii Date: Tue, 28 May 2019 18:43:34 +0200 Subject: [PATCH 22/78] add closed loop example for pendulum_on_cart_model --- .../example_closed_loop.m | 348 ++++++++++++++++++ 1 file changed, 348 insertions(+) create mode 100644 examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m new file mode 100644 index 0000000000..a13d876113 --- /dev/null +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -0,0 +1,348 @@ +%% test of native matlab interface +clear all + + + +%% arguments +compile_mex = 'true'; +codgen_model = 'true'; +% simulation +sim_method = 'irk'; +sim_sens_forw = 'false'; +sim_num_stages = 4; +sim_num_steps = 4; +% ocp +param_scheme = 'multiple_shooting_unif_grid'; +ocp_N = 50; +nlp_solver = 'sqp'; +%nlp_solver = 'sqp_rti'; +%nlp_solver_exact_hessian = 'false' +nlp_solver_exact_hessian = 'true' +%regularize_method = 'no_regularize'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; +%regularize_method = 'mirror'; +%regularize_method = 'convexify'; +nlp_solver_max_iter = 100; +qp_solver = 'partial_condensing_hpipm'; +%qp_solver = 'full_condensing_hpipm'; +qp_solver_pcond_N = 5; +qp_solver_pcond_ric_alg = 0; +qp_solver_ric_alg = 0; +ocp_sim_method = 'erk'; +%ocp_sim_method = 'irk'; +ocp_sim_method_num_stages = 4; +ocp_sim_method_num_steps = 1; +cost_type = 'linear_ls'; +%cost_type = 'ext_cost'; + +h = 0.01; + + +%% create model entries +model = pendulum_on_cart_model; + + + +% dims +T = ocp_N*h; % horizon length time +nx = model.nx; +nu = model.nu; +ny = nu+nx; % number of outputs in lagrange term +ny_e = nx; % number of outputs in mayer term +if 0 + nbx = 0; + nbu = nu; + ng = 0; + ng_e = 0; + nh = 0; + nh_e = 0; +else + nbx = 0; + nbu = 0; + ng = 0; + ng_e = 0; + nh = nu; + nh_e = 0; +end + +% cost +Vu = zeros(ny, nu); for ii=1:nu Vu(ii,ii)=1.0; end % input-to-output matrix in lagrange term +Vx = zeros(ny, nx); for ii=1:nx Vx(nu+ii,ii)=1.0; end % state-to-output matrix in lagrange term +Vx_e = zeros(ny_e, nx); for ii=1:nx Vx_e(ii,ii)=1.0; end % state-to-output matrix in mayer term +W = eye(ny); % weight matrix in lagrange term +for ii=1:nu W(ii,ii)=1e-2; end +for ii=nu+1:nu+nx/2 W(ii,ii)=1e3; end +for ii=nu+nx/2+1:nu+nx W(ii,ii)=1e-2; end +W_e = W(nu+1:nu+nx, nu+1:nu+nx); % weight matrix in mayer term +yr = zeros(ny, 1); % output reference in lagrange term +yr_e = zeros(ny_e, 1); % output reference in mayer term + +% constraints +x0 = [0; pi; 0; 0]; +%Jbx = zeros(nbx, nx); for ii=1:nbx Jbx(ii,ii)=1.0; end +%lbx = -4*ones(nbx, 1); +%ubx = 4*ones(nbx, 1); +Jbu = zeros(nbu, nu); for ii=1:nbu Jbu(ii,ii)=1.0; end +lbu = -80*ones(nu, 1); +ubu = 80*ones(nu, 1); + + + + +%% acados ocp model +ocp_model = acados_ocp_model(); +% dims +ocp_model.set('T', T); +ocp_model.set('dim_nx', nx); +ocp_model.set('dim_nu', nu); +if (strcmp(cost_type, 'linear_ls')) + ocp_model.set('dim_ny', ny); + ocp_model.set('dim_ny_e', ny_e); +end +ocp_model.set('dim_nbx', nbx); +ocp_model.set('dim_nbu', nbu); +ocp_model.set('dim_ng', ng); +ocp_model.set('dim_ng_e', ng_e); +ocp_model.set('dim_nh', nh); +ocp_model.set('dim_nh_e', nh_e); +% symbolics +ocp_model.set('sym_x', model.sym_x); +if isfield(model, 'sym_u') + ocp_model.set('sym_u', model.sym_u); +end +if isfield(model, 'sym_xdot') + ocp_model.set('sym_xdot', model.sym_xdot); +end +% cost +ocp_model.set('cost_type', cost_type); +ocp_model.set('cost_type_e', cost_type); +%if (strcmp(cost_type, 'linear_ls')) + ocp_model.set('cost_Vu', Vu); + ocp_model.set('cost_Vx', Vx); + ocp_model.set('cost_Vx_e', Vx_e); + ocp_model.set('cost_W', W); + ocp_model.set('cost_W_e', W_e); + ocp_model.set('cost_yr', yr); + ocp_model.set('cost_yr_e', yr_e); +%else % if (strcmp(cost_type, 'ext_cost')) +% ocp_model.set('cost_expr_ext_cost', model.expr_ext_cost); +% ocp_model.set('cost_expr_ext_cost_e', model.expr_ext_cost_e); +%end +% dynamics +if (strcmp(ocp_sim_method, 'erk')) + ocp_model.set('dyn_type', 'explicit'); + ocp_model.set('dyn_expr_f', model.expr_f_expl); +else % irk + ocp_model.set('dyn_type', 'implicit'); + ocp_model.set('dyn_expr_f', model.expr_f_impl); +end +% constraints +ocp_model.set('constr_x0', x0); +if (ng>0) + ocp_model.set('constr_C', C); + ocp_model.set('constr_D', D); + ocp_model.set('constr_lg', lg); + ocp_model.set('constr_ug', ug); + ocp_model.set('constr_C_e', C_e); + ocp_model.set('constr_lg_e', lg_e); + ocp_model.set('constr_ug_e', ug_e); +elseif (nh>0) + ocp_model.set('constr_expr_h', model.expr_h); + ocp_model.set('constr_lh', lbu); + ocp_model.set('constr_uh', ubu); +% ocp_model.set('constr_expr_h_e', model.expr_h_e); +% ocp_model.set('constr_lh_e', lh_e); +% ocp_model.set('constr_uh_e', uh_e); +else +% ocp_model.set('constr_Jbx', Jbx); +% ocp_model.set('constr_lbx', lbx); +% ocp_model.set('constr_ubx', ubx); + ocp_model.set('constr_Jbu', Jbu); + ocp_model.set('constr_lbu', lbu); + ocp_model.set('constr_ubu', ubu); +end + +ocp_model.model_struct + + + +%% acados ocp opts +ocp_opts = acados_ocp_opts(); +ocp_opts.set('compile_mex', compile_mex); +ocp_opts.set('codgen_model', codgen_model); +ocp_opts.set('param_scheme', param_scheme); +ocp_opts.set('param_scheme_N', ocp_N); +ocp_opts.set('nlp_solver', nlp_solver); +ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); +ocp_opts.set('regularize_method', regularize_method); +if (strcmp(nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +end +ocp_opts.set('qp_solver', qp_solver); +if (strcmp(qp_solver, 'partial_condensing_hpipm')) + ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); +end +ocp_opts.set('sim_method', ocp_sim_method); +ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); +ocp_opts.set('sim_method_num_steps', ocp_sim_method_num_steps); + +ocp_opts.opts_struct + + + +%% acados ocp +% create ocp +ocp = acados_ocp(ocp_model, ocp_opts); +ocp +ocp.C_ocp +ocp.C_ocp_ext_fun + + + +%% acados sim model +sim_model = acados_sim_model(); +% dims +sim_model.set('dim_nx', nx); +sim_model.set('dim_nu', nu); +% symbolics +sim_model.set('sym_x', model.sym_x); +if isfield(model, 'sym_u') + sim_model.set('sym_u', model.sym_u); +end +if isfield(model, 'sym_xdot') + sim_model.set('sym_xdot', model.sym_xdot); +end +% model +sim_model.set('T', T/ocp_N); +if (strcmp(sim_method, 'erk')) + sim_model.set('dyn_type', 'explicit'); + sim_model.set('dyn_expr_f', model.expr_f_expl); +else % irk + sim_model.set('dyn_type', 'implicit'); + sim_model.set('dyn_expr_f', model.expr_f_impl); +end + +%sim_model.model_struct + + + +%% acados sim opts +sim_opts = acados_sim_opts(); +sim_opts.set('compile_mex', compile_mex); +sim_opts.set('codgen_model', codgen_model); +sim_opts.set('num_stages', sim_num_stages); +sim_opts.set('num_steps', sim_num_steps); +sim_opts.set('method', sim_method); +sim_opts.set('sens_forw', sim_sens_forw); + +%sim_opts.opts_struct + + + +%% acados sim +% create sim +sim = acados_sim(sim_model, sim_opts); +%sim +%sim.C_sim +%sim.C_sim_ext_fun + + + +%% closed loop simulation +n_sim = 200; +x_sim = zeros(nx, n_sim+1); +x_sim(:,1) = x0; % initial state +u_sim = zeros(nu, n_sim); + +% set trajectory initialization +%x_traj_init = zeros(nx, ocp_N+1); +%for ii=1:ocp_N x_traj_init(:,ii) = [0; pi; 0; 0]; end +x_traj_init = linspace([0; pi; 0; 0], [0; 0; 0; 0], ocp_N+1); + +u_traj_init = zeros(nu, ocp_N); +ocp.set('init_x', x_traj_init); +ocp.set('init_u', u_traj_init); + + +tic; + +for ii=1:n_sim + + % set x0 + ocp.set('constr_x0', x_sim(:,ii)); + + % set trajectory initialization (if not, set internally using previous solution) +% ocp.set('init_x', x_traj_init); +% ocp.set('init_u', u_traj_init); + + % solve OCP + ocp.solve(); + + if 1 + status = ocp.get('status'); + sqp_iter = ocp.get('sqp_iter'); + time_tot = ocp.get('time_tot'); + time_lin = ocp.get('time_lin'); + time_qp_sol = ocp.get('time_qp_sol'); + + fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); + end + + % get solution + %x_traj = ocp.get('x'); + %u_traj = ocp.get('u'); + u_sim(:,ii) = ocp.get('u', 0); + + % set initial state of sim + sim.set('x', x_sim(:,ii)); + % set input in sim + sim.set('u', u_sim(:,ii)); + + % simulate state + sim.solve(); + + % get new state + x_sim(:,ii+1) = sim.get('xn'); + +end + +avg_time_solve = toc/n_sim + + + +% print solution +for ii=1:n_sim+1 + x_cur = x_sim(:,ii); + visualize; +end + + + +status = ocp.get('status'); + +figure(2); +subplot(2,1,1); +plot(0:n_sim, x_sim); +xlim([0 n_sim]); +legend('p', 'theta', 'v', 'omega'); +subplot(2,1,2); +plot(0:n_sim-1, u_sim); +xlim([0 n_sim]); +legend('F'); + + + +if status==0 + fprintf('\nsuccess!\n\n'); +else + fprintf('\nsolution failed!\n\n'); +end + + + +return; + + From da4f64ca2f5c9f1e5d4d04f0b6ea289e935b92ac Mon Sep 17 00:00:00 2001 From: giaf Date: Tue, 28 May 2019 23:34:17 +0200 Subject: [PATCH 23/78] implement shift initialization for closed loop example for pendumul_on_cart_model: --- .../example_closed_loop.m | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m index a13d876113..aec1de08d4 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -13,7 +13,7 @@ sim_num_steps = 4; % ocp param_scheme = 'multiple_shooting_unif_grid'; -ocp_N = 50; +ocp_N = 100; nlp_solver = 'sqp'; %nlp_solver = 'sqp_rti'; %nlp_solver_exact_hessian = 'false' @@ -275,8 +275,8 @@ ocp.set('constr_x0', x_sim(:,ii)); % set trajectory initialization (if not, set internally using previous solution) -% ocp.set('init_x', x_traj_init); -% ocp.set('init_u', u_traj_init); + ocp.set('init_x', x_traj_init); + ocp.set('init_u', u_traj_init); % solve OCP ocp.solve(); @@ -291,9 +291,14 @@ fprintf('\nstatus = %d, sqp_iter = %d, time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); end - % get solution - %x_traj = ocp.get('x'); - %u_traj = ocp.get('u'); + % get solution for initialization of next NLP + x_traj = ocp.get('x'); + u_traj = ocp.get('u'); + + x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; + u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; + + % get solution for sim u_sim(:,ii) = ocp.get('u', 0); % set initial state of sim From 1ca14508212d604feb2da1215be79921ac0beb17 Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 29 May 2019 09:43:48 +0200 Subject: [PATCH 24/78] fix to test_qpsolvers --- test/ocp_qp/test_qpsolvers.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/ocp_qp/test_qpsolvers.cpp b/test/ocp_qp/test_qpsolvers.cpp index 264163cd40..738f934639 100644 --- a/test/ocp_qp/test_qpsolvers.cpp +++ b/test/ocp_qp/test_qpsolvers.cpp @@ -81,19 +81,19 @@ void set_N2(std::string const &inString, void *opts, int N2, int N) if (inString == "SPARSE_HPIPM") { - option_found = set_option_int(opts, "sparse_hpipm.N2", N2); + option_found = set_option_int(opts, "sparse_hpipm.pcond_N2", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_HPMPC") { - option_found = set_option_int(opts, "hpmpc.N2", N2); + option_found = set_option_int(opts, "hpmpc.pcond_N2", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_QPDUNES") { - option_found = set_option_int(opts, "qpdunes.N2", N2); + option_found = set_option_int(opts, "qpdunes.pcond_N2", N2); REQUIRE(option_found == true); if (N2 == N) { @@ -109,13 +109,13 @@ void set_N2(std::string const &inString, void *opts, int N2, int N) if (inString == "SPARSE_OOQP") { - option_found = set_option_int(opts, "sparse_ooqp.N2", N2); + option_found = set_option_int(opts, "sparse_ooqp.pcond_N2", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_OSQP") { - option_found = set_option_int(opts, "sparse_osqp.N2", N2); + option_found = set_option_int(opts, "sparse_osqp.pcond_N2", N2); REQUIRE(option_found == true); } } From 969bdd193f3de3069118c1b78863dde2198bfc2a Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 29 May 2019 10:18:18 +0200 Subject: [PATCH 25/78] remove some todo --- acados/ocp_nlp/ocp_nlp_sqp.c | 3 --- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 3 --- 2 files changed, 6 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 03e0ece345..deafcd123b 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -346,7 +346,6 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v -// TODO rename ... opts_set_dynamics !!! void ocp_nlp_sqp_dynamics_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -362,7 +361,6 @@ void ocp_nlp_sqp_dynamics_opts_set(void *config_, void *opts_, int stage, -// TODO rename ... opts_set_cost !!! void ocp_nlp_sqp_cost_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -378,7 +376,6 @@ void ocp_nlp_sqp_cost_opts_set(void *config_, void *opts_, int stage, -// TODO rename ... opts_set_constraints !!! void ocp_nlp_sqp_constraints_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 2a30391472..f57786c2de 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -316,7 +316,6 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi -// TODO rename ... opts_set_dynamics !!! void ocp_nlp_sqp_rti_dynamics_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -332,7 +331,6 @@ void ocp_nlp_sqp_rti_dynamics_opts_set(void *config_, void *opts_, int stage, -// TODO rename ... opts_set_cost !!! void ocp_nlp_sqp_rti_cost_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { @@ -348,7 +346,6 @@ void ocp_nlp_sqp_rti_cost_opts_set(void *config_, void *opts_, int stage, -// TODO rename ... opts_set_constraints !!! void ocp_nlp_sqp_rti_constraints_opts_set(void *config_, void *opts_, int stage, const char *field, void *value) { From a05382270ef2aef3b28ca3259c4114a5aa99ca4b Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 29 May 2019 17:00:44 +0200 Subject: [PATCH 26/78] small edit to pendumul examples --- .../example_closed_loop.m | 13 ++++---- .../pendulum_on_cart_model/example_ocp.m | 33 ++++++++++++++++--- 2 files changed, 36 insertions(+), 10 deletions(-) diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m index aec1de08d4..87e92999f7 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -16,8 +16,8 @@ ocp_N = 100; nlp_solver = 'sqp'; %nlp_solver = 'sqp_rti'; -%nlp_solver_exact_hessian = 'false' -nlp_solver_exact_hessian = 'true' +%nlp_solver_exact_hessian = 'false'; +nlp_solver_exact_hessian = 'true'; %regularize_method = 'no_regularize'; %regularize_method = 'project'; regularize_method = 'project_reduc_hess'; @@ -260,7 +260,7 @@ % set trajectory initialization %x_traj_init = zeros(nx, ocp_N+1); %for ii=1:ocp_N x_traj_init(:,ii) = [0; pi; 0; 0]; end -x_traj_init = linspace([0; pi; 0; 0], [0; 0; 0; 0], ocp_N+1); +x_traj_init = [linspace(0, 0, ocp_N+1); linspace(pi, 0, ocp_N+1); linspace(0, 0, ocp_N+1); linspace(0, 0, ocp_N+1)]; u_traj_init = zeros(nu, ocp_N); ocp.set('init_x', x_traj_init); @@ -318,7 +318,8 @@ -% print solution +% figures + for ii=1:n_sim+1 x_cur = x_sim(:,ii); visualize; @@ -326,8 +327,6 @@ -status = ocp.get('status'); - figure(2); subplot(2,1,1); plot(0:n_sim, x_sim); @@ -340,6 +339,8 @@ +status = ocp.get('status'); + if status==0 fprintf('\nsuccess!\n\n'); else diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 8f072afbb6..99492af9b9 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -11,8 +11,8 @@ nlp_solver = 'sqp'; %nlp_solver = 'sqp_rti'; -%nlp_solver_exact_hessian = 'false' -nlp_solver_exact_hessian = 'true' +%nlp_solver_exact_hessian = 'false'; +nlp_solver_exact_hessian = 'true'; %regularize_method = 'no_regularize'; %regularize_method = 'project'; regularize_method = 'project_reduc_hess'; @@ -200,7 +200,7 @@ % set trajectory initialization %x_traj_init = zeros(nx, N+1); %for ii=1:N x_traj_init(:,ii) = [0; pi; 0; 0]; end -x_traj_init = linspace([0; pi; 0; 0], [0; 0; 0; 0], N+1); +x_traj_init = [linspace(0, 0, N+1); linspace(pi, 0, N+1); linspace(0, 0, N+1); linspace(0, 0, N+1)]; u_traj_init = zeros(nu, N); ocp.set('init_x', x_traj_init); @@ -244,7 +244,20 @@ fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); -stat = ocp.get('stat') +stat = ocp.get('stat'); +if (strcmp(nlp_solver, 'sqp')) + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); +else % sqp_rti + fprintf('\niter\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + end + fprintf('\n'); +end % figures @@ -264,6 +277,18 @@ xlim([0 N]); legend('F'); +if (strcmp(nlp_solver, 'sqp')) + figure(3); + plot([0: sqp_iter], log10(stat(:,2)), 'r-x'); + hold on + plot([0: sqp_iter], log10(stat(:,3)), 'b-x'); + plot([0: sqp_iter], log10(stat(:,4)), 'g-x'); + plot([0: sqp_iter], log10(stat(:,5)), 'k-x'); + hold off + xlabel('iter') + ylabel('res') +end + if status==0 From 3860e4b907db685874b1fe7677d2487ae4b9b506 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 30 May 2019 15:37:44 +0200 Subject: [PATCH 27/78] disable exact hessian of constraints --- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 2 ++ acados/ocp_nlp/ocp_nlp_sqp.c | 10 +++++----- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 6 +++--- 3 files changed, 10 insertions(+), 8 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 4bb6ecf1fa..0f18c17f1b 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -927,6 +927,8 @@ void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims_, void mult_in.xi = 0; // TODO check that it is (upper - lower) and not the other way around blasfeo_daxpy(nh, -1.0, memory->lam, nb+ng, memory->lam, 2*nb+2*ng+nh, &work->tmp_nh, 0); +// blasfeo_daxpy(nh, -1.0, memory->lam, 2*nb+2*ng+nh, memory->lam, nb+ng, &work->tmp_nh, 0); +// blasfeo_daxpy(nh, 1.0, memory->lam, nb+ng, memory->lam, 2*nb+2*ng+nh, &work->tmp_nh, 0); struct blasfeo_dmat_args hess_out; hess_out.A = &work->tmp_nv_nv; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index deafcd123b..70811f4d98 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -329,9 +329,9 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v // dynamics for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); - // constraints - for (ii=0; ii<=N; ii++) - config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); + // constraints TODO disabled for now as prevents convergence !!! +// for (ii=0; ii<=N; ii++) +// config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); } else { @@ -451,7 +451,7 @@ int ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void *opts_) size += ocp_nlp_memory_calculate_size(config, dims); // stat - size += 5*opts->max_iter*sizeof(double); + size += 5*(opts->max_iter+1)*sizeof(double); size += 8; // initial align @@ -536,7 +536,7 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *r // stat mem->stat = (double *) c_ptr; - mem->stat_m = opts->max_iter; + mem->stat_m = opts->max_iter+1; mem->stat_n = 5; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index f57786c2de..7563f63e4e 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -299,9 +299,9 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi // dynamics for (ii=0; iidynamics[ii]->opts_set(config->dynamics[ii], opts->dynamics[ii], "compute_hess", value); - // constraints - for (ii=0; ii<=N; ii++) - config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); +// // constraints TODO disabled for now as prevents convergence !!! +// for (ii=0; ii<=N; ii++) +// config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); } else { From 14680b51d58ab69fc1552d5196b3ba20f14eaa26 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 30 May 2019 15:55:52 +0200 Subject: [PATCH 28/78] matlab interface: improve wind turbine examples --- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 1 + .../wind_turbine_nx6/example_closed_loop.m | 63 +++++++++++++++- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 75 ++++++++++++++++++- 3 files changed, 134 insertions(+), 5 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 0f18c17f1b..4672169733 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -920,6 +920,7 @@ void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims_, void jac_out.ai = 0; jac_out.aj = ng; + // TODO check that it is correct, as it prevents convergence !!!!! if (opts->compute_hess) { struct blasfeo_dvec_args mult_in; // multipliers of external fun; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m index 9203408e64..c4ff8aec58 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m @@ -14,11 +14,21 @@ % ocp ocp_param_scheme = 'multiple_shooting_unif_grid'; ocp_N = 40; -%ocp_nlp_solver = 'sqp'; -ocp_nlp_solver = 'sqp_rti'; +ocp_nlp_solver = 'sqp'; +%ocp_nlp_solver = 'sqp_rti'; +%ocp_nlp_solver_exact_hessian = 'false'; +ocp_nlp_solver_exact_hessian = 'true'; +%regularize_method = 'no_regularize'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; +%regularize_method = 'mirror'; +%regularize_method = 'convexify'; +ocp_nlp_solver_max_iter = 50; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; ocp_qp_solver_pcond_N = 5; +ocp_qp_solver_pcond_ric_alg = 0; +ocp_qp_solver_ric_alg = 0; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4; @@ -213,9 +223,16 @@ ocp_opts.set('param_scheme', ocp_param_scheme); ocp_opts.set('param_scheme_N', ocp_N); ocp_opts.set('nlp_solver', ocp_nlp_solver); +ocp_opts.set('nlp_solver_exact_hessian', ocp_nlp_solver_exact_hessian); +ocp_opts.set('regularize_method', regularize_method); +if (strcmp(ocp_nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', ocp_nlp_solver_max_iter); +end ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', ocp_qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', ocp_qp_solver_ric_alg); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); @@ -293,7 +310,7 @@ % get references compute_setup; -n_sim = 40; +n_sim = 100; n_sim_max = length(wind0_ref) - ocp_N; if n_sim>n_sim_max n_sim = s_sim_max; @@ -302,6 +319,8 @@ x_sim(:,1) = x0_ref; % initial state u_sim = zeros(nu, n_sim); +sqp_iter_sim = zeros(n_sim,1); + % set trajectory initialization x_traj_init = repmat(x0_ref, 1, ocp_N+1); u_traj_init = repmat(u0_ref, 1, ocp_N); @@ -315,7 +334,6 @@ % set x0 ocp.set('constr_x0', x_sim(:,ii)); % set parameter - % TODO different parameter at each stage !!!!! % ocp.set('p', wind0_ref(:,ii)); for jj=0:ocp_N-1 ocp.set('p', jj, wind0_ref(:,ii+jj)); @@ -396,12 +414,47 @@ time_lin = ocp.get('time_lin'); time_qp_sol = ocp.get('time_qp_sol'); + sqp_iter_sim(ii) = sqp_iter; + fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms]), Pel = %f\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, electrical_power); end %u_sim %x_sim +electrical_power = 0.944*97/100*x_sim(1,:).*x_sim(6,:); + + +% figures + +figure(1); +subplot(3,1,1); +plot(0:n_sim, x_sim); +xlim([0 n_sim]); +ylabel('states'); +%legend('p', 'theta', 'v', 'omega'); +subplot(3,1,2); +plot(0:n_sim-1, u_sim); +xlim([0 n_sim]); +ylabel('inputs'); +%legend('F'); +subplot(3,1,3); +plot(0:n_sim, electrical_power); +hold on +plot([0 n_sim], [Pel_max Pel_max]); +hold off +xlim([0 n_sim]); +ylim([4.0 6.0]); +ylabel('electrical power'); +%legend('F'); + +figure(2); +plot(1:n_sim, sqp_iter_sim, 'rx'); +hold on +plot([1 n_sim], [ocp_nlp_solver_max_iter ocp_nlp_solver_max_iter]); +hold off +ylim([0 ocp_nlp_solver_max_iter+1]) + if status==0 fprintf('\nsuccess!\n\n'); @@ -410,6 +463,8 @@ end +waitforbuttonpress; + return; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index cc50f2f06f..94955dae81 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -10,10 +10,20 @@ N = 40; nlp_solver = 'sqp'; %nlp_solver = 'sqp_rti'; +%nlp_solver_exact_hessian = 'false'; +nlp_solver_exact_hessian = 'true'; +%regularize_method = 'no_regularize'; +regularize_method = 'project'; +%regularize_method = 'project_reduc_hess'; +%regularize_method = 'mirror'; +%regularize_method = 'convexify'; +nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_pcond_N = 5; -% sim_method = 'erk'; +qp_solver_pcond_ric_alg = 0; +qp_solver_ric_alg = 0; +%sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; sim_method_num_steps = 1; @@ -207,9 +217,16 @@ ocp_opts.set('param_scheme', param_scheme); ocp_opts.set('param_scheme_N', N); ocp_opts.set('nlp_solver', nlp_solver); +ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); +ocp_opts.set('regularize_method', regularize_method); +if (strcmp(nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); end ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); @@ -266,6 +283,9 @@ %electrical_power = 0.944*97/100*x(1,1)*x(6,1) electrical_power = 0.944*97/100*x(1,:).*x(6,:) + +% statistics + status = ocp.get('status'); sqp_iter = ocp.get('sqp_iter'); time_tot = ocp.get('time_tot'); @@ -274,6 +294,57 @@ fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +stat = ocp.get('stat'); +if (strcmp(nlp_solver, 'sqp')) + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); +else % sqp_rti + fprintf('\niter\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + end + fprintf('\n'); +end + + +% figures + +figure(1); +subplot(3,1,1); +plot(0:N, x); +xlim([0 N]); +ylabel('states'); +%legend('p', 'theta', 'v', 'omega'); +subplot(3,1,2); +plot(0:N-1, u); +xlim([0 N]); +ylabel('inputs'); +%legend('F'); +subplot(3,1,3); +plot(0:N, electrical_power); +hold on +plot([0 N], [Pel_max Pel_max]); +hold off +xlim([0 N]); +ylim([4.5 6.0]); +ylabel('electrical power'); +%legend('F'); + +if (strcmp(nlp_solver, 'sqp')) + figure(2); + plot([0: sqp_iter], log10(stat(:,2)), 'r-x'); + hold on + plot([0: sqp_iter], log10(stat(:,3)), 'b-x'); + plot([0: sqp_iter], log10(stat(:,4)), 'g-x'); + plot([0: sqp_iter], log10(stat(:,5)), 'k-x'); + hold off + xlabel('iter') + ylabel('res') +end + if status==0 @@ -283,5 +354,7 @@ end +waitforbuttonpress; + return; From fe77c0d382261509ce8206a846736d74445fce81 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 30 May 2019 16:36:44 +0200 Subject: [PATCH 29/78] matlab interface: add get pi & set pi_init --- acados/ocp_nlp/ocp_nlp_common.c | 18 ++++++++++++++ .../example_closed_loop.m | 10 ++++++-- .../pendulum_on_cart_model/example_ocp.m | 2 ++ .../wind_turbine_nx6/example_closed_loop.m | 8 +++++-- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 4 ++-- interfaces/acados_c/ocp_nlp_interface.c | 10 ++++++++ interfaces/acados_matlab/ocp_get.c | 24 +++++++++++++++++++ interfaces/acados_matlab/ocp_set.c | 8 +++++++ 8 files changed, 78 insertions(+), 6 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 1865989422..8e480519ba 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -624,6 +624,8 @@ int ocp_nlp_in_calculate_size_self(int N) return size; } + + int ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) { int ii; @@ -659,6 +661,8 @@ int ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) return size; } + + ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -689,6 +693,8 @@ ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory) return in; } + + ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) { int ii; @@ -731,6 +737,8 @@ ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void * return in; } + + /************************************************ * out ************************************************/ @@ -770,6 +778,8 @@ int ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) return size; } + + ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) { // extract sizes @@ -838,6 +848,8 @@ ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void return out; } + + /************************************************ * memory ************************************************/ @@ -876,6 +888,8 @@ int ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims) return size; } + + ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory) { @@ -941,6 +955,8 @@ ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims return mem; } + + /************************************************ * residuals ************************************************/ @@ -1036,6 +1052,8 @@ ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory) return res; } + + void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_res *res, ocp_nlp_memory *mem) { diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m index 87e92999f7..397515cc09 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -263,8 +263,8 @@ x_traj_init = [linspace(0, 0, ocp_N+1); linspace(pi, 0, ocp_N+1); linspace(0, 0, ocp_N+1); linspace(0, 0, ocp_N+1)]; u_traj_init = zeros(nu, ocp_N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +pi_traj_init = zeros(nx, ocp_N); + tic; @@ -277,6 +277,7 @@ % set trajectory initialization (if not, set internally using previous solution) ocp.set('init_x', x_traj_init); ocp.set('init_u', u_traj_init); + ocp.set('init_pi', pi_traj_init); % solve OCP ocp.solve(); @@ -294,9 +295,12 @@ % get solution for initialization of next NLP x_traj = ocp.get('x'); u_traj = ocp.get('u'); + pi_traj = ocp.get('pi'); + % shift trajectory for initialization x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; + pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; % get solution for sim u_sim(:,ii) = ocp.get('u', 0); @@ -348,6 +352,8 @@ end +waitforbuttonpress; + return; diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 99492af9b9..ef647eb863 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -298,6 +298,8 @@ end +waitforbuttonpress; + return; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m index c4ff8aec58..e758c48d09 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m @@ -324,6 +324,7 @@ % set trajectory initialization x_traj_init = repmat(x0_ref, 1, ocp_N+1); u_traj_init = repmat(u0_ref, 1, ocp_N); +pi_traj_init = zeros(nx, ocp_N); for ii=1:n_sim @@ -346,9 +347,10 @@ end ocp.set('cost_yr_e', y_ref(:,ii+ocp_N)); - % initialize trajectory + % set trajectory initialization (if not, set internally using previous solution) ocp.set('init_x', x_traj_init); ocp.set('init_u', u_traj_init); + ocp.set('init_pi', pi_traj_init); % x_traj_init % u_traj_init @@ -359,6 +361,7 @@ % get solution x = ocp.get('x'); u = ocp.get('u'); + pi = ocp.get('pi'); % x % u @@ -394,12 +397,13 @@ % simulate state % sim.solve(); - % shift initialization + % shift trajectory for initialization % x_traj_init = [x(:,2:ocp_N+1), zeros(nx, 1)]; x_traj_init = [x(:,2:ocp_N+1), x(:,ocp_N+1)]; % x_traj_init = [x(:,2:ocp_N+1), sim.get('xn')]; % u_traj_init = [u(:,2:ocp_N), zeros(nu, 1)]; u_traj_init = [u(:,2:ocp_N), u(:,ocp_N)]; + pi_traj_init = [pi(:,2:ocp_N), pi(:,ocp_N)]; time_ext = toc; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 94955dae81..fac116142d 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -13,8 +13,8 @@ %nlp_solver_exact_hessian = 'false'; nlp_solver_exact_hessian = 'true'; %regularize_method = 'no_regularize'; -regularize_method = 'project'; -%regularize_method = 'project_reduc_hess'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 88b8f872b7..6956a779e6 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -457,6 +457,11 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou double *double_values = value; blasfeo_pack_dvec(dims->nu[stage], double_values, &out->ux[stage], 0); } + else if (!strcmp(field, "pi")) + { + double *double_values = value; + blasfeo_pack_dvec(dims->nx[stage+1], double_values, &out->pi[stage], 0); + } else { printf("\nerror: ocp_nlp_out_set: field %s not available\n", field); @@ -479,6 +484,11 @@ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou double *double_values = value; blasfeo_unpack_dvec(dims->nu[stage], &out->ux[stage], 0, double_values); } + else if (!strcmp(field, "pi")) + { + double *double_values = value; + blasfeo_unpack_dvec(dims->nx[stage+1], &out->pi[stage], 0, double_values); + } else { printf("\nerror: ocp_nlp_out_get: field %s not available\n", field); diff --git a/interfaces/acados_matlab/ocp_get.c b/interfaces/acados_matlab/ocp_get.c index 6ccfc478f2..ebf1fff1f7 100644 --- a/interfaces/acados_matlab/ocp_get.c +++ b/interfaces/acados_matlab/ocp_get.c @@ -95,6 +95,30 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) return; } } + else if(!strcmp(field, "pi")) + { + if(nrhs==2) + { + plhs[0] = mxCreateNumericMatrix(nx, N, mxDOUBLE_CLASS, mxREAL); + double *pi = mxGetPr( plhs[0] ); + for(ii=0; ii Date: Thu, 30 May 2019 17:28:04 +0200 Subject: [PATCH 30/78] add pivoting in reg_permute_reduc_hess --- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 92 ++++++++++++++++--- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.h | 1 + 2 files changed, 82 insertions(+), 11 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index 70ef2f4767..e7c7739e4c 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -56,6 +56,7 @@ void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_n ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; opts->epsilon = 1e-4; + opts->pivoting = 1; return; } @@ -72,6 +73,11 @@ void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *di double *d_ptr = value; opts->epsilon = *d_ptr; } + else if (!strcmp(field, "pivoting")) + { + int *i_ptr = value; + opts->pivoting = *i_ptr; + } else { printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_opts_set\n", field); @@ -373,7 +379,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) mem_; ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; - int ii, jj, kk, ll, ss; + int ii, jj, kk, ll, ss, idx; //printf("\nhola\n"); int *nx = dims->nx; @@ -398,7 +404,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg // last stage ss = N; blasfeo_dtrtr_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, mem->RSQrq[ss], 0, 0); // necessary ??? - // TODO !!!!!!!!!!!!!!!!!! + // TODO copy middle stage for nu[N}>0 !!!!!!!!!!!!!!!!!! // blasfeo_unpack_dmat(nu[ss], nu[ss], mem->RSQrq[ss], 0, 0, mem->reg_hess, nu[ss]); // acados_project(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon); // blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], mem->RSQrq[ss], 0, 0); @@ -458,27 +464,91 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg //printf("\nL2\n"); //blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L2, 0, 0); +//printf("\nL3\n"); +//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L3, 0, 0); // compute true_schur +// if(1) if(do_reg) { - for(jj=0; jjpivoting) + { + for(jj=0; jj-pivot) + { + pivot = BLASFEO_DMATEL(L3, kk, kk); + idx = kk; + } + } + // symmetric permute + if(idx!=jj) + { + // top triangle + for(kk=jj; kk Date: Thu, 30 May 2019 18:07:48 +0200 Subject: [PATCH 31/78] matlab interface: improve chain of masses examples --- .../masses_chain_model/example_closed_loop.m | 43 +++++++++---- .../masses_chain_model/example_ocp.m | 64 +++++++++++++++---- 2 files changed, 84 insertions(+), 23 deletions(-) diff --git a/examples/matlab_mex/masses_chain_model/example_closed_loop.m b/examples/matlab_mex/masses_chain_model/example_closed_loop.m index 4c95bcfbf0..a5a1a8265d 100644 --- a/examples/matlab_mex/masses_chain_model/example_closed_loop.m +++ b/examples/matlab_mex/masses_chain_model/example_closed_loop.m @@ -15,16 +15,19 @@ ocp_param_scheme = 'multiple_shooting_unif_grid'; ocp_nlp_solver = 'sqp'; %ocp_nlp_solver = 'sqp_rti'; -%nlp_solver_exact_hessian = 'false' -nlp_solver_exact_hessian = 'true' +%ocp_nlp_solver_exact_hessian = 'false'; +ocp_nlp_solver_exact_hessian = 'true'; %regularize_method = 'no_regularize'; -regularize_method = 'project'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; ocp_qp_solver_pcond_N = 5; +ocp_qp_solver_pcond_ric_alg = 0; +ocp_qp_solver_ric_alg = 0; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4; @@ -155,12 +158,16 @@ ocp_opts.set('param_scheme', ocp_param_scheme); ocp_opts.set('param_scheme_N', ocp_N); ocp_opts.set('nlp_solver', ocp_nlp_solver); -ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); +ocp_opts.set('nlp_solver_exact_hessian', ocp_nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +if (strcmp(ocp_nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +end ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', ocp_qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', ocp_qp_solver_ric_alg); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); @@ -236,8 +243,11 @@ x_traj_init = repmat(model.x_ref, 1, ocp_N+1); u_traj_init = zeros(nu, ocp_N); -ocp.set('init_x', x_traj_init); -ocp.set('init_u', u_traj_init); +pi_traj_init = zeros(nx, ocp_N); + +%ocp.set('init_x', x_traj_init); +%ocp.set('init_u', u_traj_init); +%ocp.set('init_pi', pi_traj_init); tic; @@ -247,8 +257,9 @@ ocp.set('constr_x0', x_sim(:,ii)); % set trajectory initialization (if not, set internally using previous solution) -% ocp.set('init_x', x_traj_init); -% ocp.set('init_u', u_traj_init); + ocp.set('init_x', x_traj_init); + ocp.set('init_u', u_traj_init); + ocp.set('init_pi', pi_traj_init); % solve OCP ocp.solve(); @@ -264,8 +275,16 @@ end % get solution - %x_traj = ocp.get('x'); - %u_traj = ocp.get('u'); + x_traj = ocp.get('x'); + u_traj = ocp.get('u'); + pi_traj = ocp.get('pi'); + + % shift trajectory for initialization + x_traj_init = [x_traj(:,2:end), x_traj(:,end)]; + u_traj_init = [u_traj(:,2:end), u_traj(:,end)]; + pi_traj_init = [pi_traj(:,2:end), pi_traj(:,end)]; + + % get solution for sim u_sim(:,ii) = ocp.get('u', 0); % set initial state of sim @@ -302,5 +321,7 @@ end +waitforbuttonpress; + return; diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 1aa658f2d7..19142c4506 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -14,13 +14,16 @@ %nlp_solver_exact_hessian = 'false'; nlp_solver_exact_hessian = 'true'; %regularize_method = 'no_regularize'; -regularize_method = 'project'; +%regularize_method = 'project'; +regularize_method = 'project_reduc_hess'; %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_pcond_N = 5; +qp_solver_pcond_ric_alg = 0; +qp_solver_ric_alg = 0; %dyn_type = 'explicit'; dyn_type = 'implicit'; %dyn_type = 'discrete'; @@ -167,10 +170,14 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); -ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +if (strcmp(nlp_solver, 'sqp')) + ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); +end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); end if (strcmp(dyn_type, 'explicit')) ocp_opts.set('sim_method', 'erk'); @@ -205,7 +212,7 @@ ocp.set('p', T/N); % solve -nrep = 10; +nrep = 1; tic; for rep=1:nrep ocp.set('init_x', x_traj_init); @@ -223,6 +230,32 @@ +% statistics + +status = ocp.get('status'); +sqp_iter = ocp.get('sqp_iter'); +time_tot = ocp.get('time_tot'); +time_lin = ocp.get('time_lin'); +time_qp_sol = ocp.get('time_qp_sol'); + +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); + +stat = ocp.get('stat'); +if (strcmp(nlp_solver, 'sqp')) + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); +else % sqp_rti + fprintf('\niter\tqp_iter\n'); + for ii=1:size(stat,1) + fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + end + fprintf('\n'); +end + + % plot result %figure() %subplot(2, 1, 1) @@ -235,22 +268,26 @@ %xlabel('sample') -% print solution +% figures + for ii=1:N cur_pos = x(:,ii); visualize; end +if (strcmp(nlp_solver, 'sqp')) + figure(2); + plot([0: sqp_iter], log10(stat(:,2)), 'r-x'); + hold on + plot([0: sqp_iter], log10(stat(:,3)), 'b-x'); + plot([0: sqp_iter], log10(stat(:,4)), 'g-x'); + plot([0: sqp_iter], log10(stat(:,5)), 'k-x'); + hold off + xlabel('iter') + ylabel('res') +end -status = ocp.get('status'); -sqp_iter = ocp.get('sqp_iter'); -time_tot = ocp.get('time_tot'); -time_lin = ocp.get('time_lin'); -time_qp_sol = ocp.get('time_qp_sol'); - -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); - if status==0 fprintf('\nsuccess!\n\n'); @@ -259,4 +296,7 @@ end +waitforbuttonpress; + + return; From c6fcfba21b256b28be43b6060481c646b0412d44 Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 31 May 2019 17:38:47 +0200 Subject: [PATCH 32/78] update hpipm --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index c1cfc9f172..3b37bf269b 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit c1cfc9f1728479eef1b6f73e25d02b256e0f12a8 +Subproject commit 3b37bf269b9b22752c29554fa28c4782ba32dbe8 From ef193822ae6dfa71302de61c189c9d4403a5c0ae Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 31 May 2019 17:41:33 +0200 Subject: [PATCH 33/78] add qp_warm_start option to sqp solvers --- acados/ocp_nlp/ocp_nlp_sqp.c | 22 +++++++++++++++++++ acados/ocp_nlp/ocp_nlp_sqp.h | 1 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 16 +++++++++++--- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 1 + .../masses_chain_model/example_ocp.m | 2 ++ .../pendulum_on_cart_model/example_ocp.m | 2 ++ .../matlab_mex/wind_turbine_nx6/example_ocp.m | 2 ++ interfaces/acados_matlab/acados_ocp_opts.m | 2 ++ interfaces/acados_matlab/ocp_create.c | 14 +++++++++++- 9 files changed, 58 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 70811f4d98..5908fd2a81 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -184,6 +184,8 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ opts->num_threads = ACADOS_NUM_THREADS; #endif + opts->qp_warm_start = 0; + // submodules opts // qp solver @@ -282,6 +284,12 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v if(!strcmp(ptr_module, "qp")) { config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field+module_length+1, value); + + if(!strcmp(field, "qp_warm_start")) + { + int* i_ptr = (int *) value; + opts->qp_warm_start = *i_ptr; + } } else // nlp opts { @@ -1270,9 +1278,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // if(sqp_iter==1) // exit(1); + // no warm start at first iteration + if(sqp_iter==0) + { + int tmp_int = 0; + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "warm_start", &tmp_int); + } + // start timer acados_tic(&timer1); + // TODO move qp_out in memory !!!!! (it has to be preserved to do warm start) int qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); @@ -1282,6 +1298,12 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // stop timer mem->time_qp_sol += acados_toc(&timer1); + // restore default warm start + if(sqp_iter==0) + { + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "warm_start", &opts->qp_warm_start); + } + // TODO move into QP solver memory ??? nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index dd53e7c4e7..b5e0b203ef 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -57,6 +57,7 @@ typedef struct int max_iter; int reuse_workspace; int num_threads; + int qp_warm_start; } ocp_nlp_sqp_opts; // diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 7563f63e4e..0a8d28e8b1 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -282,6 +282,12 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi if(!strcmp(ptr_module, "qp")) { config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field+module_length+1, value); + + if(!strcmp(field, "qp_warm_start")) + { + int* i_ptr = (int *) value; + opts->qp_warm_start = *i_ptr; + } } else // nlp opts { @@ -1177,12 +1183,16 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // print_ocp_qp_in(work->qp_in); // exit(1); + // TODO no warm start across NLP solutions (yet) + int tmp_int = 0; + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "warm_start", &tmp_int); + // start timer acados_tic(&timer1); - int qp_status = - qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, - mem->qp_solver_mem, work->qp_work); + // TODO move qp_out in memory !!!!! (it has to be preserved to do warm start) + int qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, + mem->qp_solver_mem, work->qp_work); // stop timer mem->time_qp_sol += acados_toc(&timer1); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 1003d041f0..16cfe2488f 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -52,6 +52,7 @@ typedef struct int compute_dual_sol; int reuse_workspace; int num_threads; + int qp_warm_start; } ocp_nlp_sqp_rti_opts; // diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 19142c4506..a9b0342ad6 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -24,6 +24,7 @@ qp_solver_pcond_N = 5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; +qp_solver_warm_start = 0; %dyn_type = 'explicit'; dyn_type = 'implicit'; %dyn_type = 'discrete'; @@ -178,6 +179,7 @@ ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); + ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end if (strcmp(dyn_type, 'explicit')) ocp_opts.set('sim_method', 'erk'); diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index ef647eb863..e69e696725 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -24,6 +24,7 @@ qp_solver_pcond_N = 5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; +qp_solver_warm_start = 0; %sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; @@ -179,6 +180,7 @@ ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); + ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index fac116142d..eb80188ef4 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -23,6 +23,7 @@ qp_solver_pcond_N = 5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; +qp_solver_warm_start = 0; %sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; @@ -227,6 +228,7 @@ ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); + ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 8082f8ed14..3ab845ad9c 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -50,6 +50,8 @@ obj.opts_struct.qp_solver_pcond_ric_alg = value; elseif (strcmp(field, 'qp_solver_ric_alg')) obj.opts_struct.qp_solver_ric_alg = value; + elseif (strcmp(field, 'qp_solver_warm_start')) + obj.opts_struct.qp_solver_warm_start = value; elseif (strcmp(field, 'sim_method')) obj.opts_struct.sim_method = value; elseif (strcmp(field, 'sim_method_num_stages')) diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index a8287d704f..432dbcbdfd 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -623,6 +623,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) int qp_solver_pcond_N; bool set_qp_solver_pcond_N = false; int qp_solver_pcond_ric_alg; bool set_qp_solver_pcond_ric_alg = false; int qp_solver_ric_alg; bool set_qp_solver_ric_alg = false; + int qp_solver_warm_start; bool set_qp_solver_warm_start = false; char *sim_method; int sim_method_num_stages; bool set_sim_method_num_stages = false; int sim_method_num_steps; bool set_sim_method_num_steps = false; @@ -676,6 +677,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) set_qp_solver_ric_alg = true; qp_solver_ric_alg = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_ric_alg" ) ); } + // qp solver: warm start + if(mxGetField( prhs[1], 0, "qp_solver_warm_start" )!=NULL) + { + set_qp_solver_warm_start = true; + qp_solver_warm_start = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_warm_start" ) ); + } // sim_method // TODO check sim_method = mxArrayToString( mxGetField( prhs[1], 0, "sim_method" ) ); @@ -1074,11 +1081,16 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "qp_pcond_ric_alg", &qp_solver_pcond_ric_alg); } - // qp_solver_ric_alg + // qp_solver_ric_alg TODO only for hpipm !!! if(set_qp_solver_ric_alg) { ocp_nlp_opts_set(config, opts, "qp_ric_alg", &qp_solver_ric_alg); } + // qp_solver_warm_start + if(set_qp_solver_warm_start) + { + ocp_nlp_opts_set(config, opts, "qp_warm_start", &qp_solver_warm_start); + } // sim_method_num_stages if(set_sim_method_num_stages) { From df32f87d13f2c2207a239ad99ca936e09a7d2c81 Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 31 May 2019 18:15:28 +0200 Subject: [PATCH 34/78] pass tolerances of nlp to qp too --- acados/ocp_nlp/ocp_nlp_sqp.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 5908fd2a81..34c4e90b26 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -190,6 +190,11 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ // qp solver qp_solver->opts_initialize_default(qp_solver, dims->qp_solver, opts->qp_solver_opts); + // overwrite default + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_stat", &opts->min_res_g); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_eq", &opts->min_res_b); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_ineq", &opts->min_res_d); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_comp", &opts->min_res_m); // regularization regularize->opts_initialize_default(regularize, dims->regularize, opts->regularize); @@ -308,25 +313,33 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v int* num_threads = (int *) value; opts->num_threads = *num_threads; } - else if (!strcmp(field, "min_res_g")) + else if (!strcmp(field, "min_res_g")) // TODO rename !!! { double* min_res_g = (double *) value; opts->min_res_g = *min_res_g; + // pass to QP too + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_stat", value); } - else if (!strcmp(field, "min_res_b")) + else if (!strcmp(field, "min_res_b")) // TODO rename !!! { double* min_res_b = (double *) value; opts->min_res_b = *min_res_b; + // pass to QP too + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_eq", value); } - else if (!strcmp(field, "min_res_d")) + else if (!strcmp(field, "min_res_d")) // TODO rename !!! { double* min_res_d = (double *) value; opts->min_res_d = *min_res_d; + // pass to QP too + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_ineq", value); } - else if (!strcmp(field, "min_res_m")) + else if (!strcmp(field, "min_res_m")) // TODO rename !!! { double* min_res_m = (double *) value; opts->min_res_m = *min_res_m; + // pass to QP too + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_comp", value); } else if (!strcmp(field, "exact_hess")) { From 05514c1bd1a2d4429418a98d0a8fb236fd7328b8 Mon Sep 17 00:00:00 2001 From: giaf Date: Sat, 1 Jun 2019 17:27:17 +0200 Subject: [PATCH 35/78] zero primal sol before calling hpipm solvers --- acados/dense_qp/dense_qp_hpipm.c | 19 ++++++++++++- acados/ocp_qp/ocp_qp_hpipm.c | 19 ++++++++++--- .../ocp_qp/ocp_qp_partial_condensing_solver.c | 27 ++++++++++++------- 3 files changed, 52 insertions(+), 13 deletions(-) diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index d5f3468d67..03d17ae1e9 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -139,7 +139,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 +162,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 +188,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_; diff --git a/acados/ocp_qp/ocp_qp_hpipm.c b/acados/ocp_qp/ocp_qp_hpipm.c index fb0c8ad834..5242cd7537 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.c +++ b/acados/ocp_qp/ocp_qp_hpipm.c @@ -238,13 +238,25 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void * ocp_qp_in *qp_in = qp_in_; ocp_qp_out *qp_out = qp_out_; - ocp_qp_info *info = (ocp_qp_info *) qp_out->misc; + ocp_qp_info *info = qp_out->misc; acados_timer tot_timer, qp_timer; acados_tic(&tot_timer); // cast data structures - ocp_qp_hpipm_opts *opts = (ocp_qp_hpipm_opts *) opts_; - ocp_qp_hpipm_memory *memory = (ocp_qp_hpipm_memory *) mem_; + ocp_qp_hpipm_opts *opts = opts_; + ocp_qp_hpipm_memory *memory = mem_; + + // zero primal solution + // TODO add a check if warm start of first SQP iteration is implemented !!!!!! + int ii; + int N = qp_in->dim->N; + int *nx = qp_in->dim->nx; + int *nu = qp_in->dim->nu; + int *ns = qp_in->dim->ns; + for(ii=0; ii<=N; ii++) + { + blasfeo_dvecse(nu[ii]+nx[ii]+2*ns[ii], 0.0, qp_out->ux+ii, 0); + } // solve ipm acados_tic(&qp_timer); @@ -262,6 +274,7 @@ int ocp_qp_hpipm(void *config_, void *qp_in_, void *qp_out_, void *opts_, void * if (hpipm_status == 0) acados_status = ACADOS_SUCCESS; if (hpipm_status == 1) acados_status = ACADOS_MAXITER; if (hpipm_status == 2) acados_status = ACADOS_MINSTEP; + return acados_status; } diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 6a94295d4c..72bc0db984 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -199,6 +199,8 @@ int ocp_qp_partial_condensing_solver_memory_calculate_size(void *config_, ocp_qp return size; } + + void *ocp_qp_partial_condensing_solver_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory) { @@ -274,6 +276,8 @@ void *ocp_qp_partial_condensing_solver_memory_assign(void *config_, ocp_qp_dims return mem; } + + /************************************************ * workspace ************************************************/ @@ -305,6 +309,8 @@ int ocp_qp_partial_condensing_solver_workspace_calculate_size(void *config_, ocp return size; } + + static void cast_workspace(void *config_, ocp_qp_dims *dims, ocp_qp_partial_condensing_solver_opts *opts, ocp_qp_partial_condensing_solver_memory *mem, @@ -335,6 +341,8 @@ static void cast_workspace(void *config_, ocp_qp_dims *dims, c_ptr += qp_solver->workspace_calculate_size(qp_solver, pcond_dims, opts->qp_solver_opts); } + + /************************************************ * functions ************************************************/ @@ -350,11 +358,9 @@ int ocp_qp_partial_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out acados_tic(&tot_timer); // cast data structures - ocp_qp_partial_condensing_solver_opts *opts = (ocp_qp_partial_condensing_solver_opts *) opts_; - ocp_qp_partial_condensing_solver_memory *memory = - (ocp_qp_partial_condensing_solver_memory *) mem_; - ocp_qp_partial_condensing_solver_workspace *work = - (ocp_qp_partial_condensing_solver_workspace *) work_; + ocp_qp_partial_condensing_solver_opts *opts = opts_; + ocp_qp_partial_condensing_solver_memory *memory = mem_; + ocp_qp_partial_condensing_solver_workspace *work = work_; // cast workspace cast_workspace(config_, qp_in->dim, opts, memory, work); @@ -377,9 +383,8 @@ int ocp_qp_partial_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out int solver_status; if (memory->solver_memory != NULL) { - solver_status = - qp_solver->evaluate(qp_solver, memory->pcond_qp_in, memory->pcond_qp_out, - opts->qp_solver_opts, memory->solver_memory, work->solver_work); + solver_status = qp_solver->evaluate(qp_solver, memory->pcond_qp_in, memory->pcond_qp_out, + opts->qp_solver_opts, memory->solver_memory, work->solver_work); } else { @@ -390,8 +395,10 @@ int ocp_qp_partial_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out if (opts->pcond_opts->N2 < qp_in->dim->N) { acados_tic(&cond_timer); + ocp_qp_partial_expansion(memory->pcond_qp_out, qp_out, opts->pcond_opts, - memory->pcond_memory, work->pcond_work); + memory->pcond_memory, work->pcond_work); + info->condensing_time += acados_toc(&cond_timer); } @@ -404,6 +411,8 @@ int ocp_qp_partial_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out return solver_status; } + + void ocp_qp_partial_condensing_solver_config_initialize_default(void *config_) { ocp_qp_xcond_solver_config *config = config_; From 24030f85659f01ba47b48f3f1d573e1ce8c38c7d Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 11:35:54 +0200 Subject: [PATCH 36/78] add check that env.sh is sourced --- examples/matlab_mex/linear_mass_spring_model/env.sh | 3 +++ .../linear_mass_spring_model/example_closed_loop.m | 11 +++++++++++ .../linear_mass_spring_model/example_ocp.m | 10 ++++++++++ .../linear_mass_spring_model/example_sim.m | 10 ++++++++++ examples/matlab_mex/masses_chain_model/env.sh | 3 +++ .../masses_chain_model/example_closed_loop.m | 11 +++++++++++ examples/matlab_mex/masses_chain_model/example_ocp.m | 10 ++++++++++ examples/matlab_mex/pendulum_on_cart_model/env.sh | 3 +++ .../pendulum_on_cart_model/example_closed_loop.m | 10 ++++++++++ .../matlab_mex/pendulum_on_cart_model/example_ocp.m | 12 +++++++++++- .../matlab_mex/pendulum_on_cart_model/example_sim.m | 10 ++++++++++ examples/matlab_mex/wind_turbine_nx6/env.sh | 3 +++ .../wind_turbine_nx6/example_closed_loop.m | 10 ++++++++++ examples/matlab_mex/wind_turbine_nx6/example_ocp.m | 10 ++++++++++ examples/matlab_mex/wind_turbine_nx6/example_sim.m | 10 ++++++++++ 15 files changed, 125 insertions(+), 1 deletion(-) diff --git a/examples/matlab_mex/linear_mass_spring_model/env.sh b/examples/matlab_mex/linear_mass_spring_model/env.sh index bf97e0e357..55d3c74397 100644 --- a/examples/matlab_mex/linear_mass_spring_model/env.sh +++ b/examples/matlab_mex/linear_mass_spring_model/env.sh @@ -10,6 +10,9 @@ else exit fi +# check that this file is run +export ENV_RUN=true + # if acados folder not specified assume parent of this folder ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} export ACADOS_INSTALL_DIR diff --git a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m index 7332f2a8ee..8d8f915886 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m @@ -2,6 +2,17 @@ clear all + +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% handy arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index 7409d4254e..4b3345ec06 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/linear_mass_spring_model/example_sim.m b/examples/matlab_mex/linear_mass_spring_model/example_sim.m index 6b4af16f26..f69984c3cb 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_sim.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_sim.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/masses_chain_model/env.sh b/examples/matlab_mex/masses_chain_model/env.sh index 69a6030d92..cfc5929853 100644 --- a/examples/matlab_mex/masses_chain_model/env.sh +++ b/examples/matlab_mex/masses_chain_model/env.sh @@ -10,6 +10,9 @@ else exit fi +# check that this file is run +export ENV_RUN=true + # if acados folder not specified assume parent of this folder ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} export ACADOS_INSTALL_DIR diff --git a/examples/matlab_mex/masses_chain_model/example_closed_loop.m b/examples/matlab_mex/masses_chain_model/example_closed_loop.m index a5a1a8265d..d2e708ca47 100644 --- a/examples/matlab_mex/masses_chain_model/example_closed_loop.m +++ b/examples/matlab_mex/masses_chain_model/example_closed_loop.m @@ -2,6 +2,17 @@ clear all + +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% handy arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index a9b0342ad6..0d807c45ef 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/pendulum_on_cart_model/env.sh b/examples/matlab_mex/pendulum_on_cart_model/env.sh index bf97e0e357..55d3c74397 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/env.sh +++ b/examples/matlab_mex/pendulum_on_cart_model/env.sh @@ -10,6 +10,9 @@ else exit fi +# check that this file is run +export ENV_RUN=true + # if acados folder not specified assume parent of this folder ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} export ACADOS_INSTALL_DIR diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m index 397515cc09..578bc23d50 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index e69e696725..21c9b8ecf0 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; @@ -24,7 +34,7 @@ qp_solver_pcond_N = 5; qp_solver_pcond_ric_alg = 0; qp_solver_ric_alg = 0; -qp_solver_warm_start = 0; +qp_solver_warm_start = 2; %sim_method = 'erk'; sim_method = 'irk'; sim_method_num_stages = 4; diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_sim.m b/examples/matlab_mex/pendulum_on_cart_model/example_sim.m index e8e25ce75e..b923c77a9a 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_sim.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_sim.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/wind_turbine_nx6/env.sh b/examples/matlab_mex/wind_turbine_nx6/env.sh index 69a6030d92..cfc5929853 100644 --- a/examples/matlab_mex/wind_turbine_nx6/env.sh +++ b/examples/matlab_mex/wind_turbine_nx6/env.sh @@ -10,6 +10,9 @@ else exit fi +# check that this file is run +export ENV_RUN=true + # if acados folder not specified assume parent of this folder ACADOS_INSTALL_DIR=${ACADOS_INSTALL_DIR:-"$(pwd)/../../.."} export ACADOS_INSTALL_DIR diff --git a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m index e758c48d09..2b844c4268 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index eb80188ef4..341779a9e8 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + %% arguments compile_mex = 'true'; codgen_model = 'true'; diff --git a/examples/matlab_mex/wind_turbine_nx6/example_sim.m b/examples/matlab_mex/wind_turbine_nx6/example_sim.m index c03baa6f35..81fa25d165 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_sim.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_sim.m @@ -3,6 +3,16 @@ +% check that env.sh has been run +env_run = getenv('ENV_RUN'); +if (~strcmp(env_run, 'true')) + disp('ERROR: env.sh has not been sourced! Before executing this example, run:'); + disp('source env.sh'); + return; +end + + + % load sim data load testSim.mat From f3babb41292eb5a67783b6d8475982fceb30e711 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 12:22:14 +0200 Subject: [PATCH 37/78] renamed pcond N2 => cond N --- acados/ocp_qp/ocp_qp_partial_condensing.c | 4 +-- .../ocp_qp/ocp_qp_partial_condensing_solver.c | 2 +- examples/c/wind_turbine_nmpc.c | 4 +-- .../example_closed_loop.m | 4 +-- .../linear_mass_spring_model/example_ocp.m | 4 +-- .../masses_chain_model/example_closed_loop.m | 8 ++--- .../masses_chain_model/example_ocp.m | 8 ++--- .../example_closed_loop.m | 10 ++++--- .../pendulum_on_cart_model/example_ocp.m | 8 ++--- .../wind_turbine_nx6/example_closed_loop.m | 8 ++--- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 8 ++--- interfaces/acados_c/options_interface.c | 10 +++---- interfaces/acados_matlab/acados_ocp_opts.m | 8 ++--- interfaces/acados_matlab/ocp_create.c | 30 +++++++++---------- test/ocp_nlp/test_wind_turbine.cpp | 4 +-- 15 files changed, 61 insertions(+), 59 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_partial_condensing.c b/acados/ocp_qp/ocp_qp_partial_condensing.c index 41b5fadf54..81b3220117 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing.c @@ -136,12 +136,12 @@ void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* va ocp_qp_partial_condensing_opts *opts = opts_; - if(!strcmp(field, "N2")) + if(!strcmp(field, "N")) { int *tmp_ptr = value; opts->N2 = *tmp_ptr; } - else if(!strcmp(field, "N2_bkp")) + else if(!strcmp(field, "N_bkp")) { int *tmp_ptr = value; opts->N2_bkp = *tmp_ptr; diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index 72bc0db984..afddc3ae28 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -141,7 +141,7 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const ptr_module = module; } - if(!strcmp(ptr_module, "pcond")) // pass options to pcond module + if(!strcmp(ptr_module, "cond")) // pass options to (partial) condensing module { // TODO config !!! ocp_qp_partial_condensing_opts_set(opts->pcond_opts, field+module_length+1, value); diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index a6fd9e7940..3adf1236b5 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -873,8 +873,8 @@ int main() // partial condensing opts if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { - int pcond_N2 = 5; - ocp_nlp_opts_set(config, nlp_opts, "qp_pcond_N2", &pcond_N2); + int cond_N = 5; + ocp_nlp_opts_set(config, nlp_opts, "qp_cond_N", &cond_N); } // update opts after manual changes diff --git a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m index 8d8f915886..6590d0bac9 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_closed_loop.m @@ -27,7 +27,7 @@ %ocp_nlp_solver = 'sqp_rti'; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_pcond_N = 5; +ocp_qp_solver_cond_N = 5; ocp_sim_method = 'erk'; %ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 2; @@ -139,7 +139,7 @@ ocp_opts.set('nlp_solver', ocp_nlp_solver); ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); + ocp_opts.set('qp_solver_cond_N', ocp_qp_solver_cond_N); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index 4b3345ec06..36ab777621 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -30,7 +30,7 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; +qp_solver_cond_N = 5; %dyn_type = 'explicit'; %dyn_type = 'implicit'; dyn_type = 'discrete'; @@ -211,7 +211,7 @@ ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); end if (strcmp(dyn_type, 'explicit')) ocp_opts.set('sim_method', 'erk'); diff --git a/examples/matlab_mex/masses_chain_model/example_closed_loop.m b/examples/matlab_mex/masses_chain_model/example_closed_loop.m index d2e708ca47..a0a64c0cc9 100644 --- a/examples/matlab_mex/masses_chain_model/example_closed_loop.m +++ b/examples/matlab_mex/masses_chain_model/example_closed_loop.m @@ -36,8 +36,8 @@ nlp_solver_max_iter = 100; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_pcond_N = 5; -ocp_qp_solver_pcond_ric_alg = 0; +ocp_qp_solver_cond_N = 5; +ocp_qp_solver_cond_ric_alg = 0; ocp_qp_solver_ric_alg = 0; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; @@ -176,8 +176,8 @@ end ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', ocp_qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', ocp_qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', ocp_qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', ocp_qp_solver_ric_alg); end ocp_opts.set('sim_method', ocp_sim_method); diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 0d807c45ef..0fe12eff14 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -31,8 +31,8 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; -qp_solver_pcond_ric_alg = 0; +qp_solver_cond_N = 5; +qp_solver_cond_ric_alg = 0; qp_solver_ric_alg = 0; qp_solver_warm_start = 0; %dyn_type = 'explicit'; @@ -186,8 +186,8 @@ end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m index 578bc23d50..6fb0e57e41 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_closed_loop.m @@ -36,9 +36,10 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; -qp_solver_pcond_ric_alg = 0; +qp_solver_cond_N = 5; +qp_solver_cond_ric_alg = 0; qp_solver_ric_alg = 0; +qp_solver_warm_start = 0; ocp_sim_method = 'erk'; %ocp_sim_method = 'irk'; ocp_sim_method_num_stages = 4; @@ -191,9 +192,10 @@ end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); + ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end ocp_opts.set('sim_method', ocp_sim_method); ocp_opts.set('sim_method_num_stages', ocp_sim_method_num_stages); diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 21c9b8ecf0..613945eb18 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -31,8 +31,8 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; -qp_solver_pcond_ric_alg = 0; +qp_solver_cond_N = 5; +qp_solver_cond_ric_alg = 0; qp_solver_ric_alg = 0; qp_solver_warm_start = 2; %sim_method = 'erk'; @@ -187,8 +187,8 @@ end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end diff --git a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m index 2b844c4268..bb27fcd140 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_closed_loop.m @@ -36,8 +36,8 @@ ocp_nlp_solver_max_iter = 50; ocp_qp_solver = 'partial_condensing_hpipm'; %ocp_qp_solver = 'full_condensing_hpipm'; -ocp_qp_solver_pcond_N = 5; -ocp_qp_solver_pcond_ric_alg = 0; +ocp_qp_solver_cond_N = 5; +ocp_qp_solver_cond_ric_alg = 0; ocp_qp_solver_ric_alg = 0; %ocp_sim_method = 'erk'; ocp_sim_method = 'irk'; @@ -240,8 +240,8 @@ end ocp_opts.set('qp_solver', ocp_qp_solver); if (strcmp(ocp_qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', ocp_qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', ocp_qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', ocp_qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', ocp_qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', ocp_qp_solver_ric_alg); end ocp_opts.set('sim_method', ocp_sim_method); diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 341779a9e8..3354789f40 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -30,8 +30,8 @@ nlp_solver_max_iter = 100; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; -qp_solver_pcond_N = 5; -qp_solver_pcond_ric_alg = 0; +qp_solver_cond_N = 5; +qp_solver_cond_ric_alg = 0; qp_solver_ric_alg = 0; qp_solver_warm_start = 0; %sim_method = 'erk'; @@ -235,8 +235,8 @@ end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) - ocp_opts.set('qp_solver_pcond_N', qp_solver_pcond_N); - ocp_opts.set('qp_solver_pcond_ric_alg', qp_solver_pcond_ric_alg); + ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); + ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end diff --git a/interfaces/acados_c/options_interface.c b/interfaces/acados_c/options_interface.c index 25c27de78f..aaf8b9a9af 100644 --- a/interfaces/acados_c/options_interface.c +++ b/interfaces/acados_c/options_interface.c @@ -107,7 +107,7 @@ bool set_option_int(void *args_, const char *option, const int value) args->hpipm_opts->iter_max = value; else if (!strcmp(token, "max_stat")) args->hpipm_opts->stat_max = value; - else if (!strcmp(token, "pcond_N2")) + else if (!strcmp(token, "cond_N")) pcond_opts->N2 = value; else return false; @@ -140,7 +140,7 @@ bool set_option_int(void *args_, const char *option, const int value) args->warm_start = value; // NOTE(dimitris): HPMPC partial condesing has a bug, using hpipm partial condensing // instead - else if (!strcmp(token, "pcond_N2")) + else if (!strcmp(token, "cond_N")) pcond_opts->N2 = value; // partial tightening else if (!strcmp(token, "N")) @@ -162,7 +162,7 @@ bool set_option_int(void *args_, const char *option, const int value) (ocp_qp_partial_condensing_opts *) sparse_args->pcond_opts; if (!strcmp(token, "print_level")) args->printLevel = value; - else if (!strcmp(token, "pcond_N2")) + else if (!strcmp(token, "cond_N")) pcond_opts->N2 = value; else return false; @@ -188,7 +188,7 @@ bool set_option_int(void *args_, const char *option, const int value) ocp_qp_osqp_opts *args = (ocp_qp_osqp_opts *) sparse_args->qp_solver_opts; ocp_qp_partial_condensing_opts *pcond_opts = (ocp_qp_partial_condensing_opts *) sparse_args->pcond_opts; - if (!strcmp(token, "pcond_N2")) + if (!strcmp(token, "cond_N")) pcond_opts->N2 = value; else return false; @@ -215,7 +215,7 @@ bool set_option_int(void *args_, const char *option, const int value) { args->options.maxIter = value; } - else if (!strcmp(token, "pcond_N2")) + else if (!strcmp(token, "cond_N")) { pcond_opts->N2 = value; } diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 3ab845ad9c..8ef028a63e 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -44,10 +44,10 @@ obj.opts_struct.nlp_solver_max_iter = value; elseif (strcmp(field, 'qp_solver')) obj.opts_struct.qp_solver = value; - elseif (strcmp(field, 'qp_solver_pcond_N')) - obj.opts_struct.qp_solver_pcond_N = value; - elseif (strcmp(field, 'qp_solver_pcond_ric_alg')) - obj.opts_struct.qp_solver_pcond_ric_alg = value; + elseif (strcmp(field, 'qp_solver_cond_N')) + obj.opts_struct.qp_solver_cond_N = value; + elseif (strcmp(field, 'qp_solver_cond_ric_alg')) + obj.opts_struct.qp_solver_cond_ric_alg = value; elseif (strcmp(field, 'qp_solver_ric_alg')) obj.opts_struct.qp_solver_ric_alg = value; elseif (strcmp(field, 'qp_solver_warm_start')) diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 432dbcbdfd..df7a3931fe 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -620,8 +620,8 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) bool nlp_solver_exact_hessian; int nlp_solver_max_iter; bool set_nlp_solver_max_iter = false; char *qp_solver; - int qp_solver_pcond_N; bool set_qp_solver_pcond_N = false; - int qp_solver_pcond_ric_alg; bool set_qp_solver_pcond_ric_alg = false; + int qp_solver_cond_N; bool set_qp_solver_cond_N = false; + int qp_solver_cond_ric_alg; bool set_qp_solver_cond_ric_alg = false; int qp_solver_ric_alg; bool set_qp_solver_ric_alg = false; int qp_solver_warm_start; bool set_qp_solver_warm_start = false; char *sim_method; @@ -660,16 +660,16 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) // TODO check qp_solver = mxArrayToString( mxGetField( prhs[1], 0, "qp_solver" ) ); // N_part_cond - if(mxGetField( prhs[1], 0, "qp_solver_pcond_N" )!=NULL) + if(mxGetField( prhs[1], 0, "qp_solver_cond_N" )!=NULL) { - set_qp_solver_pcond_N = true; - qp_solver_pcond_N = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_pcond_N" ) ); + set_qp_solver_cond_N = true; + qp_solver_cond_N = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_cond_N" ) ); } - // pcond riccati-like algorithm - if(mxGetField( prhs[1], 0, "qp_solver_pcond_ric_alg" )!=NULL) + // cond riccati-like algorithm + if(mxGetField( prhs[1], 0, "qp_solver_cond_ric_alg" )!=NULL) { - set_qp_solver_pcond_ric_alg = true; - qp_solver_pcond_ric_alg = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_pcond_ric_alg" ) ); + set_qp_solver_cond_ric_alg = true; + qp_solver_cond_ric_alg = mxGetScalar( mxGetField( prhs[1], 0, "qp_solver_cond_ric_alg" ) ); } // hpipm: riccati algorithm if(mxGetField( prhs[1], 0, "qp_solver_ric_alg" )!=NULL) @@ -1071,15 +1071,15 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "max_iter", &nlp_solver_max_iter); } - // qp_solver_pcond_N - if(set_qp_solver_pcond_N) + // qp_solver_cond_N + if(set_qp_solver_cond_N) { - ocp_nlp_opts_set(config, opts, "qp_pcond_N2", &qp_solver_pcond_N); + ocp_nlp_opts_set(config, opts, "qp_cond_N", &qp_solver_cond_N); } - // qp_solver_pcond_ric alg - if(set_qp_solver_pcond_N) + // qp_solver_cond_ric alg + if(set_qp_solver_cond_N) { - ocp_nlp_opts_set(config, opts, "qp_pcond_ric_alg", &qp_solver_pcond_ric_alg); + ocp_nlp_opts_set(config, opts, "qp_cond_ric_alg", &qp_solver_cond_ric_alg); } // qp_solver_ric_alg TODO only for hpipm !!! if(set_qp_solver_ric_alg) diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index c91036f82b..e191edd4bb 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -945,8 +945,8 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q // partial condensing if (plan->ocp_qp_solver_plan.qp_solver == PARTIAL_CONDENSING_HPIPM) { - int pcond_N2 = 10; - ocp_nlp_opts_set(config, nlp_opts, "qp_pcond_N2", &pcond_N2); + int cond_N = 10; + ocp_nlp_opts_set(config, nlp_opts, "qp_cond_N", &cond_N); } config->opts_update(config, dims, nlp_opts); From 48ae1ce1eb017debeba1d4a7453d4129d26a8d45 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 12:56:49 +0200 Subject: [PATCH 38/78] add full cond opts set & fixes to full cond solver opts set --- acados/ocp_qp/ocp_qp_full_condensing.c | 82 +++++++++++++++++-- acados/ocp_qp/ocp_qp_full_condensing.h | 13 ++- acados/ocp_qp/ocp_qp_full_condensing_solver.c | 71 ++++++++++++---- acados/ocp_qp/ocp_qp_full_condensing_solver.h | 8 ++ .../ocp_qp/ocp_qp_partial_condensing_solver.c | 4 +- .../mass_spring_offline_fcond_qpoases_split.c | 7 +- 6 files changed, 151 insertions(+), 34 deletions(-) diff --git a/acados/ocp_qp/ocp_qp_full_condensing.c b/acados/ocp_qp/ocp_qp_full_condensing.c index c50a5de552..42c85799a2 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.c +++ b/acados/ocp_qp/ocp_qp_full_condensing.c @@ -18,7 +18,9 @@ */ // external +#include #include +#include // hpipm #include "hpipm/include/hpipm_d_cond.h" #include "hpipm/include/hpipm_d_dense_qp.h" @@ -33,6 +35,8 @@ #include "acados/utils/mem.h" #include "acados/utils/types.h" + + /************************************************ * dims ************************************************/ @@ -42,6 +46,8 @@ void compute_dense_qp_dims(ocp_qp_dims *dims, dense_qp_dims *ddims) d_compute_qp_dim_ocp2dense(dims, ddims); } + + /************************************************ * opts ************************************************/ @@ -60,6 +66,8 @@ int ocp_qp_full_condensing_opts_calculate_size(ocp_qp_dims *dims) return size; } + + void *ocp_qp_full_condensing_opts_assign(ocp_qp_dims *dims, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -83,19 +91,66 @@ void *ocp_qp_full_condensing_opts_assign(ocp_qp_dims *dims, void *raw_memory) return opts; } + + void ocp_qp_full_condensing_opts_initialize_default(ocp_qp_dims *dims, void *opts_) { ocp_qp_full_condensing_opts *opts = opts_; // condense both Hessian and gradient by default - opts->condense_rhs_only = 0; + opts->cond_hess = 1; // expand only primal solution (linear MPC, Gauss-Newton) - opts->expand_primal_sol_only = 0; + opts->expand_dual_sol = 1; // hpipm_opts d_set_default_cond_qp_ocp2dense_arg(opts->hpipm_opts); } -void ocp_qp_full_condensing_opts_update(ocp_qp_dims *dims, void *opts_) { return; } + + +void ocp_qp_full_condensing_opts_update(ocp_qp_dims *dims, void *opts_) +{ + ocp_qp_full_condensing_opts *opts = opts_; + + // hpipm_opts + d_set_cond_qp_ocp2dense_arg_ric_alg(opts->ric_alg, opts->hpipm_opts); + + return; +} + + + +void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value) +{ + + ocp_qp_full_condensing_opts *opts = opts_; + + if(!strcmp(field, "ric_alg")) + { + int *tmp_ptr = value; + opts->ric_alg = *tmp_ptr; + } + else if(!strcmp(field, "hess")) + { + int *tmp_ptr = value; + opts->cond_hess = *tmp_ptr; + } + else if(!strcmp(field, "dual_sol")) + { + int *tmp_ptr = value; + opts->expand_dual_sol = *tmp_ptr; + } + else + { + printf("\nerror: field %s not available in ocp_qp_full_condensing_opts_set\n", field); + exit(1); + } + + return; + +} + + + /************************************************ * memory ************************************************/ @@ -112,6 +167,8 @@ int ocp_qp_full_condensing_memory_calculate_size(ocp_qp_dims *dims, void *opts_) return size; } + + void *ocp_qp_full_condensing_memory_assign(ocp_qp_dims *dims, void *opts_, void *raw_memory) { ocp_qp_full_condensing_opts *opts = opts_; @@ -135,7 +192,15 @@ void *ocp_qp_full_condensing_memory_assign(ocp_qp_dims *dims, void *opts_, void return mem; } -int ocp_qp_full_condensing_workspace_calculate_size(ocp_qp_dims *dims, void *opts_) { return 0; } + + +int ocp_qp_full_condensing_workspace_calculate_size(ocp_qp_dims *dims, void *opts_) +{ +return 0; +} + + + void ocp_qp_full_condensing(ocp_qp_in *in, dense_qp_in *out, ocp_qp_full_condensing_opts *opts, ocp_qp_full_condensing_memory *mem, void *work) { @@ -143,7 +208,7 @@ void ocp_qp_full_condensing(ocp_qp_in *in, dense_qp_in *out, ocp_qp_full_condens mem->qp_in = in; // convert to dense qp structure - if (opts->condense_rhs_only == 1) + if (opts->cond_hess == 0) { // condense gradient only d_cond_rhs_qp_ocp2dense(in, out, opts->hpipm_opts, mem->hpipm_workspace); @@ -165,10 +230,12 @@ void ocp_qp_full_condensing(ocp_qp_in *in, dense_qp_in *out, ocp_qp_full_condens } } + + void ocp_qp_full_expansion(dense_qp_out *in, ocp_qp_out *out, ocp_qp_full_condensing_opts *opts, ocp_qp_full_condensing_memory *mem, void *work) { - if (opts->expand_primal_sol_only == 1) + if (opts->expand_dual_sol == 0) { d_expand_primal_sol_dense2ocp(mem->qp_in, in, out, opts->hpipm_opts, mem->hpipm_workspace); } @@ -178,6 +245,8 @@ void ocp_qp_full_expansion(dense_qp_out *in, ocp_qp_out *out, ocp_qp_full_conden } } + + void ocp_qp_full_condensing_config_initialize_default(void *config_) { ocp_qp_condensing_config *config = config_; @@ -186,6 +255,7 @@ void ocp_qp_full_condensing_config_initialize_default(void *config_) config->opts_assign = &ocp_qp_full_condensing_opts_assign; config->opts_initialize_default = &ocp_qp_full_condensing_opts_initialize_default; config->opts_update = &ocp_qp_full_condensing_opts_update; + config->opts_set = &ocp_qp_full_condensing_opts_set; config->memory_calculate_size = &ocp_qp_full_condensing_memory_calculate_size; config->memory_assign = &ocp_qp_full_condensing_memory_assign; config->workspace_calculate_size = &ocp_qp_full_condensing_workspace_calculate_size; diff --git a/acados/ocp_qp/ocp_qp_full_condensing.h b/acados/ocp_qp/ocp_qp_full_condensing.h index e9384a6895..3ebce0bd54 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing.h +++ b/acados/ocp_qp/ocp_qp_full_condensing.h @@ -29,13 +29,18 @@ extern "C" { #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/utils/types.h" + + typedef struct ocp_qp_full_condensing_opts_ { struct d_cond_qp_ocp2dense_arg *hpipm_opts; - int condense_rhs_only; - int expand_primal_sol_only; + int cond_hess; // 0 cond only rhs, 1 cond hess + rhs + int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol + int ric_alg; } ocp_qp_full_condensing_opts; + + typedef struct ocp_qp_full_condensing_memory_ { struct d_cond_qp_ocp2dense_workspace *hpipm_workspace; @@ -43,6 +48,8 @@ typedef struct ocp_qp_full_condensing_memory_ ocp_qp_in *qp_in; } ocp_qp_full_condensing_memory; + + // void compute_dense_qp_dims(ocp_qp_dims *dims, dense_qp_dims *ddims); // @@ -54,6 +61,8 @@ void ocp_qp_full_condensing_opts_initialize_default(ocp_qp_dims *dims, void *opt // void ocp_qp_full_condensing_opts_update(ocp_qp_dims *dims, void *opts_); // +void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value); +// int ocp_qp_full_condensing_memory_calculate_size(ocp_qp_dims *dims, void *opts_); // void *ocp_qp_full_condensing_memory_assign(ocp_qp_dims *dims, void *opts_, void *raw_memory); diff --git a/acados/ocp_qp/ocp_qp_full_condensing_solver.c b/acados/ocp_qp/ocp_qp_full_condensing_solver.c index c57640dddb..99022721dd 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_full_condensing_solver.c @@ -32,6 +32,8 @@ #include "acados/utils/timing.h" #include "acados/utils/types.h" + + /************************************************ * opts ************************************************/ @@ -55,6 +57,8 @@ int ocp_qp_full_condensing_solver_opts_calculate_size(void *config_, ocp_qp_dims return size; } + + void *ocp_qp_full_condensing_solver_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory) { ocp_qp_xcond_solver_config *config = config_; @@ -84,6 +88,8 @@ void *ocp_qp_full_condensing_solver_opts_assign(void *config_, ocp_qp_dims *dims return (void *) opts; } + + void ocp_qp_full_condensing_solver_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_) { @@ -99,6 +105,8 @@ void ocp_qp_full_condensing_solver_opts_initialize_default(void *config_, ocp_qp opts->qp_solver_opts); // TODO(all): pass dense_qp_dims ??? } + + void ocp_qp_full_condensing_solver_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) { ocp_qp_xcond_solver_config *config = config_; @@ -112,27 +120,44 @@ void ocp_qp_full_condensing_solver_opts_update(void *config_, ocp_qp_dims *dims, qp_solver->opts_update(qp_solver, NULL, opts->qp_solver_opts); // TODO(all): pass dense_qp_dims } + + void ocp_qp_full_condensing_solver_opts_set(void *config_, void *opts_, const char *field, void* value) { ocp_qp_full_condensing_solver_opts *opts = (ocp_qp_full_condensing_solver_opts *) opts_; - // ocp_qp_xcond_solver_config *config = config_; - - if (!strcmp(field, "condense_rhs_only")) - { - int* condense_rhs_only = (int *) value; - opts->cond_opts->condense_rhs_only = *condense_rhs_only; - } - else if (!strcmp(field, "expand_primal_sol_only")) - { - int* expand_primal_sol_only = (int *) value; - opts->cond_opts->expand_primal_sol_only = *expand_primal_sol_only; - } - else - { - printf("\nerror: option type %s not available in ocp_qp_full_condensing_solver module\n", - field); - exit(1); - } + ocp_qp_xcond_solver_config *config = config_; + + int ii; + + char module[MAX_STR_LEN]; + char *ptr_module = NULL; + int module_length = 0; + + // extract module name + char *char_ = strchr(field, '_'); + if(char_!=NULL) + { + module_length = char_-field; + for(ii=0; iicond_opts, field+module_length+1, value); + } + else // pass options to QP module + { +//printf("\ncalling qp solver opts set: %s\n", field); + config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); + } + + return; + } @@ -164,6 +189,8 @@ int ocp_qp_full_condensing_solver_memory_calculate_size(void *config_, ocp_qp_di return size; } + + void *ocp_qp_full_condensing_solver_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory) { @@ -208,6 +235,8 @@ void *ocp_qp_full_condensing_solver_memory_assign(void *config_, ocp_qp_dims *di return mem; } + + /************************************************ * workspace ************************************************/ @@ -230,6 +259,8 @@ int ocp_qp_full_condensing_solver_workspace_calculate_size(void *config_, ocp_qp return size; } + + static void cast_workspace(void *config_, ocp_qp_dims *dims, ocp_qp_full_condensing_solver_opts *opts, ocp_qp_full_condensing_solver_memory *mem, @@ -255,6 +286,8 @@ static void cast_workspace(void *config_, ocp_qp_dims *dims, c_ptr); } + + /************************************************ * functions ************************************************/ @@ -304,6 +337,8 @@ int ocp_qp_full_condensing_solver(void *config_, ocp_qp_in *qp_in, ocp_qp_out *q return solver_status; } + + void ocp_qp_full_condensing_solver_config_initialize_default(void *config_) { ocp_qp_xcond_solver_config *config = config_; diff --git a/acados/ocp_qp/ocp_qp_full_condensing_solver.h b/acados/ocp_qp/ocp_qp_full_condensing_solver.h index 3c73b2d02c..27e98e7cd9 100644 --- a/acados/ocp_qp/ocp_qp_full_condensing_solver.h +++ b/acados/ocp_qp/ocp_qp_full_condensing_solver.h @@ -30,12 +30,16 @@ extern "C" { #include "acados/ocp_qp/ocp_qp_full_condensing.h" #include "acados/utils/types.h" + + typedef struct ocp_qp_full_condensing_solver_opts_ { ocp_qp_full_condensing_opts *cond_opts; void *qp_solver_opts; } ocp_qp_full_condensing_solver_opts; + + typedef struct ocp_qp_full_condensing_solver_memory_ { ocp_qp_full_condensing_memory *cond_memory; @@ -44,6 +48,8 @@ typedef struct ocp_qp_full_condensing_solver_memory_ dense_qp_out *qpd_out; } ocp_qp_full_condensing_solver_memory; + + typedef struct ocp_qp_full_condensing_solver_workspace_ { void *cond_work; @@ -53,6 +59,8 @@ typedef struct ocp_qp_full_condensing_solver_workspace_ // dense_qp_out *qpd_out; } ocp_qp_full_condensing_solver_workspace; + + // int ocp_qp_full_condensing_solver_opts_calculate_size(void *config, ocp_qp_dims *dims); // diff --git a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c index afddc3ae28..0f45bb4458 100644 --- a/acados/ocp_qp/ocp_qp_partial_condensing_solver.c +++ b/acados/ocp_qp/ocp_qp_partial_condensing_solver.c @@ -122,8 +122,6 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const ocp_qp_partial_condensing_solver_opts *opts = (ocp_qp_partial_condensing_solver_opts *) opts_; ocp_qp_xcond_solver_config *config = config_; - // TODO extract module name 'pcond' for partial condensing optsion - int ii; char module[MAX_STR_LEN]; @@ -150,7 +148,7 @@ void ocp_qp_partial_condensing_solver_opts_set(void *config_, void *opts_, const { config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, field, value); } - + return; } diff --git a/examples/c/mass_spring_offline_fcond_qpoases_split.c b/examples/c/mass_spring_offline_fcond_qpoases_split.c index b99707964f..d217bad8ce 100644 --- a/examples/c/mass_spring_offline_fcond_qpoases_split.c +++ b/examples/c/mass_spring_offline_fcond_qpoases_split.c @@ -150,11 +150,8 @@ int main() { if(OFFLINE_CONDENSING == 1) { - cond_opts->condense_rhs_only = 1; - cond_opts->expand_primal_sol_only = 1; - - cond_opts->condense_rhs_only = 1; - cond_opts->expand_primal_sol_only = 1; + cond_opts->cond_hess = 0; + cond_opts->expand_dual_sol = 0; ocp_qp_full_condensing_opts_update(qp_in->dim, cond_opts); ocp_qp_full_condensing_opts_update(qp_in->dim, cond_opts); From b223c41a6d8e793c192e08a33b3db198d5f99fe2 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 13:10:09 +0200 Subject: [PATCH 39/78] add dense hpipm opts set --- acados/dense_qp/dense_qp_hpipm.c | 65 +++++++++++++++++++ acados/dense_qp/dense_qp_hpipm.h | 6 ++ acados/ocp_qp/ocp_qp_hpipm.h | 2 + .../pendulum_on_cart_model/example_ocp.m | 4 +- external/hpipm | 2 +- 5 files changed, 76 insertions(+), 3 deletions(-) diff --git a/acados/dense_qp/dense_qp_hpipm.c b/acados/dense_qp/dense_qp_hpipm.c index 03d17ae1e9..716473ec76 100644 --- a/acados/dense_qp/dense_qp_hpipm.c +++ b/acados/dense_qp/dense_qp_hpipm.c @@ -18,7 +18,9 @@ */ // external +#include #include +#include // 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,6 +94,8 @@ 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_; @@ -93,6 +103,58 @@ void dense_qp_hpipm_opts_update(void *config_, void *dims_, void *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_; @@ -199,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; diff --git a/acados/dense_qp/dense_qp_hpipm.h b/acados/dense_qp/dense_qp_hpipm.h index 94b1d545c4..4c5e428d63 100644 --- a/acados/dense_qp/dense_qp_hpipm.h +++ b/acados/dense_qp/dense_qp_hpipm.h @@ -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); // diff --git a/acados/ocp_qp/ocp_qp_hpipm.h b/acados/ocp_qp/ocp_qp_hpipm.h index b6d8b8879c..8fbbdc6d42 100644 --- a/acados/ocp_qp/ocp_qp_hpipm.h +++ b/acados/ocp_qp/ocp_qp_hpipm.h @@ -39,6 +39,8 @@ typedef struct ocp_qp_hpipm_opts_ struct d_ocp_qp_ipm_arg *hpipm_opts; } ocp_qp_hpipm_opts; + + // TODO(roversch): why not make this a typedef of the underlying struct? // struct of the solver memory typedef struct ocp_qp_hpipm_memory_ diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 613945eb18..f6010d482b 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -188,10 +188,10 @@ ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) ocp_opts.set('qp_solver_cond_N', qp_solver_cond_N); - ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); ocp_opts.set('qp_solver_ric_alg', qp_solver_ric_alg); - ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); end +ocp_opts.set('qp_solver_cond_ric_alg', qp_solver_cond_ric_alg); +ocp_opts.set('qp_solver_warm_start', qp_solver_warm_start); ocp_opts.set('sim_method', sim_method); ocp_opts.set('sim_method_num_stages', sim_method_num_stages); ocp_opts.set('sim_method_num_steps', sim_method_num_steps); diff --git a/external/hpipm b/external/hpipm index 3b37bf269b..7ab221e966 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 3b37bf269b9b22752c29554fa28c4782ba32dbe8 +Subproject commit 7ab221e966db2a676d053228b96ad42ba1b8f529 From 0422878bec38afd3a05cf374c0eb8116abb81096 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 13:57:23 +0200 Subject: [PATCH 40/78] add dummy qpoases opts set --- acados/dense_qp/dense_qp_qpoases.c | 50 ++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) diff --git a/acados/dense_qp/dense_qp_qpoases.c b/acados/dense_qp/dense_qp_qpoases.c index feb70068e6..9d79999eea 100644 --- a/acados/dense_qp/dense_qp_qpoases.c +++ b/acados/dense_qp/dense_qp_qpoases.c @@ -18,7 +18,9 @@ */ // external +#include #include +#include // blasfeo #include "blasfeo/include/blasfeo_d_aux.h" #include "blasfeo/include/blasfeo_d_blas.h" @@ -61,6 +63,8 @@ #include "acados_c/dense_qp_interface.h" + + /************************************************ * opts ************************************************/ @@ -73,6 +77,8 @@ int dense_qp_qpoases_opts_calculate_size(void *config_, dense_qp_dims *dims) return size; } + + void *dense_qp_qpoases_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory) { dense_qp_qpoases_opts *opts; @@ -87,6 +93,8 @@ void *dense_qp_qpoases_opts_assign(void *config_, dense_qp_dims *dims, void *raw return (void *) opts; } + + void dense_qp_qpoases_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_) { dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *) opts_; @@ -102,6 +110,8 @@ void dense_qp_qpoases_opts_initialize_default(void *config_, dense_qp_dims *dims return; } + + void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts_) { // dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *)opts_; @@ -109,6 +119,43 @@ void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts return; } + + +void dense_qp_qpoases_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + dense_qp_qpoases_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: dense_qp_qpoases_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -168,6 +215,8 @@ int dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, v return size; } + + void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, void *raw_memory) { @@ -628,6 +677,7 @@ void dense_qp_qpoases_config_initialize_default(void *config_) 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->opts_set = &dense_qp_qpoases_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & dense_qp_qpoases_memory_calculate_size; config->memory_assign = From a4bbf38b5e2c5622c4bf956d37199a1a1763249e Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 15:56:40 +0200 Subject: [PATCH 41/78] add dummy dense ooqp and qore opts set --- acados/dense_qp/dense_qp_ooqp.c | 78 +++++++++++++++++++++++++++++++++ acados/dense_qp/dense_qp_qore.c | 54 +++++++++++++++++++++++ 2 files changed, 132 insertions(+) diff --git a/acados/dense_qp/dense_qp_ooqp.c b/acados/dense_qp/dense_qp_ooqp.c index 5b33d27be7..40067f5c66 100644 --- a/acados/dense_qp/dense_qp_ooqp.c +++ b/acados/dense_qp/dense_qp_ooqp.c @@ -35,6 +35,8 @@ #include "acados/utils/print.h" #include "acados/utils/mem.h" + + static void update_gradient(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -42,6 +44,8 @@ static void update_gradient(const dense_qp_in *in, dense_qp_ooqp_memory *mem) blasfeo_unpack_dvec(dims->nv, in->gz, 0, &mem->c[0]); } + + static void update_hessian_data(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -53,6 +57,8 @@ static void update_hessian_data(const dense_qp_in *in, dense_qp_ooqp_memory *mem blasfeo_unpack_tran_dmat(dims->nv, dims->nv, in->Hv, 0, 0, mem->dQ, dims->nv); } + + static void update_b_vector(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -60,6 +66,8 @@ static void update_b_vector(const dense_qp_in *in, dense_qp_ooqp_memory *mem) blasfeo_unpack_dvec(dims->ne, in->b, 0, mem->bA); } + + static void update_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -90,6 +98,8 @@ static void update_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem) } } + + static void update_ineq_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -103,6 +113,8 @@ static void update_ineq_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem) } } + + static void update_inequalities_data(const dense_qp_in *in, dense_qp_ooqp_memory *mem) { dense_qp_dims *dims = in->dim; @@ -110,6 +122,8 @@ static void update_inequalities_data(const dense_qp_in *in, dense_qp_ooqp_memory blasfeo_unpack_dmat(dims->nv, dims->ng, in->Ct, 0, 0, mem->dC, dims->nv); } + + static void dense_qp_ooqp_update_memory(const dense_qp_in *in, const dense_qp_ooqp_opts *opts, dense_qp_ooqp_memory *mem) { @@ -138,6 +152,8 @@ static void dense_qp_ooqp_update_memory(const dense_qp_in *in, const dense_qp_oo mem->firstRun = 0; } + + static void print_inputs(dense_qp_ooqp_memory *mem) { printf("\n----------> OOQP INPUTS <----------\n\n"); @@ -188,6 +204,8 @@ static void print_inputs(dense_qp_ooqp_memory *mem) } } + + static void print_outputs(dense_qp_ooqp_memory *mem, dense_qp_ooqp_workspace *work, int return_value) { @@ -232,6 +250,8 @@ static void print_outputs(dense_qp_ooqp_memory *mem, dense_qp_ooqp_workspace *wo } } + + static void fill_in_qp_out(const dense_qp_in *in, dense_qp_out *out, dense_qp_ooqp_workspace *work) { int nb = in->dim->nb; @@ -267,6 +287,8 @@ static void fill_in_qp_out(const dense_qp_in *in, dense_qp_out *out, dense_qp_oo } + + int dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims) { int size = 0; @@ -274,6 +296,8 @@ int dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims) return size; } + + void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory) { dense_qp_ooqp_opts *opts; @@ -288,6 +312,8 @@ void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_me return (void *) opts; } + + void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_) { dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *) opts_; @@ -300,11 +326,50 @@ void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, v return; } + + void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_) { return; } + + +void dense_qp_ooqp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + dense_qp_ooqp_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: dense_qp_ooqp_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + int dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) { int nv = dims->nv; @@ -333,6 +398,8 @@ int dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void return size; } + + void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, void *raw_memory) { // dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *)opts_; @@ -383,6 +450,8 @@ void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts return mem; } + + int dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_) { // dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *)opts_; @@ -400,6 +469,8 @@ int dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, v return size; } + + static void dense_qp_ooqp_cast_workspace(dense_qp_ooqp_workspace *work, dense_qp_ooqp_memory *mem) { char *ptr = (char *)work; @@ -421,6 +492,8 @@ static void dense_qp_ooqp_cast_workspace(dense_qp_ooqp_workspace *work, dense_qp ptr += (mem->mz) * sizeof(double); } + + int_t dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_) { @@ -486,12 +559,16 @@ int_t dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, voi return acados_status; } + + void dense_qp_ooqp_destroy(void *mem_, void *work) { free(work); free(mem_); } + + void dense_qp_ooqp_config_initialize_default(void *config_) { qp_solver_config *config = config_; @@ -501,6 +578,7 @@ void dense_qp_ooqp_config_initialize_default(void *config_) config->opts_initialize_default = (void (*)(void *, void *, void *)) & dense_qp_ooqp_opts_initialize_default; config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_ooqp_opts_update; + config->opts_set = &dense_qp_ooqp_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & dense_qp_ooqp_memory_calculate_size; config->memory_assign = diff --git a/acados/dense_qp/dense_qp_qore.c b/acados/dense_qp/dense_qp_qore.c index f02f787bfa..764da6d717 100644 --- a/acados/dense_qp/dense_qp_qore.c +++ b/acados/dense_qp/dense_qp_qore.c @@ -30,6 +30,8 @@ #include "acados/utils/mem.h" #include "acados/utils/timing.h" + + /************************************************ * opts ************************************************/ @@ -42,6 +44,8 @@ int dense_qp_qore_opts_calculate_size(void *config_, dense_qp_dims *dims) return size; } + + void *dense_qp_qore_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory) { dense_qp_qore_opts *opts; @@ -56,6 +60,8 @@ void *dense_qp_qore_opts_assign(void *config_, dense_qp_dims *dims, void *raw_me return (void *) opts; } + + void dense_qp_qore_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_) { dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_; @@ -71,6 +77,8 @@ void dense_qp_qore_opts_initialize_default(void *config_, dense_qp_dims *dims, v return; } + + void dense_qp_qore_opts_update(void *config_, dense_qp_dims *dims, void *opts_) { // dense_qp_qore_opts *opts = (dense_qp_qore_opts *)opts_; @@ -78,6 +86,43 @@ void dense_qp_qore_opts_update(void *config_, dense_qp_dims *dims, void *opts_) return; } + + +void dense_qp_qore_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + dense_qp_qore_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: dense_qp_qore_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -133,6 +178,8 @@ int dense_qp_qore_memory_calculate_size(void *config_, dense_qp_dims *dims, void return size; } + + void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, void *raw_memory) { dense_qp_qore_memory *mem; @@ -217,6 +264,8 @@ void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts return mem; } + + /************************************************ * workspace ************************************************/ @@ -226,6 +275,8 @@ int dense_qp_qore_workspace_calculate_size(void *config_, dense_qp_dims *dims, v return 0; } + + /************************************************ * functions ************************************************/ @@ -455,6 +506,8 @@ int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void return acados_status; } + + void dense_qp_qore_config_initialize_default(void *config_) { qp_solver_config *config = config_; @@ -464,6 +517,7 @@ void dense_qp_qore_config_initialize_default(void *config_) config->opts_initialize_default = (void (*)(void *, void *, void *)) & dense_qp_qore_opts_initialize_default; config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_qore_opts_update; + config->opts_set = &dense_qp_qore_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & dense_qp_qore_memory_calculate_size; config->memory_assign = From 930619efaae22090a1a42849b00c55be215be4a1 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 16:15:06 +0200 Subject: [PATCH 42/78] add dummy ocp ooqp osqp hpmpc qpdumes opts set --- acados/ocp_qp/ocp_qp_hpmpc.c | 47 ++++++++++++++ acados/ocp_qp/ocp_qp_ooqp.c | 108 +++++++++++++++++++++++++++++++++ acados/ocp_qp/ocp_qp_osqp.c | 37 +++++++++++ acados/ocp_qp/ocp_qp_qpdunes.c | 79 ++++++++++++++++++++++++ 4 files changed, 271 insertions(+) diff --git a/acados/ocp_qp/ocp_qp_hpmpc.c b/acados/ocp_qp/ocp_qp_hpmpc.c index c099b5ba7b..6ff8aa0e92 100644 --- a/acados/ocp_qp/ocp_qp_hpmpc.c +++ b/acados/ocp_qp/ocp_qp_hpmpc.c @@ -22,6 +22,7 @@ #include #include #include +#include #include "blasfeo/include/blasfeo_d_aux.h" #include "blasfeo/include/blasfeo_d_blas.h" @@ -37,6 +38,8 @@ #include "acados/utils/timing.h" #include "acados/utils/types.h" + + /************************************************ * opts ************************************************/ @@ -49,6 +52,8 @@ int ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims) return size; } + + void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory) { ocp_qp_hpmpc_opts *args; @@ -69,6 +74,8 @@ void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memor return (void *) args; } + + void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_) { ocp_qp_hpmpc_opts *args = (ocp_qp_hpmpc_opts *) opts_; @@ -81,6 +88,8 @@ void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void return; } + + void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) { // ocp_qp_hpmpc_opts *args = (ocp_qp_hpmpc_opts *)opts_; @@ -88,6 +97,43 @@ void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) return; } + + +void ocp_qp_hpmpc_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_hpmpc_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: ocp_qp_hpmpc_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -461,6 +507,7 @@ void ocp_qp_hpmpc_config_initialize_default(void *config_) config->opts_initialize_default = (void (*)(void *, void *, void *)) & ocp_qp_hpmpc_opts_initialize_default; config->opts_update = (void (*)(void *, void *, void *)) & ocp_qp_hpmpc_opts_update; + config->opts_set = &ocp_qp_hpmpc_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & ocp_qp_hpmpc_memory_calculate_size; config->memory_assign = diff --git a/acados/ocp_qp/ocp_qp_ooqp.c b/acados/ocp_qp/ocp_qp_ooqp.c index 12a78a9436..f8bb59fea1 100644 --- a/acados/ocp_qp/ocp_qp_ooqp.c +++ b/acados/ocp_qp/ocp_qp_ooqp.c @@ -35,10 +35,14 @@ #define FLIP_BOUNDS + + int *rows; int *cols; int lda; + + static int max_of_three(int a, int b, int c) { int ans = a; @@ -49,6 +53,8 @@ static int max_of_three(int a, int b, int c) return ans; } + + // comparator for qsort static int comparator(const void *p1, const void *p2) { @@ -62,6 +68,8 @@ static int comparator(const void *p1, const void *p2) return ans1 - ans2; } + + static void sort_matrix_structure_row_major(int *order, int *irow, int nnz, int *jcol, int *tmp) { @@ -88,6 +96,8 @@ static void sort_matrix_structure_row_major(int *order, int *irow, int nnz, int } } + + static void sort_matrix_data_row_major(int *order, int nnz, real_t *d, real_t *tmp) { int ii; @@ -103,6 +113,8 @@ static void sort_matrix_data_row_major(int *order, int nnz, real_t *d, real_t *t } } + + static int get_number_of_primal_vars(const ocp_qp_dims *dims) { int nx = 0; @@ -116,6 +128,8 @@ static int get_number_of_primal_vars(const ocp_qp_dims *dims) return nx; } + + static int get_number_of_equalities(const ocp_qp_dims *dims) { int my = 0; @@ -129,6 +143,8 @@ static int get_number_of_equalities(const ocp_qp_dims *dims) return my; } + + static int get_number_of_inequalities(const ocp_qp_dims *dims) { int mz = 0; @@ -142,6 +158,8 @@ static int get_number_of_inequalities(const ocp_qp_dims *dims) return mz; } + + static int get_nnzQ(const ocp_qp_dims *dims) { int kk; @@ -157,6 +175,8 @@ static int get_nnzQ(const ocp_qp_dims *dims) return nnzQ; } + + static int get_nnzA(const ocp_qp_dims *dims) { int kk; @@ -170,6 +190,8 @@ static int get_nnzA(const ocp_qp_dims *dims) return nnzA; } + + static int get_nnzC(const ocp_qp_dims *dims) { int kk; @@ -183,6 +205,8 @@ static int get_nnzC(const ocp_qp_dims *dims) return nnzC; } + + static void update_gradient(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) { int kk, nn; @@ -198,6 +222,8 @@ static void update_gradient(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) } } + + static void update_hessian_structure(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -253,6 +279,8 @@ static void update_hessian_structure(const ocp_qp_in *in, ocp_qp_ooqp_memory *me sort_matrix_structure_row_major(mem->orderQ, mem->irowQ, mem->nnzQ, mem->jcolQ, work->tmpInt); } + + static void update_hessian_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -290,6 +318,8 @@ static void update_hessian_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, sort_matrix_data_row_major(mem->orderQ, mem->nnzQ, mem->dQ, work->tmpReal); } + + static void update_b_vector(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) { int ii, kk; @@ -305,6 +335,8 @@ static void update_b_vector(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) } } + + static void update_dynamics_structure(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -354,6 +386,8 @@ static void update_dynamics_structure(const ocp_qp_in *in, ocp_qp_ooqp_memory *m sort_matrix_structure_row_major(mem->orderA, mem->irowA, mem->nnzA, mem->jcolA, work->tmpInt); } + + static void update_dynamics_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -391,6 +425,8 @@ static void update_dynamics_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, sort_matrix_data_row_major(mem->orderA, mem->nnzA, mem->dA, work->tmpReal); } + + static void update_bounds(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) { int ii, kk; @@ -441,6 +477,8 @@ static void update_bounds(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) } } + + static void update_ineq_bounds(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) { int ii, kk; @@ -460,6 +498,8 @@ static void update_ineq_bounds(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem) } } + + static void update_inequalities_structure(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -502,6 +542,8 @@ static void update_inequalities_structure(const ocp_qp_in *in, ocp_qp_ooqp_memor sort_matrix_structure_row_major(mem->orderC, mem->irowC, mem->nnzC, mem->jcolC, work->tmpInt); } + + static void update_inequalities_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -533,6 +575,8 @@ static void update_inequalities_data(const ocp_qp_in *in, ocp_qp_ooqp_memory *me sort_matrix_data_row_major(mem->orderC, mem->nnzC, mem->dC, work->tmpReal); } + + static void ocp_qp_ooqp_update_memory(const ocp_qp_in *in, const ocp_qp_ooqp_opts *opts, ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work) { @@ -587,6 +631,8 @@ static void ocp_qp_ooqp_update_memory(const ocp_qp_in *in, const ocp_qp_ooqp_opt mem->firstRun = 0; } + + static void print_inputs(ocp_qp_ooqp_memory *mem) { printf("\n----------> OOQP INPUTS <----------\n\n"); @@ -637,6 +683,8 @@ static void print_inputs(ocp_qp_ooqp_memory *mem) } } + + static void print_outputs(ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work, int return_value) { int ii; @@ -680,6 +728,8 @@ static void print_outputs(ocp_qp_ooqp_memory *mem, ocp_qp_ooqp_workspace *work, } } + + static void fill_in_qp_out(const ocp_qp_in *in, ocp_qp_out *out, ocp_qp_ooqp_workspace *work) { int kk, ii, nn, mm; @@ -748,6 +798,8 @@ static void fill_in_qp_out(const ocp_qp_in *in, ocp_qp_out *out, ocp_qp_ooqp_wor } } + + int ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims) { int size = 0; @@ -755,6 +807,8 @@ int ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims) return size; } + + void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory) { ocp_qp_ooqp_opts *opts; @@ -769,6 +823,8 @@ void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory return (void *) opts; } + + void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_) { ocp_qp_ooqp_opts *opts = (ocp_qp_ooqp_opts *) opts_; @@ -784,11 +840,50 @@ void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void return; } + + void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) { return; } + + +void ocp_qp_ooqp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_ooqp_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: ocp_qp_ooqp_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + int ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_) { int nx = get_number_of_primal_vars(dims); @@ -820,6 +915,8 @@ int ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *op return size; } + + void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory) { // ocp_qp_ooqp_opts *opts = (ocp_qp_ooqp_opts *)opts_; @@ -888,6 +985,8 @@ void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, v return mem; } + + int ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_) { UNUSED(opts_); @@ -911,6 +1010,8 @@ int ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void return size; } + + static void ocp_qp_ooqp_cast_workspace(ocp_qp_ooqp_workspace *work, ocp_qp_ooqp_memory *mem) { char *ptr = (char *)work; @@ -936,6 +1037,8 @@ static void ocp_qp_ooqp_cast_workspace(ocp_qp_ooqp_workspace *work, ocp_qp_ooqp_ // ptr += (mem->nnz)*sizeof(double); } + + int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_, void *work_) { @@ -994,12 +1097,16 @@ int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_ return acados_status; } + + void ocp_qp_ooqp_destroy(void *mem_, void *work) { free(work); free(mem_); } + + void ocp_qp_ooqp_config_initialize_default(void *config_) { qp_solver_config *config = config_; @@ -1010,6 +1117,7 @@ void ocp_qp_ooqp_config_initialize_default(void *config_) config->opts_initialize_default = (void (*)(void *, void *, void *)) & ocp_qp_ooqp_opts_initialize_default; config->opts_update = (void (*)(void *, void *, void *)) & ocp_qp_ooqp_opts_update; + config->opts_set = &ocp_qp_ooqp_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & ocp_qp_ooqp_memory_calculate_size; config->memory_assign = diff --git a/acados/ocp_qp/ocp_qp_osqp.c b/acados/ocp_qp/ocp_qp_osqp.c index e6a8f2c847..ad599116ad 100644 --- a/acados/ocp_qp/ocp_qp_osqp.c +++ b/acados/ocp_qp/ocp_qp_osqp.c @@ -38,6 +38,7 @@ #include "acados/utils/types.h" + /************************************************ * helper functions ************************************************/ @@ -640,6 +641,41 @@ void ocp_qp_osqp_opts_update(void *config_, void *dims_, void *opts_) return; } +void ocp_qp_osqp_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_osqp_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: ocp_qp_osqp_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -1260,6 +1296,7 @@ void ocp_qp_osqp_config_initialize_default(void *config_) config->opts_assign = &ocp_qp_osqp_opts_assign; config->opts_initialize_default = &ocp_qp_osqp_opts_initialize_default; config->opts_update = &ocp_qp_osqp_opts_update; + config->opts_set = &ocp_qp_osqp_opts_set; config->memory_calculate_size = &ocp_qp_osqp_memory_calculate_size; config->memory_assign = &ocp_qp_osqp_memory_assign; config->workspace_calculate_size = &ocp_qp_osqp_workspace_calculate_size; diff --git a/acados/ocp_qp/ocp_qp_qpdunes.c b/acados/ocp_qp/ocp_qp_qpdunes.c index 142dd7b820..9c4dff09c6 100644 --- a/acados/ocp_qp/ocp_qp_qpdunes.c +++ b/acados/ocp_qp/ocp_qp_qpdunes.c @@ -25,6 +25,7 @@ #include #include #include +#include // blasfeo #include "blasfeo/include/blasfeo_d_aux.h" @@ -35,6 +36,8 @@ #include "acados/utils/print.h" #include "acados/utils/timing.h" + + static int get_maximum_number_of_inequality_constraints(ocp_qp_dims *dims) { int nDmax = dims->ng[0]; @@ -46,6 +49,8 @@ static int get_maximum_number_of_inequality_constraints(ocp_qp_dims *dims) return nDmax; } + + static qpdunes_stage_qp_solver_t check_stage_qp_solver(ocp_qp_qpdunes_opts *opts, ocp_qp_in *qp_in) { int N = qp_in->dim->N; @@ -108,6 +113,8 @@ static qpdunes_stage_qp_solver_t check_stage_qp_solver(ocp_qp_qpdunes_opts *opts return stageQpSolver; } + + /************************************************ * opts ************************************************/ @@ -119,6 +126,8 @@ int ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims) return size; } + + void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory) { ocp_qp_qpdunes_opts *opts; @@ -133,6 +142,8 @@ void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_mem return (void *) opts; } + + void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_) { ocp_qp_qpdunes_opts *opts = (ocp_qp_qpdunes_opts *) opts_; @@ -185,6 +196,8 @@ void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, vo return; } + + void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) { // ocp_qp_qpdunes_opts *opts = (ocp_qp_qpdunes_opts *)opts_; @@ -192,6 +205,43 @@ void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_) return; } + + +void ocp_qp_qpdunes_opts_set(void *config_, void *opts_, const char *field, void *value) +{ + ocp_qp_qpdunes_opts *opts = opts_; + + if (!strcmp(field, "tol_stat")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_eq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_ineq")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "tol_comp")) + { + // TODO set solver exit tolerance + } + else if (!strcmp(field, "warm_start")) + { + // TODO set solver warm start + } + else + { + printf("\nerror: ocp_qp_qpdunes_opts_set: wrong field: %s\n", field); + exit(1); + } + + return; +} + + + /************************************************ * memory ************************************************/ @@ -204,6 +254,8 @@ int ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void return size; } + + void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory) { ocp_qp_qpdunes_opts *opts = (ocp_qp_qpdunes_opts *) opts_; @@ -262,6 +314,8 @@ void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_ return mem; } + + static void form_H(double *H, int nx, int nu, struct blasfeo_dmat *sRSQrq) { // make Q full @@ -285,6 +339,8 @@ static void form_H(double *H, int nx, int nu, struct blasfeo_dmat *sRSQrq) // printf("********************************************\n\n"); } + + static void form_RSQ(double *R, double *S, double *Q, int nx, int nu, struct blasfeo_dmat *sRSQrq) { // make Q full @@ -312,6 +368,8 @@ static void form_RSQ(double *R, double *S, double *Q, int nx, int nu, struct bla // printf("********************************************\n\n"); } + + static void form_g(double *g, int nx, int nu, struct blasfeo_dvec *srq) { blasfeo_unpack_dvec(nx, srq, nu, &g[0]); @@ -324,6 +382,8 @@ static void form_g(double *g, int nx, int nu, struct blasfeo_dvec *srq) // printf("********************************************\n\n"); } + + static void form_dynamics(double *ABt, double *b, int nx, int nu, struct blasfeo_dmat *sBAbt, struct blasfeo_dvec *sb) { @@ -343,6 +403,8 @@ static void form_dynamics(double *ABt, double *b, int nx, int nu, struct blasfeo // printf("********************************************\n\n"); } + + static void form_bounds(double *zLow, double *zUpp, int nx, int nu, int nb, int ng, int *idxb, struct blasfeo_dvec *sd, double infty) { @@ -366,6 +428,8 @@ static void form_bounds(double *zLow, double *zUpp, int nx, int nu, int nb, int } } + + static void form_inequalities(double *Ct, double *lc, double *uc, int nx, int nu, int nb, int ng, struct blasfeo_dmat *sDCt, struct blasfeo_dvec *sd) { @@ -383,6 +447,8 @@ static void form_inequalities(double *Ct, double *lc, double *uc, int nx, int nu for (ii = 0; ii < ng; ii++) uc[ii] = -uc[ii]; } + + /************************************************ * workspcae ************************************************/ @@ -410,6 +476,8 @@ int ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, vo return size; } + + static void cast_workspace(ocp_qp_qpdunes_workspace *work, ocp_qp_qpdunes_memory *mem) { char *c_ptr = (char *) work; @@ -434,6 +502,8 @@ static void cast_workspace(ocp_qp_qpdunes_workspace *work, ocp_qp_qpdunes_memory assign_and_advance_double(nz, &work->zUpp, &c_ptr); } + + static int update_memory(ocp_qp_in *in, ocp_qp_qpdunes_opts *opts, ocp_qp_qpdunes_memory *mem, ocp_qp_qpdunes_workspace *work) { @@ -620,6 +690,8 @@ static int update_memory(ocp_qp_in *in, ocp_qp_qpdunes_opts *opts, ocp_qp_qpdune return (int) value; } + + static void fill_in_qp_out(ocp_qp_in *in, ocp_qp_out *out, ocp_qp_qpdunes_memory *mem) { int N = in->dim->N; @@ -671,6 +743,8 @@ static void fill_in_qp_out(ocp_qp_in *in, ocp_qp_out *out, ocp_qp_qpdunes_memory } } + + // TODO(dimitris): free also qp_in, qp_out, etc and write for all solvers? void ocp_qp_qpdunes_free_memory(void *mem_) { @@ -678,6 +752,8 @@ void ocp_qp_qpdunes_free_memory(void *mem_) qpDUNES_cleanup(&(mem->qpData)); } + + /************************************************ * functions ************************************************/ @@ -731,6 +807,8 @@ int ocp_qp_qpdunes(void *config_, ocp_qp_in *in, ocp_qp_out *out, void *opts_, v return acados_status; } + + void ocp_qp_qpdunes_config_initialize_default(void *config_) { qp_solver_config *config = config_; @@ -741,6 +819,7 @@ void ocp_qp_qpdunes_config_initialize_default(void *config_) config->opts_initialize_default = (void (*)(void *, void *, void *)) & ocp_qp_qpdunes_opts_initialize_default; config->opts_update = (void (*)(void *, void *, void *)) & ocp_qp_qpdunes_opts_update; + config->opts_set = &ocp_qp_qpdunes_opts_set; config->memory_calculate_size = (int (*)(void *, void *, void *)) & ocp_qp_qpdunes_memory_calculate_size; config->memory_assign = From 91e73eec18b82bafe9e5409819e1b2bbc20c4871 Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 2 Jun 2019 18:08:10 +0200 Subject: [PATCH 43/78] fix to unit tests --- test/ocp_qp/test_qpsolvers.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/test/ocp_qp/test_qpsolvers.cpp b/test/ocp_qp/test_qpsolvers.cpp index 738f934639..8a8fc4126d 100644 --- a/test/ocp_qp/test_qpsolvers.cpp +++ b/test/ocp_qp/test_qpsolvers.cpp @@ -81,19 +81,19 @@ void set_N2(std::string const &inString, void *opts, int N2, int N) if (inString == "SPARSE_HPIPM") { - option_found = set_option_int(opts, "sparse_hpipm.pcond_N2", N2); + option_found = set_option_int(opts, "sparse_hpipm.cond_N", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_HPMPC") { - option_found = set_option_int(opts, "hpmpc.pcond_N2", N2); + option_found = set_option_int(opts, "hpmpc.cond_N", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_QPDUNES") { - option_found = set_option_int(opts, "qpdunes.pcond_N2", N2); + option_found = set_option_int(opts, "qpdunes.cond_N", N2); REQUIRE(option_found == true); if (N2 == N) { @@ -109,13 +109,13 @@ void set_N2(std::string const &inString, void *opts, int N2, int N) if (inString == "SPARSE_OOQP") { - option_found = set_option_int(opts, "sparse_ooqp.pcond_N2", N2); + option_found = set_option_int(opts, "sparse_ooqp.cond_N", N2); REQUIRE(option_found == true); } if (inString == "SPARSE_OSQP") { - option_found = set_option_int(opts, "sparse_osqp.pcond_N2", N2); + option_found = set_option_int(opts, "sparse_osqp.cond_N", N2); REQUIRE(option_found == true); } } From 552dc1b951bcf81035da8adf8288bb51b3a1dbbb Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 10:32:38 +0200 Subject: [PATCH 44/78] add min_pivot to permute_reduc_hess --- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 80 ++++++++----------- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.h | 3 +- 2 files changed, 37 insertions(+), 46 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index e7c7739e4c..b6fcc12320 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -55,7 +55,8 @@ void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_n { ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; - opts->epsilon = 1e-4; + opts->min_eig = 1e-4; + opts->min_pivot = 1e-12; opts->pivoting = 1; return; @@ -68,10 +69,15 @@ void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *di ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; - if (!strcmp(field, "epsilon")) + if (!strcmp(field, "min_eig")) { double *d_ptr = value; - opts->epsilon = *d_ptr; + opts->min_eig = *d_ptr; + } + else if (!strcmp(field, "min_pivot")) + { + double *d_ptr = value; + opts->min_pivot = *d_ptr; } else if (!strcmp(field, "pivoting")) { @@ -406,7 +412,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg blasfeo_dtrtr_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, mem->RSQrq[ss], 0, 0); // necessary ??? // TODO copy middle stage for nu[N}>0 !!!!!!!!!!!!!!!!!! // blasfeo_unpack_dmat(nu[ss], nu[ss], mem->RSQrq[ss], 0, 0, mem->reg_hess, nu[ss]); -// acados_project(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon); +// acados_project(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e, opts->min_eig); // blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], mem->RSQrq[ss], 0, 0); blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], mem->RSQrq[ss], 0, 0, L, 0, 0); //printf("\nii = %d\n", ss); @@ -434,9 +440,9 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg do_reg = 0; for(jj=0; jjd[jj]epsilon) + if(mem->d[jj]min_eig) { - mem->e[jj] = opts->epsilon - mem->d[jj]; + mem->e[jj] = opts->min_eig - mem->d[jj]; do_reg = 1; } else @@ -470,9 +476,9 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg // if(1) if(do_reg) { - if(opts->pivoting) + for(jj=0; jjpivoting) { // find pivot element pivot = BLASFEO_DMATEL(L3, jj, jj); @@ -504,44 +510,28 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg BLASFEO_DMATEL(L3, kk, idx) = tmp_el; } } - - pivot = BLASFEO_DMATEL(L3, jj, jj); - // TODO check for singularity of pivot !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - if(pivot==0.0) - { - printf("\nocp_nlp_project_reduc_hess: zero pivot element!\n"); - exit(1); - } - pivot = 1.0/pivot; - for(kk=jj+1; kkmin_pivot & pivot>-opts->min_pivot) { - pivot = BLASFEO_DMATEL(L3, jj, jj); - // TODO check for singularity of pivot !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! - if(pivot==0.0) - { - printf("\nocp_nlp_project_reduc_hess: zero pivot element!\n"); - exit(1); - } - pivot = 1.0/pivot; - for(kk=jj+1; kkmin_pivot; + else + pivot = - opts->min_pivot; + } + pivot = 1.0/pivot; + for(kk=jj+1; kkreg_hess, mem->V, mem->d, mem->e); for(jj=0; jjd[jj]epsilon) - mem->e[jj] = opts->epsilon - mem->d[jj]; + if(mem->d[jj]min_eig) + mem->e[jj] = opts->min_eig - mem->d[jj]; else mem->e[jj] = 0.0; } diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h index 99ad630992..54570d14f0 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -51,7 +51,8 @@ extern "C" { typedef struct { - double epsilon; + double min_eig; + double min_pivot; int pivoting; } ocp_nlp_reg_project_reduc_hess_opts; From 1a065a47fc3b1dd46cad2b0004153d9b91c4262a Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 12:45:28 +0200 Subject: [PATCH 45/78] project_reduc_hess: regularize also last stage --- .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 70 ++++--------------- 1 file changed, 15 insertions(+), 55 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index b6fcc12320..788b6756b8 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -387,7 +387,6 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg int ii, jj, kk, ll, ss, idx; -//printf("\nhola\n"); int *nx = dims->nx; int *nu = dims->nu; int N = dims->N; @@ -402,32 +401,23 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg int do_reg = 0; double pivot, tmp_el; -// for(ii=0; ii<=N; ii++) -// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); -// exit(1); -//printf("\nin project\n"); - // last stage - ss = N; - blasfeo_dtrtr_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, mem->RSQrq[ss], 0, 0); // necessary ??? - // TODO copy middle stage for nu[N}>0 !!!!!!!!!!!!!!!!!! -// blasfeo_unpack_dmat(nu[ss], nu[ss], mem->RSQrq[ss], 0, 0, mem->reg_hess, nu[ss]); -// acados_project(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e, opts->min_eig); -// blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], mem->RSQrq[ss], 0, 0); - blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], mem->RSQrq[ss], 0, 0, L, 0, 0); -//printf("\nii = %d\n", ss); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); - blasfeo_dgecp(nx[ss], nu[ss], L, nu[ss], 0, Ls, 0, 0); - blasfeo_dsyrk_ln_mn(nx[ss], nx[ss], nu[ss], -1.0, Ls, 0, 0, Ls, 0, 0, 1.0, mem->RSQrq[ss], nu[ss], nu[ss], P, 0, 0); - blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0); - - // middle stages - for(ii=0; iiBAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm - blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0); + // last stage + if(ss==N) + { + blasfeo_dtrcp_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, L, 0, 0); + } + // middle stages + else + { + blasfeo_dgemm_nt(nu[ss]+nx[ss], nx[ss+1], nx[ss+1], 1.0, mem->BAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm + blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0); + } blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ??? // backup L in L3 @@ -467,13 +457,8 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], L2, 0, 0, L2, 0, 0); blasfeo_dgecp(nx[ss], nu[ss], L2, nu[ss], 0, Ls, 0, 0); blasfeo_dsyrk_ln_mn(nx[ss], nx[ss], nu[ss], -1.0, Ls, 0, 0, Ls, 0, 0, 0.0, L2, nu[ss], nu[ss], L2, nu[ss], nu[ss]); -//printf("\nL2\n"); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L2, 0, 0); -//printf("\nL3\n"); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L3, 0, 0); // compute true_schur -// if(1) if(do_reg) { for(jj=0; jjmin_pivot & pivot>-opts->min_pivot) { if(pivot<0.0) @@ -535,10 +515,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg } } } -//printf("\nL3\n"); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L3, 0, 0); } -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L3, 0, 0); // apply shur to P blasfeo_dgecp(nx[ss], nx[ss], L, nu[ss], nu[ss], P, 0, 0); @@ -556,19 +533,15 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg blasfeo_dgead(nx[ss], nx[ss], 1.0, L2, nu[ss], nu[ss], P, 0, 0); } blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0); -//printf("\nP\n"); -//blasfeo_print_dmat(nx[ss], nx[ss], P, 0, 0); } + // first stage: factorize P in L too ss = 0; -//printf("\nss %d\n", ss); blasfeo_dgemm_nt(nu[ss]+nx[ss], nx[ss+1], nx[ss+1], 1.0, mem->BAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0); blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ??? - // TODO regularize RSQ (also SQ !!!) -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); blasfeo_unpack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0, mem->reg_hess, nu[ss]+nx[ss]); acados_eigen_decomposition(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->d, mem->e); for(jj=0; jje[jj] = 0.0; } -//d_print_mat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->V, nu[ss]+nx[ss]); -//d_print_mat(1, nu[ss]+nx[ss], mem->d, 1); -//d_print_mat(1, nu[ss]+nx[ss], mem->e, 1); acados_reconstruct_A(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->e); -//d_print_mat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->reg_hess, nu[ss]+nx[ss]); blasfeo_pack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->reg_hess, nu[ss]+nx[ss], L2, 0, 0); blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, mem->RSQrq[ss], 0, 0); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0); - // TODO till here -//blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, L, 0, 0); -//blasfeo_dpotrf_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); -//printf("\nL0\n"); -//blasfeo_print_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0); - -// blasfeo_print_dmat(nx[ii], nx[ii], P, 0, 0); -// exit(1); // printf("\nhessian after\n"); // for(ii=0; ii<=N; ii++) From 830b1cbe31a7b0f699c6c32be05572fcaf474a4d Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 13:02:45 +0200 Subject: [PATCH 46/78] sqp: renamed min_res_{g,b,d,m} => tol_{stat,eq,ineq,comp} --- acados/ocp_nlp/ocp_nlp_sqp.c | 48 +++++++++---------- acados/ocp_nlp/ocp_nlp_sqp.h | 8 ++-- examples/c/mass_spring_nmpc_example.c | 16 +++---- .../nonlinear_chain_ocp_nlp_no_interface.c | 8 ++-- examples/c/nonlinear_chain_ocp_nlp.c | 22 ++++----- examples/c/pendulum_scqp.cpp | 20 ++++---- examples/c/simple_dae_example.c | 8 ++-- examples/c/wind_turbine_nmpc.c | 22 ++++----- .../c_templates/acados_solver.in.c | 16 +++---- .../c_templates_inja/acados_solver.in.c | 16 +++---- .../c_templates_tera/acados_solver.in.c | 16 +++---- test/ocp_nlp/test_chain.cpp | 16 +++---- test/ocp_nlp/test_wind_turbine.cpp | 16 +++---- 13 files changed, 116 insertions(+), 116 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 34c4e90b26..fde4e61853 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -174,10 +174,10 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ // SQP opts opts->max_iter = 20; - opts->min_res_g = 1e-8; - opts->min_res_b = 1e-8; - opts->min_res_d = 1e-8; - opts->min_res_m = 1e-8; + opts->tol_stat = 1e-8; + opts->tol_eq = 1e-8; + opts->tol_ineq = 1e-8; + opts->tol_comp = 1e-8; opts->reuse_workspace = 1; #if defined(ACADOS_WITH_OPENMP) @@ -191,10 +191,10 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ // qp solver qp_solver->opts_initialize_default(qp_solver, dims->qp_solver, opts->qp_solver_opts); // overwrite default - qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_stat", &opts->min_res_g); - qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_eq", &opts->min_res_b); - qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_ineq", &opts->min_res_d); - qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_comp", &opts->min_res_m); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_stat", &opts->tol_stat); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_eq", &opts->tol_eq); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_ineq", &opts->tol_ineq); + qp_solver->opts_set(qp_solver, opts->qp_solver_opts, "tol_comp", &opts->tol_comp); // regularization regularize->opts_initialize_default(regularize, dims->regularize, opts->regularize); @@ -313,31 +313,31 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v int* num_threads = (int *) value; opts->num_threads = *num_threads; } - else if (!strcmp(field, "min_res_g")) // TODO rename !!! + else if (!strcmp(field, "tol_stat")) // TODO rename !!! { - double* min_res_g = (double *) value; - opts->min_res_g = *min_res_g; + double* tol_stat = (double *) value; + opts->tol_stat = *tol_stat; // pass to QP too config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_stat", value); } - else if (!strcmp(field, "min_res_b")) // TODO rename !!! + else if (!strcmp(field, "tol_eq")) // TODO rename !!! { - double* min_res_b = (double *) value; - opts->min_res_b = *min_res_b; + double* tol_eq = (double *) value; + opts->tol_eq = *tol_eq; // pass to QP too config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_eq", value); } - else if (!strcmp(field, "min_res_d")) // TODO rename !!! + else if (!strcmp(field, "tol_ineq")) // TODO rename !!! { - double* min_res_d = (double *) value; - opts->min_res_d = *min_res_d; + double* tol_ineq = (double *) value; + opts->tol_ineq = *tol_ineq; // pass to QP too config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_ineq", value); } - else if (!strcmp(field, "min_res_m")) // TODO rename !!! + else if (!strcmp(field, "tol_comp")) // TODO rename !!! { - double* min_res_m = (double *) value; - opts->min_res_m = *min_res_m; + double* tol_comp = (double *) value; + opts->tol_comp = *tol_comp; // pass to QP too config->qp_solver->opts_set(config->qp_solver, opts->qp_solver_opts, "tol_comp", value); } @@ -1256,10 +1256,10 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } // exit conditions on residuals - if ((mem->nlp_res->inf_norm_res_g < opts->min_res_g) & - (mem->nlp_res->inf_norm_res_b < opts->min_res_b) & - (mem->nlp_res->inf_norm_res_d < opts->min_res_d) & - (mem->nlp_res->inf_norm_res_m < opts->min_res_m)) + if ((mem->nlp_res->inf_norm_res_g < opts->tol_stat) & + (mem->nlp_res->inf_norm_res_b < opts->tol_eq) & + (mem->nlp_res->inf_norm_res_d < opts->tol_ineq) & + (mem->nlp_res->inf_norm_res_m < opts->tol_comp)) { // printf("%d sqp iterations\n", sqp_iter); // print_ocp_qp_in(work->qp_in); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index b5e0b203ef..3e47211465 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -50,10 +50,10 @@ typedef struct void **dynamics; // dynamics_opts void **cost; // cost_opts void **constraints; // constraints_opts - double min_res_g; - double min_res_b; - double min_res_d; - double min_res_m; + double tol_stat; // exit tolerance on stationarity condition + double tol_eq; // exit tolerance on equality constraints + double tol_ineq; // exit tolerance on inequality constraints + double tol_comp; // exit tolerance on complemetarity condition int max_iter; int reuse_workspace; int num_threads; diff --git a/examples/c/mass_spring_nmpc_example.c b/examples/c/mass_spring_nmpc_example.c index 10f75ce511..b93c1a4940 100644 --- a/examples/c/mass_spring_nmpc_example.c +++ b/examples/c/mass_spring_nmpc_example.c @@ -730,16 +730,16 @@ int main() { int max_iter = MAX_SQP_ITERS; - double min_res_g = 1e-6; - double min_res_b = 1e-9; - double min_res_d = 1e-9; - double min_res_m = 1e-9; + double tol_stat = 1e-6; + double tol_eq = 1e-9; + double tol_ineq = 1e-9; + double tol_comp = 1e-9; ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); // update after user-defined options ocp_nlp_sqp_opts_update(config, dims, nlp_opts); diff --git a/examples/c/no_interface_examples/nonlinear_chain_ocp_nlp_no_interface.c b/examples/c/no_interface_examples/nonlinear_chain_ocp_nlp_no_interface.c index 89ccc381ba..3b28e9665c 100644 --- a/examples/c/no_interface_examples/nonlinear_chain_ocp_nlp_no_interface.c +++ b/examples/c/no_interface_examples/nonlinear_chain_ocp_nlp_no_interface.c @@ -1886,10 +1886,10 @@ int main() { nlp_opts->max_iter = MAX_SQP_ITERS; - nlp_opts->min_res_g = 1e-9; - nlp_opts->min_res_b = 1e-9; - nlp_opts->min_res_d = 1e-9; - nlp_opts->min_res_m = 1e-9; + nlp_opts->tol_stat = 1e-9; + nlp_opts->tol_eq = 1e-9; + nlp_opts->tol_ineq = 1e-9; + nlp_opts->tol_comp = 1e-9; // update after user-defined options ocp_nlp_sqp_opts_update(config, dims, nlp_opts); diff --git a/examples/c/nonlinear_chain_ocp_nlp.c b/examples/c/nonlinear_chain_ocp_nlp.c index 995541ce56..aac10a0338 100644 --- a/examples/c/nonlinear_chain_ocp_nlp.c +++ b/examples/c/nonlinear_chain_ocp_nlp.c @@ -1324,17 +1324,17 @@ int main() } } - int max_iter = MAX_SQP_ITERS; - double min_res_g = 1e-9; - double min_res_b = 1e-9; - double min_res_d = 1e-9; - double min_res_m = 1e-9; - - ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + int max_iter = MAX_SQP_ITERS; + double tol_stat = 1e-9; + double tol_eq = 1e-9; + double tol_ineq = 1e-9; + double tol_comp = 1e-9; + + ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); /************************************************ * ocp_nlp out diff --git a/examples/c/pendulum_scqp.cpp b/examples/c/pendulum_scqp.cpp index 1fa00e2021..83d0b9a036 100644 --- a/examples/c/pendulum_scqp.cpp +++ b/examples/c/pendulum_scqp.cpp @@ -202,16 +202,16 @@ int main() { void *nlp_opts = ocp_nlp_opts_create(config, dims); int max_iter = max_num_sqp_iterations; - double min_res_g = 1e-9; - double min_res_b = 1e-9; - double min_res_d = 1e-9; - double min_res_m = 1e-9; - - ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + double tol_stat = 1e-9; + double tol_eq = 1e-9; + double tol_ineq = 1e-9; + double tol_comp = 1e-9; + + ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); ocp_nlp_sqp_opts *sqp_opts = (ocp_nlp_sqp_opts *) nlp_opts; ((ocp_qp_partial_condensing_solver_opts *) sqp_opts->qp_solver_opts)->pcond_opts->N2 = N; diff --git a/examples/c/simple_dae_example.c b/examples/c/simple_dae_example.c index 5508c380c6..adc12409d2 100644 --- a/examples/c/simple_dae_example.c +++ b/examples/c/simple_dae_example.c @@ -315,10 +315,10 @@ int main() { ocp_nlp_sqp_opts *sqp_opts = (ocp_nlp_sqp_opts *) nlp_opts; sqp_opts->max_iter = max_num_sqp_iterations; - sqp_opts->min_res_g = 1e-6; - sqp_opts->min_res_b = 1e-6; - sqp_opts->min_res_d = 1e-6; - sqp_opts->min_res_m = 1e-6; + sqp_opts->tol_stat = 1e-6; + sqp_opts->tol_eq = 1e-6; + sqp_opts->tol_ineq = 1e-6; + sqp_opts->tol_comp = 1e-6; ((ocp_qp_partial_condensing_solver_opts *) sqp_opts->qp_solver_opts)->pcond_opts->N2 = N; ocp_nlp_out *nlp_out = ocp_nlp_out_create(config, dims); diff --git a/examples/c/wind_turbine_nmpc.c b/examples/c/wind_turbine_nmpc.c index 3adf1236b5..1c4734aa13 100644 --- a/examples/c/wind_turbine_nmpc.c +++ b/examples/c/wind_turbine_nmpc.c @@ -794,17 +794,17 @@ int main() if (plan->nlp_solver == SQP) { - int max_iter = MAX_SQP_ITERS; - double min_res_g = 1e-6; - double min_res_b = 1e-8; - double min_res_d = 1e-8; - double min_res_m = 1e-8; - - ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + int max_iter = MAX_SQP_ITERS; + double tol_stat = 1e-6; + double tol_eq = 1e-8; + double tol_ineq = 1e-8; + double tol_comp = 1e-8; + + ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); } else if (plan->nlp_solver == SQP_RTI) { diff --git a/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c index 736e3cf0ce..4098980ecf 100644 --- a/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates/acados_solver.in.c @@ -966,16 +966,16 @@ int acados_create() { {% if ocp.solver_config.nlp_solver_type == "SQP" %} int max_iter = max_num_sqp_iterations; - double min_res_g = 1e-6; - double min_res_b = 1e-6; - double min_res_d = 1e-6; - double min_res_m = 1e-6; + double tol_stat = 1e-6; + double tol_eq = 1e-6; + double tol_ineq = 1e-6; + double tol_comp = 1e-6; ocp_nlp_opts_set(nlp_config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_comp", &tol_comp); {% else %} diff --git a/interfaces/acados_template/acados_template/c_templates_inja/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_inja/acados_solver.in.c index a45a72133f..bcfa2d4ce4 100644 --- a/interfaces/acados_template/acados_template/c_templates_inja/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_inja/acados_solver.in.c @@ -783,16 +783,16 @@ int acados_create() { {% if ocp.solver_config.nlp_solver_type == "SQP" %} int max_iter = max_num_sqp_iterations; - double min_res_g = 1e-6; - double min_res_b = 1e-6; - double min_res_d = 1e-6; - double min_res_m = 1e-6; + double tol_stat = 1e-6; + double tol_eq = 1e-6; + double tol_ineq = 1e-6; + double tol_comp = 1e-6; ocp_nlp_opts_set(nlp_config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_comp", &tol_comp); {% else %} diff --git a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c index 7570453684..2223f71875 100644 --- a/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c +++ b/interfaces/acados_template/acados_template/c_templates_tera/acados_solver.in.c @@ -763,16 +763,16 @@ int acados_create() { {% if solver_config.nlp_solver_type == "SQP" %} int max_iter = max_num_sqp_iterations; - double min_res_g = 1e-6; - double min_res_b = 1e-6; - double min_res_d = 1e-6; - double min_res_m = 1e-6; + double tol_stat = 1e-6; + double tol_eq = 1e-6; + double tol_ineq = 1e-6; + double tol_comp = 1e-6; ocp_nlp_opts_set(nlp_config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(nlp_config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(nlp_config, nlp_opts, "tol_comp", &tol_comp); {% else %} diff --git a/test/ocp_nlp/test_chain.cpp b/test/ocp_nlp/test_chain.cpp index 3e2faec17a..81b04abc2a 100644 --- a/test/ocp_nlp/test_chain.cpp +++ b/test/ocp_nlp/test_chain.cpp @@ -1381,16 +1381,16 @@ void setup_and_solve_nlp(int NN, } } int max_iter = MAX_SQP_ITERS; - double min_res_g = 1e-6; - double min_res_b = 1e-6; - double min_res_d = 1e-6; - double min_res_m = 1e-6; + double tol_stat = 1e-6; + double tol_eq = 1e-6; + double tol_ineq = 1e-6; + double tol_comp = 1e-6; ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); /************************************************ * ocp_nlp out diff --git a/test/ocp_nlp/test_wind_turbine.cpp b/test/ocp_nlp/test_wind_turbine.cpp index e191edd4bb..4290d3df77 100644 --- a/test/ocp_nlp/test_wind_turbine.cpp +++ b/test/ocp_nlp/test_wind_turbine.cpp @@ -930,16 +930,16 @@ void setup_and_solve_nlp(std::string const& integrator_str, std::string const& q } int max_iter = MAX_SQP_ITERS; - double min_res_g = 1e-6; - double min_res_b = 1e-8; - double min_res_d = 1e-8; - double min_res_m = 1e-8; + double tol_stat = 1e-6; + double tol_eq = 1e-8; + double tol_ineq = 1e-8; + double tol_comp = 1e-8; ocp_nlp_opts_set(config, nlp_opts, "max_iter", &max_iter); - ocp_nlp_opts_set(config, nlp_opts, "min_res_g", &min_res_g); - ocp_nlp_opts_set(config, nlp_opts, "min_res_b", &min_res_b); - ocp_nlp_opts_set(config, nlp_opts, "min_res_d", &min_res_d); - ocp_nlp_opts_set(config, nlp_opts, "min_res_m", &min_res_m); + ocp_nlp_opts_set(config, nlp_opts, "tol_stat", &tol_stat); + ocp_nlp_opts_set(config, nlp_opts, "tol_eq", &tol_eq); + ocp_nlp_opts_set(config, nlp_opts, "tol_ineq", &tol_ineq); + ocp_nlp_opts_set(config, nlp_opts, "tol_comp", &tol_comp); // partial condensing From 80b4be3a34940d30411a81fb62ec6675bf4a6ba7 Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 15:28:26 +0200 Subject: [PATCH 47/78] matlab interface: add nlp tolerances --- .../pendulum_on_cart_model/example_ocp.m | 8 ++++ interfaces/acados_matlab/acados_ocp_opts.m | 8 ++++ interfaces/acados_matlab/ocp_create.c | 45 +++++++++++++++++++ 3 files changed, 61 insertions(+) diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index f6010d482b..0de410aa38 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -29,6 +29,10 @@ %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; +nlp_solver_tol_stat = 1e-8; +nlp_solver_tol_eq = 1e-8; +nlp_solver_tol_ineq = 1e-8; +nlp_solver_tol_comp = 1e-8; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_cond_N = 5; @@ -184,6 +188,10 @@ ocp_opts.set('regularize_method', regularize_method); if (strcmp(nlp_solver, 'sqp')) ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); + ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat); + ocp_opts.set('nlp_solver_tol_eq', nlp_solver_tol_eq); + ocp_opts.set('nlp_solver_tol_ineq', nlp_solver_tol_ineq); + ocp_opts.set('nlp_solver_tol_comp', nlp_solver_tol_comp); end ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 8ef028a63e..1dc3ccfb65 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -42,6 +42,14 @@ obj.opts_struct.nlp_solver_exact_hessian = value; elseif (strcmp(field, 'nlp_solver_max_iter')) obj.opts_struct.nlp_solver_max_iter = value; + elseif (strcmp(field, 'nlp_solver_tol_stat')) + obj.opts_struct.nlp_solver_tol_stat = value; + elseif (strcmp(field, 'nlp_solver_tol_eq')) + obj.opts_struct.nlp_solver_tol_eq = value; + elseif (strcmp(field, 'nlp_solver_tol_ineq')) + obj.opts_struct.nlp_solver_tol_ineq = value; + elseif (strcmp(field, 'nlp_solver_tol_comp')) + obj.opts_struct.nlp_solver_tol_comp = value; elseif (strcmp(field, 'qp_solver')) obj.opts_struct.qp_solver = value; elseif (strcmp(field, 'qp_solver_cond_N')) diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index df7a3931fe..18e83953d4 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -619,6 +619,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) char *nlp_solver; bool nlp_solver_exact_hessian; int nlp_solver_max_iter; bool set_nlp_solver_max_iter = false; + double nlp_solver_tol_stat; bool set_nlp_solver_tol_stat = false; + double nlp_solver_tol_eq; bool set_nlp_solver_tol_eq = false; + double nlp_solver_tol_ineq; bool set_nlp_solver_tol_ineq = false; + double nlp_solver_tol_comp; bool set_nlp_solver_tol_comp = false; char *qp_solver; int qp_solver_cond_N; bool set_qp_solver_cond_N = false; int qp_solver_cond_ric_alg; bool set_qp_solver_cond_ric_alg = false; @@ -656,6 +660,27 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) set_nlp_solver_max_iter = true; nlp_solver_max_iter = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_max_iter" ) ); } + // nlp solver exit tolerances + if(mxGetField( prhs[1], 0, "nlp_solver_tol_stat" )!=NULL) + { + set_nlp_solver_tol_stat = true; + nlp_solver_tol_stat = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_tol_stat" ) ); + } + if(mxGetField( prhs[1], 0, "nlp_solver_tol_eq" )!=NULL) + { + set_nlp_solver_tol_eq = true; + nlp_solver_tol_eq = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_tol_eq" ) ); + } + if(mxGetField( prhs[1], 0, "nlp_solver_tol_ineq" )!=NULL) + { + set_nlp_solver_tol_ineq = true; + nlp_solver_tol_ineq = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_tol_ineq" ) ); + } + if(mxGetField( prhs[1], 0, "nlp_solver_tol_comp" )!=NULL) + { + set_nlp_solver_tol_comp = true; + nlp_solver_tol_comp = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_tol_comp" ) ); + } // qp_solver // TODO check qp_solver = mxArrayToString( mxGetField( prhs[1], 0, "qp_solver" ) ); @@ -1071,6 +1096,26 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "max_iter", &nlp_solver_max_iter); } + // nlp_solver_tol_stat + if(set_nlp_solver_tol_stat) + { + ocp_nlp_opts_set(config, opts, "tol_stat", &nlp_solver_tol_stat); + } + // nlp_solver_tol_eq + if(set_nlp_solver_tol_eq) + { + ocp_nlp_opts_set(config, opts, "tol_eq", &nlp_solver_tol_eq); + } + // nlp_solver_tol_ineq + if(set_nlp_solver_tol_ineq) + { + ocp_nlp_opts_set(config, opts, "tol_ineq", &nlp_solver_tol_ineq); + } + // nlp_solver_tol_comp + if(set_nlp_solver_tol_comp) + { + ocp_nlp_opts_set(config, opts, "tol_comp", &nlp_solver_tol_comp); + } // qp_solver_cond_N if(set_qp_solver_cond_N) { From 342de95517c7881e46cfa5c295b8b98c206fa5fc Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 15:32:11 +0200 Subject: [PATCH 48/78] matlab interface: clean sim opts & model class --- interfaces/acados_matlab/acados_sim_model.m | 41 +-------------------- interfaces/acados_matlab/acados_sim_opts.m | 40 ++++---------------- 2 files changed, 10 insertions(+), 71 deletions(-) diff --git a/interfaces/acados_matlab/acados_sim_model.m b/interfaces/acados_matlab/acados_sim_model.m index 4afbc076f2..012ddf00dc 100644 --- a/interfaces/acados_matlab/acados_sim_model.m +++ b/interfaces/acados_matlab/acados_sim_model.m @@ -3,24 +3,6 @@ properties - name - %dims - dim_nx - dim_nu - dim_nz - dim_np - % symbolics - sym_x - sym_u - sym_xdot - sym_z - sym_p - % dynamics - dyn_type - dyn_expr_f - dyn_param_f - T - seed_adj model_struct end %properties @@ -30,62 +12,43 @@ function obj = acados_sim_model() - % default values - obj.name = 'sim_model'; - % default values obj.model_struct = struct; - obj.dyn_param_f = 'false'; - % initialize model struct - obj.model_struct.name = obj.name; - obj.model_struct.dyn_param_f = obj.dyn_param_f; + obj.model_struct.name = 'sim_model'; + obj.model_struct.dyn_param_f = 'false'; end function obj = set(obj, field, value) % dims if (strcmp(field, 'dim_nx')) - obj.dim_nx = value; obj.model_struct.dim_nx = value; elseif (strcmp(field, 'dim_nu')) - obj.dim_nu = value; obj.model_struct.dim_nu = value; elseif (strcmp(field, 'dim_nz')) - obj.dim_nz = value; obj.model_struct.dim_nz = value; elseif (strcmp(field, 'dim_np')) - obj.dim_np = value; obj.model_struct.dim_np = value; % symbolics elseif (strcmp(field, 'sym_x')) - obj.sym_x = value; obj.model_struct.sym_x = value; elseif (strcmp(field, 'sym_xdot')) - obj.sym_xdot = value; obj.model_struct.sym_xdot = value; elseif (strcmp(field, 'sym_u')) - obj.sym_u = value; obj.model_struct.sym_u = value; elseif (strcmp(field, 'sym_z')) - obj.sym_z = value; obj.model_struct.sym_z = value; elseif (strcmp(field, 'sym_p')) - obj.sym_p = value; obj.model_struct.sym_p = value; % dynamics elseif (strcmp(field, 'dyn_type')) - obj.dyn_type = value; obj.model_struct.dyn_type = value; elseif (strcmp(field, 'dyn_param_f')) - obj.dyn_param_f = value; obj.model_struct.dyn_param_f = value; elseif (strcmp(field, 'dyn_expr_f')) - obj.dyn_expr_f = value; obj.model_struct.dyn_expr_f = value; elseif (strcmp(field, 'T')) - obj.T = value; obj.model_struct.T = value; elseif (strcmp(field, 'seed_adj')) - obj.seed_adj = value; obj.model_struct.seed_adj = value; else disp(['acados_sim_model: set: wrong field: ', field]); diff --git a/interfaces/acados_matlab/acados_sim_opts.m b/interfaces/acados_matlab/acados_sim_opts.m index 18030e6b66..a4850ea9ac 100644 --- a/interfaces/acados_matlab/acados_sim_opts.m +++ b/interfaces/acados_matlab/acados_sim_opts.m @@ -3,14 +3,6 @@ properties - compile_mex - codgen_model - num_stages - num_steps - method - sens_forw - sens_adj - sens_hess opts_struct end % properties @@ -20,50 +12,34 @@ function obj = acados_sim_opts() - obj.compile_mex = 'true'; - obj.codgen_model = 'true'; - obj.method = 'irk'; - obj.num_stages = 4; - obj.num_steps = 1; - obj.sens_forw = 'false'; - obj.sens_adj = 'false'; - obj.sens_hess = 'false'; obj.opts_struct = struct; - obj.opts_struct.compile_mex = obj.compile_mex; - obj.opts_struct.codgen_model = obj.codgen_model; - obj.opts_struct.method = obj.method; - obj.opts_struct.num_stages = obj.num_stages; - obj.opts_struct.num_steps = obj.num_steps; - obj.opts_struct.sens_forw = obj.sens_forw; - obj.opts_struct.sens_adj = obj.sens_adj; - obj.opts_struct.sens_hess = obj.sens_hess; + obj.opts_struct.compile_mex = 'true'; + obj.opts_struct.codgen_model = 'true'; + obj.opts_struct.method = 'irk'; + obj.opts_struct.num_stages = 4; + obj.opts_struct.num_steps = 1; + obj.opts_struct.sens_forw = 'false'; + obj.opts_struct.sens_adj = 'false'; + obj.opts_struct.sens_hess = 'false'; end function obj = set(obj, field, value) if (strcmp(field, 'compile_mex')) - obj.compile_mex = value; obj.opts_struct.compile_mex = value; elseif (strcmp(field, 'codgen_model')) - obj.codgen_model = value; obj.opts_struct.codgen_model = value; elseif (strcmp(field, 'num_stages')) - obj.num_stages = value; obj.opts_struct.num_stages = value; elseif (strcmp(field, 'num_steps')) - obj.num_steps = value; obj.opts_struct.num_steps = value; elseif (strcmp(field, 'method')) - obj.method = value; obj.opts_struct.method = value; elseif (strcmp(field, 'sens_forw')) - obj.sens_forw = value; obj.opts_struct.sens_forw = value; elseif (strcmp(field, 'sens_adj')) - obj.sens_adj = value; obj.opts_struct.sens_adj = value; elseif (strcmp(field, 'sens_hess')) - obj.sens_hess = value; obj.opts_struct.sens_hess = value; else disp(['acados_sim_opts: set: wrong field: ', field]); From ba1c57a702b636befcb3fed70d6e5252bb95982e Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 16:35:00 +0200 Subject: [PATCH 49/78] update blasfeo & hpipm (cmake fixes) --- external/blasfeo | 2 +- external/hpipm | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/external/blasfeo b/external/blasfeo index 93fb7e7f89..75a3dd8637 160000 --- a/external/blasfeo +++ b/external/blasfeo @@ -1 +1 @@ -Subproject commit 93fb7e7f897b89e3ff8b1e8fb87d184a36be5b41 +Subproject commit 75a3dd8637b5aba1a6db99d30686d645d3ece175 diff --git a/external/hpipm b/external/hpipm index 7ab221e966..9ed6084f02 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 7ab221e966db2a676d053228b96ad42ba1b8f529 +Subproject commit 9ed6084f02c3447c3067c6bbbc7b3543cd32cd02 From 6aa8039a4a9d3e14d436e5a7dd053e6a669e7032 Mon Sep 17 00:00:00 2001 From: giaf Date: Mon, 3 Jun 2019 16:57:58 +0200 Subject: [PATCH 50/78] update hpipm --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index 9ed6084f02..4025a82361 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 9ed6084f02c3447c3067c6bbbc7b3543cd32cd02 +Subproject commit 4025a82361ec19d939e7b5297ebee06b9abe0a87 From 8aa1edf16b1c16cb40df8bfd63672ae15ca0154c Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Wed, 5 Jun 2019 17:21:51 +0200 Subject: [PATCH 51/78] adding explicit setters according to giaf's style --- .../acados_template/acados_ocp_nlp.py | 23 +++++++++++++++++++ 1 file changed, 23 insertions(+) diff --git a/interfaces/acados_template/acados_template/acados_ocp_nlp.py b/interfaces/acados_template/acados_template/acados_ocp_nlp.py index bdb79d2695..b8aeb3fdce 100644 --- a/interfaces/acados_template/acados_template/acados_ocp_nlp.py +++ b/interfaces/acados_template/acados_template/acados_ocp_nlp.py @@ -261,6 +261,9 @@ def N(self, N): else: raise Exception('Invalid N value. Exiting.') + def set(self, attr, value): + setattr(self, attr, value) + class ocp_nlp_cost: """ class containing the description of the cost @@ -466,6 +469,9 @@ def zu_e(self, zu_e): else: raise Exception('Invalid zu_e value. Exiting.') + def set(self, attr, value): + setattr(self, attr, value) + class ocp_nlp_constraints: """ class containing the description of the constraints @@ -824,6 +830,9 @@ def p(self, p): else: raise Exception('Invalid p value. Exiting.') + def set(self, attr, value): + setattr(self, attr, value) + class ocp_nlp_solver_config: """ class containing the description of the solver configuration @@ -899,6 +908,9 @@ def nlp_solver_type(self, nlp_solver_type): raise Exception('Invalid nlp_solver_type value. Possible values are:\n\n' \ + ',\n'.join(nlp_solver_types) + '.\n\nYou have: ' + nlp_solver_type + '.\n\nExiting.') + def set(self, attr, value): + setattr(self, attr, value) + class acados_ocp_nlp: """ class containing the full description if the optimal control problem @@ -917,6 +929,17 @@ def __init__(self): self.acados_include_path = [] self.acados_lib_path = [] + def set(self, attr, value): + # tokenize string + tokens = attr.split('_', 1) + if len(tokens) > 1: + setter_to_call = getattr(getattr(self, tokens[0]), 'set') + else: + setter_to_call = getattr(self, 'set') + + setter_to_call(tokens[1], value) + return + def check_ra(ra): """ (DEPRECATED) function that checks the consistency of the optimal control description From 93c9801a5167d0cd301d06d0fa3a49bd42f41414 Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 5 Jun 2019 21:29:58 +0200 Subject: [PATCH 52/78] ocp nlp reg: fix to convexification & add lam ecc to all reg instances --- acados/ocp_nlp/ocp_nlp_common.c | 24 ++- acados/ocp_nlp/ocp_nlp_reg_common.c | 22 +- acados/ocp_nlp/ocp_nlp_reg_common.h | 6 + acados/ocp_nlp/ocp_nlp_reg_convexify.c | 193 ++++++++++++++++-- acados/ocp_nlp/ocp_nlp_reg_convexify.h | 6 + acados/ocp_nlp/ocp_nlp_reg_mirror.c | 24 +++ acados/ocp_nlp/ocp_nlp_reg_noreg.c | 24 +++ acados/ocp_nlp/ocp_nlp_reg_project.c | 24 +++ .../ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 102 ++------- acados/ocp_nlp/ocp_nlp_sqp.c | 31 +++ acados/ocp_nlp/ocp_nlp_sqp.h | 6 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 3 + acados/ocp_qp/ocp_qp_common.c | 38 ++++ 13 files changed, 394 insertions(+), 109 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 8e480519ba..05f8fca8fb 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -541,8 +541,12 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c // update qp_solver dims if ( (!strcmp(field, "nbx")) || (!strcmp(field, "nbu")) ) { - config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, - field, int_value); + // qp solver + config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, field, int_value); + + // regularization + config->regularize->dims_set(config->regularize, dims->regularize, i, (char *) field, int_value); + // Note(oj): how to decide if ocp_qp or dense? plan not available here.. // Note(giaf): the NLP solver always calls a xcond_solver, // so only its dimensions should be set now @@ -565,18 +569,21 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c { // update ng_qp_solver in qp_solver int ng, nh, ng_qp_solver; - config->constraints[i]->get_dims(config->constraints[i], - dims->constraints[i], "ng", &ng); - config->constraints[i]->get_dims(config->constraints[i], - dims->constraints[i], "nh", &nh); + config->constraints[i]->get_dims(config->constraints[i], dims->constraints[i], "ng", &ng); + config->constraints[i]->get_dims(config->constraints[i], dims->constraints[i], "nh", &nh); ng_qp_solver = ng + nh; + // qp solver config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, "ng", &ng_qp_solver); + + // regularization + config->regularize->dims_set(config->regularize, dims->regularize, i, "ng", &ng_qp_solver); } } + void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field, const void* value_) { @@ -589,6 +596,8 @@ void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, config->cost[stage]->dims_set(config->cost[stage], dims->cost[stage], field, int_value); } + + void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field, const void* value) { @@ -598,8 +607,7 @@ void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, int *int_value = (int *) value; - config->dynamics[stage]->dims_set(config->dynamics[stage], - dims->dynamics[stage], field, int_value); + config->dynamics[stage]->dims_set(config->dynamics[stage], dims->dynamics[stage], field, int_value); } diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.c b/acados/ocp_nlp/ocp_nlp_reg_common.c index 5b9c28bdf4..3af5381e91 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.c +++ b/acados/ocp_nlp/ocp_nlp_reg_common.c @@ -54,7 +54,7 @@ int ocp_nlp_reg_dims_calculate_size(int N) { int size = sizeof(ocp_nlp_reg_dims); - size += 2*(N+1)*sizeof(int); // nx nu + size += 5*(N+1)*sizeof(int); // nx nu nbu nbx ng return size; } @@ -74,6 +74,14 @@ ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory) // nu dims->nu = (int *) c_ptr; c_ptr += (N+1)*sizeof(int); + // nbu + dims->nbu = (int *) c_ptr; + // nbx + dims->nbx = (int *) c_ptr; + c_ptr += (N+1)*sizeof(int); + // ng + dims->ng = (int *) c_ptr; + c_ptr += (N+1)*sizeof(int); dims->N = N; @@ -95,6 +103,18 @@ void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char { dims->nu[stage] = *value; } + else if (!strcmp(field, "nbu")) + { + dims->nbu[stage] = *value; + } + else if (!strcmp(field, "nbx")) + { + dims->nbx[stage] = *value; + } + else if (!strcmp(field, "ng")) + { + dims->ng[stage] = *value; + } else { printf("\nerror: field %s not available in module ocp_nlp_reg_dims_set\n", field); diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.h b/acados/ocp_nlp/ocp_nlp_reg_common.h index 7e994751b1..53e1c8b1ad 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.h +++ b/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -41,6 +41,9 @@ typedef struct { int *nx; int *nu; + int *nbu; + int *nbx; + int *ng; int N; } ocp_nlp_reg_dims; @@ -74,8 +77,11 @@ typedef struct void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory); + void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); + void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory); /* functions */ void (*regularize_hessian)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory); diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index c8f7206827..652c8fc0b7 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -96,9 +96,12 @@ void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_) { + int N = dims->N; int *nx = dims->nx; int *nu = dims->nu; - int N = dims->N; + int *nbx = dims->nbx; + int *nbu = dims->nbu; + int *ng = dims->ng; int ii; @@ -120,6 +123,12 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; } + int nbgM = nbu[0]+nbx[0]+ng[0]; + for(ii=1; ii<=N; ii++) + { + nbgM = nbu[ii]+nbx[ii]+ng[ii]>nbgM ? nbu[ii]+nbx[ii]+ng[ii] : nbgM; + } + int size = 0; size += sizeof(ocp_nlp_reg_convexify_memory); @@ -138,6 +147,9 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims size += blasfeo_memsize_dmat(nuM, nuM); // L size += blasfeo_memsize_dmat(nxM, nuM); // St_copy + size += blasfeo_memsize_dvec(nuxM); // tmp_nuxM + size += blasfeo_memsize_dvec(nbgM); // tmp_nbgM + for (ii=0; ii<=N; ii++) { size += blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq @@ -146,8 +158,9 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims // size += 2*blasfeo_memsize_dvec(nxM); // grad b2 // giaf's - size += ((N+1)+N)*sizeof(struct blasfeo_dmat *); // RSQrq BAbt - size += (2*(N+1)+2*N)*sizeof(struct blasfeo_dvec *); // rq b ux pi + size += (2*(N+1)+N)*sizeof(struct blasfeo_dmat *); // RSQrq BAbt DCt + size += (3*(N+1)+2*N)*sizeof(struct blasfeo_dvec *); // rq b ux pi lam + size += (N+1)*sizeof(int *); // idxb return size; } @@ -157,9 +170,12 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory) { + int N = dims->N; int *nx = dims->nx; int *nu = dims->nu; - int N = dims->N; + int *nbx = dims->nbx; + int *nbu = dims->nbu; + int *ng = dims->ng; int ii; @@ -181,6 +197,12 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM; } + int nbgM = nbu[0]+nbx[0]+ng[0]; + for(ii=1; ii<=N; ii++) + { + nbgM = nbu[ii]+nbx[ii]+ng[ii]>nbgM ? nbu[ii]+nbx[ii]+ng[ii] : nbgM; + } + char *c_ptr = (char *) raw_memory; @@ -211,6 +233,9 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, mem->BAbt = (struct blasfeo_dmat **) c_ptr; c_ptr += N*sizeof(struct blasfeo_dmat *); + mem->DCt = (struct blasfeo_dmat **) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); + mem->rq = (struct blasfeo_dvec **) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dvec *); @@ -223,6 +248,12 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, mem->pi = (struct blasfeo_dvec **) c_ptr; c_ptr += N*sizeof(struct blasfeo_dvec *); + mem->lam = (struct blasfeo_dvec **) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dvec *); + + mem->idxb = (int **) c_ptr; + c_ptr += (N+1)*sizeof(int *); + align_char_to(64, &c_ptr); assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->Q_tilde, &c_ptr); @@ -237,6 +268,9 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], &c_ptr); } + assign_and_advance_blasfeo_dvec_mem(nuxM, &mem->tmp_nuxM, &c_ptr); + assign_and_advance_blasfeo_dvec_mem(nbgM, &mem->tmp_nbgM, &c_ptr); + // assign_and_advance_blasfeo_dvec_mem(nxM, &mem->grad, &c_ptr); // assign_and_advance_blasfeo_dvec_mem(nxM, &mem->b2, &c_ptr); @@ -331,6 +365,46 @@ void ocp_nlp_reg_convexify_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasf +void ocp_nlp_reg_convexify_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + ocp_nlp_reg_convexify_memory *memory = memory_; + + int ii; + + int N = dims->N; + + for(ii=0; ii<=N; ii++) + { + memory->idxb[ii] = idxb[ii]; + } + + return; +} + + + +void ocp_nlp_reg_convexify_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + ocp_nlp_reg_convexify_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nx = dims->nx; + int *nu = dims->nu; + int *ng = dims->ng; + + for(ii=0; ii<=N; ii++) + { + memory->DCt[ii] = DCt+ii; +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, ng[ii], memory->DCt[ii], 0, 0); + } + + return; +} + + + void ocp_nlp_reg_convexify_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) { ocp_nlp_reg_convexify_memory *memory = memory_; @@ -373,6 +447,28 @@ void ocp_nlp_reg_convexify_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blas +void ocp_nlp_reg_convexify_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + ocp_nlp_reg_convexify_memory *memory = memory_; + + int ii; + + int N = dims->N; + int *nbu = dims->nbu; + int *nbx = dims->nbx; + int *ng = dims->ng; + + for(ii=0; ii<=N; ii++) + { + memory->lam[ii] = lam+ii; +// blasfeo_print_dvec(2*nbu[ii]+2*nbx[ii]+2*ng[ii], memory->lam[ii], 0); + } + + return; +} + + + void ocp_nlp_reg_convexify_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) { @@ -396,6 +492,16 @@ void ocp_nlp_reg_convexify_memory_set(void *config_, ocp_nlp_reg_dims *dims, voi struct blasfeo_dvec *b = value; ocp_nlp_reg_convexify_memory_set_b_ptr(dims, b, memory_); } + else if(!strcmp(field, "idxb_ptr")) + { + int **idxb = value; + ocp_nlp_reg_convexify_memory_set_idxb_ptr(dims, idxb, memory_); + } + else if(!strcmp(field, "DCt_ptr")) + { + struct blasfeo_dmat *DCt = value; + ocp_nlp_reg_convexify_memory_set_DCt_ptr(dims, DCt, memory_); + } else if(!strcmp(field, "ux_ptr")) { struct blasfeo_dvec *ux = value; @@ -406,6 +512,11 @@ void ocp_nlp_reg_convexify_memory_set(void *config_, ocp_nlp_reg_dims *dims, voi struct blasfeo_dvec *pi = value; ocp_nlp_reg_convexify_memory_set_pi_ptr(dims, pi, memory_); } + else if(!strcmp(field, "lam_ptr")) + { + struct blasfeo_dvec *lam = value; + ocp_nlp_reg_convexify_memory_set_lam_ptr(dims, lam, memory_); + } else { printf("\nerror: field %s not available in ocp_nlp_reg_convexify_set\n", field); @@ -434,17 +545,29 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di int *nu = dims->nu; int N = dims->N; +#if 0 + for(ii=0; ii<=N; ii++) + { + blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0); + blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); + } + return; +#endif + double delta = opts->delta; // Algorithm 6 from Verschueren2017 + blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0); // TODO regularize R at last stage if needed !!! - blasfeo_dgese(nx[N], nu[N]+nx[N], 0.0, &mem->delta_eye, 0, 0); + // TODO fix for nu[N]>0 !!!!!!!!!! + blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0); blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0); blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0); - blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], 0, 0, &mem->Q_bar, 0, 0); + blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0); + blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]); blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0); blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0); @@ -470,9 +593,10 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di // TODO implement using cholesky blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0); - blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + // printf("BAQ\n"); // blasfeo_print_dmat(nx+nu, nx, &BAQ, 0, 0); @@ -486,6 +610,7 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di if (needs_regularization) { + // TODO project only nu instead ??????????? blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); @@ -495,16 +620,21 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di // blasfeo_print_dmat(nx+nu+1, nx+nu, &work->qp_in->RSQrq[i], 0, 0); + // backup Q blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0); - blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0); // R = L * L^T blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0); // Q = S^T * L^-T - blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0); +// blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0); + blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0); - // Q = S^T * R^-1 * S - blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + // Q = S^T * R^-1 * S + delta*I + blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0); + blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0); +// blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); + blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]); // printf("H_tilde\n"); // blasfeo_print_dmat(nu+nx, nu+nx, &work->qp_in->RSQrq[i], 0, 0); @@ -543,18 +673,46 @@ void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims ocp_nlp_reg_convexify_memory *mem = mem_; ocp_nlp_reg_convexify_opts *opts = opts_; - int ii; + int ii, ss; + int N = dims->N; int *nx = dims->nx; int *nu = dims->nu; - int N = dims->N; - + int *nbx = dims->nbx; + int *nbu = dims->nbu; + int *ng = dims->ng; + +// printf("\nRSQrq reg\n"); +// for(ii=0; ii<=N; ii++) +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); + + // restore original hessian and gradient + for(ii=0; ii<=N; ii++) + { + blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0); + blasfeo_drowex(nu[ii]+nx[ii], 1.0, &mem->original_RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0); + } + +// printf("\nRSQrq orig\n"); +// for(ii=0; ii<=N; ii++) +// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); + +// blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0); blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0); + // TODO use memory to avoid double constr evaluation for(ii=1; iiRSQrq[N-ii], nu[N-ii], 0, mem->ux[N-ii], 0, 1.0, mem->rq[N-ii], nu[N-ii], mem->pi[N-ii-1], 0); - blasfeo_dgemv_n(nx[N-ii], nx[N-ii+1], 1.0, mem->BAbt[N-ii], nu[N-ii], 0, mem->pi[N-ii], 0, 1.0, mem->pi[N-ii-1], 0, mem->pi[N-ii-1], 0); + ss = N-ii; +// printf("\n%d %d %d %d %d %d\n", ss, nx[ss], nu[ss], nbx[ss], nbu[ss], ng[ss]); +// blasfeo_dgemv_n(nx[ss], nu[ss]+nx[ss], 1.0, mem->RSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0); + blasfeo_dgemv_n(nx[ss], nu[ss]+nx[ss], 1.0, mem->RSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0); + blasfeo_dgemv_n(nx[ss], nx[ss+1], 1.0, mem->BAbt[ss], nu[ss], 0, mem->pi[ss], 0, 1.0, mem->pi[ss-1], 0, mem->pi[ss-1], 0); + +// blasfeo_daxpy(nbu[ss]+nbx[ss]+ng[ss], -1.0, mem->lam[ss], 0, mem->lam[ss], nbu[ss]+nbx[ss]+ng[ss], &mem->tmp_nbgM, 0); +// blasfeo_dvecse(nu[ss]+nx[ss], 0.0, &mem->tmp_nuxM, 0); +// blasfeo_dvecad_sp(nbu[ss]+nbx[ss], -1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0); +// blasfeo_print_tran_dvec(nu[ss]+nx[ss], &mem->tmp_nuxM, 0); } return; @@ -581,8 +739,11 @@ void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_rq_ptr = &ocp_nlp_reg_convexify_memory_set_rq_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_reg_convexify_memory_set_BAbt_ptr; config->memory_set_b_ptr = &ocp_nlp_reg_convexify_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_convexify_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_convexify_memory_set_DCt_ptr; config->memory_set_ux_ptr = &ocp_nlp_reg_convexify_memory_set_ux_ptr; config->memory_set_pi_ptr = &ocp_nlp_reg_convexify_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_convexify_memory_set_lam_ptr; // functions config->regularize_hessian = &ocp_nlp_reg_convexify_regularize_hessian; config->correct_dual_sol = &ocp_nlp_reg_convexify_correct_dual_sol; diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/acados/ocp_nlp/ocp_nlp_reg_convexify.h index a42f33763f..8307666bcb 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -87,6 +87,9 @@ typedef struct { struct blasfeo_dmat *original_RSQrq; + struct blasfeo_dvec tmp_nuxM; + struct blasfeo_dvec tmp_nbgM; + // struct blasfeo_dvec grad; // struct blasfeo_dvec b2; @@ -95,8 +98,11 @@ typedef struct { struct blasfeo_dvec **rq; // pointer to rq in qp_in struct blasfeo_dmat **BAbt; // pointer to BAbt in qp_in struct blasfeo_dvec **b; // pointer to b in qp_in + struct blasfeo_dmat **DCt; // pointer to DCt in qp_in struct blasfeo_dvec **ux; // pointer to ux in qp_out struct blasfeo_dvec **pi; // pointer to pi in qp_out + struct blasfeo_dvec **lam; // pointer to lam in qp_out + int **idxb; // pointer to idxb in qp_in } ocp_nlp_reg_convexify_memory; diff --git a/acados/ocp_nlp/ocp_nlp_reg_mirror.c b/acados/ocp_nlp/ocp_nlp_reg_mirror.c index c69152aa94..640fb5e7c5 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_mirror.c +++ b/acados/ocp_nlp/ocp_nlp_reg_mirror.c @@ -197,6 +197,20 @@ void ocp_nlp_reg_mirror_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_ +void ocp_nlp_reg_mirror_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_mirror_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_mirror_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) { return; @@ -211,6 +225,13 @@ void ocp_nlp_reg_mirror_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo +void ocp_nlp_reg_mirror_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_mirror_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) { @@ -285,8 +306,11 @@ void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_rq_ptr = &ocp_nlp_reg_mirror_memory_set_rq_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_reg_mirror_memory_set_BAbt_ptr; config->memory_set_b_ptr = &ocp_nlp_reg_mirror_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_mirror_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_mirror_memory_set_DCt_ptr; config->memory_set_ux_ptr = &ocp_nlp_reg_mirror_memory_set_ux_ptr; config->memory_set_pi_ptr = &ocp_nlp_reg_mirror_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_mirror_memory_set_lam_ptr; // functions config->regularize_hessian = &ocp_nlp_reg_mirror_regularize_hessian; config->correct_dual_sol = &ocp_nlp_reg_mirror_correct_dual_sol; diff --git a/acados/ocp_nlp/ocp_nlp_reg_noreg.c b/acados/ocp_nlp/ocp_nlp_reg_noreg.c index a0317d4721..0707ec83b6 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_noreg.c +++ b/acados/ocp_nlp/ocp_nlp_reg_noreg.c @@ -116,6 +116,20 @@ void ocp_nlp_reg_noreg_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_d +void ocp_nlp_reg_noreg_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_noreg_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_noreg_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) { return; @@ -130,6 +144,13 @@ void ocp_nlp_reg_noreg_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_ +void ocp_nlp_reg_noreg_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_noreg_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) { @@ -177,8 +198,11 @@ void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_rq_ptr = &ocp_nlp_reg_noreg_memory_set_rq_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_reg_noreg_memory_set_BAbt_ptr; config->memory_set_b_ptr = &ocp_nlp_reg_noreg_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_noreg_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_noreg_memory_set_DCt_ptr; config->memory_set_ux_ptr = &ocp_nlp_reg_noreg_memory_set_ux_ptr; config->memory_set_pi_ptr = &ocp_nlp_reg_noreg_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_noreg_memory_set_lam_ptr; // functions config->regularize_hessian = &ocp_nlp_reg_noreg_regularize_hessian; config->correct_dual_sol = &ocp_nlp_reg_noreg_correct_dual_sol; diff --git a/acados/ocp_nlp/ocp_nlp_reg_project.c b/acados/ocp_nlp/ocp_nlp_reg_project.c index 0bad684a6b..e9f720d994 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project.c @@ -197,6 +197,20 @@ void ocp_nlp_reg_project_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo +void ocp_nlp_reg_project_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ + return; +} + + + +void ocp_nlp_reg_project_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_project_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) { return; @@ -211,6 +225,13 @@ void ocp_nlp_reg_project_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfe +void ocp_nlp_reg_project_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_) +{ + return; +} + + + void ocp_nlp_reg_project_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value) { @@ -285,8 +306,11 @@ void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config) config->memory_set_rq_ptr = &ocp_nlp_reg_project_memory_set_rq_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_reg_project_memory_set_BAbt_ptr; config->memory_set_b_ptr = &ocp_nlp_reg_project_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_project_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_project_memory_set_DCt_ptr; config->memory_set_ux_ptr = &ocp_nlp_reg_project_memory_set_ux_ptr; config->memory_set_pi_ptr = &ocp_nlp_reg_project_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_project_memory_set_lam_ptr; // functions config->regularize_hessian = &ocp_nlp_reg_project_regularize_hessian; config->correct_dual_sol = &ocp_nlp_reg_project_correct_dual_sol; diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index 788b6756b8..9afe778e59 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -204,14 +204,11 @@ void ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, int ii; - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; + int N = dims->N; for(ii=0; ii<=N; ii++) { memory->RSQrq[ii] = RSQrq+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0); } return; @@ -221,22 +218,6 @@ void ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, void ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_) { -#if 0 - ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; - - int ii; - - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; - - for(ii=0; ii<=N; ii++) - { - memory->rq[ii] = rq+ii; -// blasfeo_print_dvec(nu[ii]+nx[ii], memory->rq[ii], 0); - } -#endif - return; } @@ -248,14 +229,11 @@ void ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, int ii; - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; + int N = dims->N; for(ii=0; iiBAbt[ii] = BAbt+ii; -// blasfeo_print_dmat(nu[ii]+nx[ii]+1, nx[ii+1], memory->BAbt[ii], 0, 0); } return; @@ -265,45 +243,27 @@ void ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, void ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_) { -#if 0 - ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; - - int ii; + return; +} - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; - for(ii=0; iib[ii] = b+ii; -// blasfeo_print_dvec(nx[ii=1], memory->b[ii], 0); - } -#endif +void ocp_nlp_reg_project_reduc_hess_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_) +{ return; } -void ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) +void ocp_nlp_reg_project_reduc_hess_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_) { -#if 0 - ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; - - int ii; + return; +} - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; - for(ii=0; ii<=N; ii++) - { - memory->ux[ii] = ux+ii; -// blasfeo_print_dvec(nu[ii]+nx[ii], memory->ux[ii], 0); - } -#endif +void ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_) +{ return; } @@ -311,22 +271,13 @@ void ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, st void ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_) { -#if 0 - ocp_nlp_reg_project_reduc_hess_memory *memory = memory_; - - int ii; + return; +} - int N = dims->N; - int *nx = dims->nx; - int *nu = dims->nu; - for(ii=0; iipi[ii] = pi+ii; -// blasfeo_print_dvec(nx[ii+1], memory->pi[ii], 0); - } -#endif +void ocp_nlp_reg_project_reduc_hess_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_) +{ return; } @@ -340,31 +291,11 @@ void ocp_nlp_reg_project_reduc_hess_memory_set(void *config_, ocp_nlp_reg_dims * struct blasfeo_dmat *RSQrq = value; ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(dims, RSQrq, memory_); } -// else if(!strcmp(field, "rq_ptr")) -// { -// struct blasfeo_dvec *rq = value; -// ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr(dims, rq, memory_); -// } else if(!strcmp(field, "BAbt_ptr")) { struct blasfeo_dmat *BAbt = value; ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(dims, BAbt, memory_); } -// else if(!strcmp(field, "b_ptr")) -// { -// struct blasfeo_dvec *b = value; -// ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr(dims, b, memory_); -// } -// else if(!strcmp(field, "ux_ptr")) -// { -// struct blasfeo_dvec *ux = value; -// ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(dims, ux, memory_); -// } -// else if(!strcmp(field, "pi_ptr")) -// { -// struct blasfeo_dvec *pi = value; -// ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr(dims, pi, memory_); -// } else { printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_set\n", field); @@ -598,8 +529,11 @@ void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config config->memory_set_rq_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr; config->memory_set_BAbt_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr; config->memory_set_b_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr; + config->memory_set_idxb_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_idxb_ptr; + config->memory_set_DCt_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_DCt_ptr; config->memory_set_ux_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr; config->memory_set_pi_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr; + config->memory_set_lam_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_lam_ptr; // functions config->regularize_hessian = &ocp_nlp_reg_project_reduc_hess_regularize_hessian; config->correct_dual_sol = &ocp_nlp_reg_project_reduc_hess_correct_dual_sol; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index fde4e61853..149f233b9d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -612,6 +612,12 @@ int ocp_nlp_sqp_workspace_calculate_size(void *config_, void *dims_, void *opts_ // qp out size += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); + // qp res + size += ocp_qp_res_calculate_size(dims->qp_solver); + + // qp res ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + if (opts->reuse_workspace) { @@ -748,6 +754,14 @@ static void ocp_nlp_sqp_cast_workspace(void *config_, ocp_nlp_dims *dims, ocp_nl work->qp_out = ocp_qp_out_assign(qp_solver, dims->qp_solver, c_ptr); c_ptr += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver); + + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + if (opts->reuse_workspace) { @@ -1192,8 +1206,11 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, config->regularize->memory_set_rq_ptr(dims->regularize, work->qp_in->rqz, mem->regularize_mem); config->regularize->memory_set_BAbt_ptr(dims->regularize, work->qp_in->BAbt, mem->regularize_mem); config->regularize->memory_set_b_ptr(dims->regularize, work->qp_in->b, mem->regularize_mem); + config->regularize->memory_set_idxb_ptr(dims->regularize, work->qp_in->idxb, mem->regularize_mem); + config->regularize->memory_set_DCt_ptr(dims->regularize, work->qp_in->DCt, mem->regularize_mem); config->regularize->memory_set_ux_ptr(dims->regularize, work->qp_out->ux, mem->regularize_mem); config->regularize->memory_set_pi_ptr(dims->regularize, work->qp_out->pi, mem->regularize_mem); + config->regularize->memory_set_lam_ptr(dims->regularize, work->qp_out->lam, mem->regularize_mem); // copy sampling times into dynamics model #if defined(ACADOS_WITH_OPENMP) @@ -1283,6 +1300,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, return mem->status; } + // regularize Hessian config->regularize->regularize_hessian(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); @@ -1321,11 +1339,24 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; + // TODO add flag to decide to compute or not +// double inf_norm_qp_res[4] = {}; +// ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); +// ocp_qp_res_compute_nrm_inf(work->qp_res, inf_norm_qp_res); +// printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); +// printf("\nres g\n"); +// for(int ii=0; ii<=N; ii++) +// blasfeo_print_tran_dvec(work->qp_in->dim->nu[ii]+work->qp_in->dim->nx[ii], &work->qp_res->res_g[ii], 0); +// printf("\nlam\n"); +// for(int ii=0; ii<=N; ii++) +// blasfeo_print_tran_dvec(2*work->qp_in->dim->nb[ii]+2*work->qp_in->dim->ng[ii], &work->qp_out->lam[ii], 0); + // printf("\n------- qp_out (sqp iter %d) ---------\n", sqp_iter); // print_ocp_qp_out(work->qp_out); // if(sqp_iter==1) // exit(1); + if (qp_status != 0) { // print_ocp_qp_in(work->qp_in); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index 3e47211465..ee2de62b9a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -112,6 +112,8 @@ int ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_); // void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); + + /************************************************ * workspace ************************************************/ @@ -122,6 +124,8 @@ typedef struct ocp_qp_in *qp_in; ocp_qp_out *qp_out; void *qp_work; + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; void **dynamics; // dynamics_workspace void **cost; // cost_workspace @@ -132,6 +136,8 @@ typedef struct // int ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_); + + /************************************************ * functions ************************************************/ diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index 0a8d28e8b1..eec42919a8 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -1138,8 +1138,11 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, config->regularize->memory_set_rq_ptr(dims->regularize, work->qp_in->rqz, mem->regularize_mem); config->regularize->memory_set_BAbt_ptr(dims->regularize, work->qp_in->BAbt, mem->regularize_mem); config->regularize->memory_set_b_ptr(dims->regularize, work->qp_in->b, mem->regularize_mem); + config->regularize->memory_set_idxb_ptr(dims->regularize, work->qp_in->idxb, mem->regularize_mem); + config->regularize->memory_set_DCt_ptr(dims->regularize, work->qp_in->DCt, mem->regularize_mem); config->regularize->memory_set_ux_ptr(dims->regularize, work->qp_out->ux, mem->regularize_mem); config->regularize->memory_set_pi_ptr(dims->regularize, work->qp_out->pi, mem->regularize_mem); + config->regularize->memory_set_lam_ptr(dims->regularize, work->qp_out->lam, mem->regularize_mem); // copy sampling times into dynamics model #if defined(ACADOS_WITH_OPENMP) diff --git a/acados/ocp_qp/ocp_qp_common.c b/acados/ocp_qp/ocp_qp_common.c index 69277d6733..746b12a188 100644 --- a/acados/ocp_qp/ocp_qp_common.c +++ b/acados/ocp_qp/ocp_qp_common.c @@ -37,6 +37,8 @@ #include "acados/ocp_qp/ocp_qp_common.h" #include "acados/utils/types.h" + + /************************************************ * config ************************************************/ @@ -50,6 +52,8 @@ int ocp_qp_solver_config_calculate_size() return size; } + + qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory) { char *c_ptr = raw_memory; @@ -60,6 +64,8 @@ qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory) return config; } + + int ocp_qp_xcond_solver_config_calculate_size() { int size = 0; @@ -71,6 +77,8 @@ int ocp_qp_xcond_solver_config_calculate_size() return size; } + + ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory) { char *c_ptr = raw_memory; @@ -84,6 +92,8 @@ ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory) return config; } + + int ocp_qp_condensing_config_calculate_size() { int size = 0; @@ -93,6 +103,8 @@ int ocp_qp_condensing_config_calculate_size() return size; } + + ocp_qp_condensing_config *ocp_qp_condensing_config_assign(void *raw_memory) { char *c_ptr = raw_memory; @@ -103,6 +115,8 @@ ocp_qp_condensing_config *ocp_qp_condensing_config_assign(void *raw_memory) return config; } + + /************************************************ * dims ************************************************/ @@ -160,6 +174,8 @@ int ocp_qp_in_calculate_size(void *config, ocp_qp_dims *dims) return size; } + + ocp_qp_in *ocp_qp_in_assign(void *config, ocp_qp_dims *dims, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -196,6 +212,8 @@ ocp_qp_in *ocp_qp_in_assign(void *config, ocp_qp_dims *dims, void *raw_memory) return qp_in; } + + /************************************************ * out ************************************************/ @@ -209,6 +227,8 @@ int ocp_qp_out_calculate_size(void *config, ocp_qp_dims *dims) return size; } + + ocp_qp_out *ocp_qp_out_assign(void *config, ocp_qp_dims *dims, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -248,6 +268,8 @@ ocp_qp_out *ocp_qp_out_assign(void *config, ocp_qp_dims *dims, void *raw_memory) return qp_out; } + + /************************************************ * res ************************************************/ @@ -261,6 +283,8 @@ int ocp_qp_res_calculate_size(ocp_qp_dims *dims) return size; } + + ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -276,6 +300,8 @@ ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory) return qp_res; } + + int ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims) { int size = sizeof(ocp_qp_res_ws); @@ -283,6 +309,8 @@ int ocp_qp_res_workspace_calculate_size(ocp_qp_dims *dims) return size; } + + ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) { char *c_ptr = (char *) raw_memory; @@ -298,6 +326,8 @@ ocp_qp_res_ws *ocp_qp_res_workspace_assign(ocp_qp_dims *dims, void *raw_memory) return qp_res_ws; } + + void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res, ocp_qp_res_ws *res_ws) { @@ -314,6 +344,8 @@ void ocp_qp_res_compute(ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_qp_res *qp_res return; } + + void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]) { // loop index @@ -389,6 +421,8 @@ void ocp_qp_res_compute_nrm_inf(ocp_qp_res *qp_res, double res[4]) return; } + + void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out) { int N = in->N; @@ -420,6 +454,8 @@ void ocp_qp_stack_slacks_dims(ocp_qp_dims *in, ocp_qp_dims *out) } } + + void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) { int N = in->dim->N; @@ -579,6 +615,8 @@ void ocp_qp_stack_slacks(ocp_qp_in *in, ocp_qp_in *out) } } + + void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out) { // loop index From cdee81bfe76ba537e361b3c8614d5248c3c03458 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 6 Jun 2019 12:03:26 +0200 Subject: [PATCH 53/78] reg_proj_reduc_hess: add trh_eig to split threshold and min eigenvalue --- acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c | 12 +++++++++--- acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h | 1 + 2 files changed, 10 insertions(+), 3 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c index 9afe778e59..6869b297ef 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c @@ -55,6 +55,7 @@ void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_n { ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; + opts->thr_eig = 1e-12; opts->min_eig = 1e-4; opts->min_pivot = 1e-12; opts->pivoting = 1; @@ -69,7 +70,12 @@ void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, ocp_nlp_reg_dims *di ocp_nlp_reg_project_reduc_hess_opts *opts = opts_; - if (!strcmp(field, "min_eig")) + if (!strcmp(field, "thr_eig")) + { + double *d_ptr = value; + opts->thr_eig = *d_ptr; + } + else if (!strcmp(field, "min_eig")) { double *d_ptr = value; opts->min_eig = *d_ptr; @@ -361,7 +367,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg do_reg = 0; for(jj=0; jjd[jj]min_eig) + if(mem->d[jj]thr_eig) { mem->e[jj] = opts->min_eig - mem->d[jj]; do_reg = 1; @@ -477,7 +483,7 @@ void ocp_nlp_reg_project_reduc_hess_regularize_hessian(void *config, ocp_nlp_reg acados_eigen_decomposition(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->d, mem->e); for(jj=0; jjd[jj]min_eig) + if(mem->d[jj]thr_eig) mem->e[jj] = opts->min_eig - mem->d[jj]; else mem->e[jj] = 0.0; diff --git a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h index 54570d14f0..9c7bd866fe 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h +++ b/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -51,6 +51,7 @@ extern "C" { typedef struct { + double thr_eig; double min_eig; double min_pivot; int pivoting; From 94776288e2f2f1bcfc3c654eb51a38e2de3bd13d Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 6 Jun 2019 12:06:51 +0200 Subject: [PATCH 54/78] update hpipm (improvements in pi computat in expand sol --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index 4025a82361..327e9d620e 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 4025a82361ec19d939e7b5297ebee06b9abe0a87 +Subproject commit 327e9d620e8c7403c90ee1ad508ea294fad1ebf0 From 56ddd947e70ddeb66d408877c374a28d1cb0389b Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 6 Jun 2019 14:12:01 +0200 Subject: [PATCH 55/78] update hpipm (fix) --- external/hpipm | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/external/hpipm b/external/hpipm index 327e9d620e..c3f2610d14 160000 --- a/external/hpipm +++ b/external/hpipm @@ -1 +1 @@ -Subproject commit 327e9d620e8c7403c90ee1ad508ea294fad1ebf0 +Subproject commit c3f2610d149d08280b830ab4194cbc7364b21379 From 56652f32490f654fa909945292cfa325ea99e993 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 6 Jun 2019 15:59:13 +0200 Subject: [PATCH 56/78] convexification: fix to computation of equality constr multipliers --- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 39 ++++++++++++++++++++------ 1 file changed, 30 insertions(+), 9 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index 652c8fc0b7..0df49e3603 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -697,24 +697,45 @@ void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims // for(ii=0; ii<=N; ii++) // blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); -// blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0); + + + // compute multipliers of equality constraints + +#if 0 blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0); - // TODO use memory to avoid double constr evaluation for(ii=1; iiRSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0); blasfeo_dgemv_n(nx[ss], nu[ss]+nx[ss], 1.0, mem->RSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0); blasfeo_dgemv_n(nx[ss], nx[ss+1], 1.0, mem->BAbt[ss], nu[ss], 0, mem->pi[ss], 0, 1.0, mem->pi[ss-1], 0, mem->pi[ss-1], 0); - -// blasfeo_daxpy(nbu[ss]+nbx[ss]+ng[ss], -1.0, mem->lam[ss], 0, mem->lam[ss], nbu[ss]+nbx[ss]+ng[ss], &mem->tmp_nbgM, 0); -// blasfeo_dvecse(nu[ss]+nx[ss], 0.0, &mem->tmp_nuxM, 0); -// blasfeo_dvecad_sp(nbu[ss]+nbx[ss], -1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0); -// blasfeo_print_tran_dvec(nu[ss]+nx[ss], &mem->tmp_nuxM, 0); } +#else + + // last stage + blasfeo_dveccp(nx[N], mem->rq[N], nu[N], &mem->tmp_nuxM, nu[N]); + blasfeo_daxpy(nbu[N]+nbx[N]+ng[N], -1.0, mem->lam[N], 0, mem->lam[N], nbu[N]+nbx[N]+ng[N], &mem->tmp_nbgM, 0); + blasfeo_dvecad_sp(nbu[N]+nbx[N], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0); + // TODO avoid to multiply by R ??? + blasfeo_dsymv_l(nu[N]+nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], 0, 0, mem->ux[N], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); + blasfeo_dgemv_n(nx[N], ng[N], 1.0, mem->DCt[N], nu[N], 0, &mem->tmp_nbgM, nbu[N]+nbx[N], 1.0, &mem->tmp_nuxM, nu[N], &mem->tmp_nuxM, nu[N]); + blasfeo_dveccp(nx[N], &mem->tmp_nuxM, nu[N], mem->pi[N-1], 0); + + // middle stages + for(ii=0; iirq[N-1-ii], nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_daxpy(nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], -1.0, mem->lam[N-1-ii], 0, mem->lam[N-1-ii], nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], &mem->tmp_nbgM, 0); + blasfeo_dvecad_sp(nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N-1-ii], &mem->tmp_nuxM, 0); + // TODO avoid to multiply by R ??? + blasfeo_dsymv_l(nu[N-1-ii]+nx[N-1-ii], nu[N-1-ii]+nx[N-1-ii], 1.0, mem->RSQrq[N-1-ii], 0, 0, mem->ux[N-1-ii], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); + blasfeo_dgemv_n(nx[N-1-ii], nx[N-ii], 1.0, mem->BAbt[N-1-ii], nu[N-1-ii], 0, mem->pi[N-1-ii], 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_dveccp(nx[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii], mem->pi[N-2-ii], 0); + } +#endif + return; } From 62c499980912da061b7fd70936502487f7db9ea3 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Thu, 6 Jun 2019 16:50:34 +0200 Subject: [PATCH 57/78] adding example with giaf's API --- .../generate_c_code_explicit_setters.py | 142 ++++++++++++++++++ 1 file changed, 142 insertions(+) create mode 100644 interfaces/acados_template/examples/python/pendulum_example/generate_c_code_explicit_setters.py diff --git a/interfaces/acados_template/examples/python/pendulum_example/generate_c_code_explicit_setters.py b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code_explicit_setters.py new file mode 100644 index 0000000000..ce302217a4 --- /dev/null +++ b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code_explicit_setters.py @@ -0,0 +1,142 @@ +from acados_template import * +import acados_template as at +from export_ode_model import * +import numpy as np +import scipy.linalg +from ctypes import * + +# create render arguments +ocp = acados_ocp_nlp() + +# export model +model = export_ode_model() + +# set model_name +ocp.model_name = model.name + +Tf = 1.0 +nx = model.x.size()[0] +nu = model.u.size()[0] +ny = nx + nu +ny_e = nx +N = 100 + +# set ocp_nlp_dimensions + +ocp.set('dims_nx', nx) +ocp.set('dims_ny', ny) +ocp.set('dims_ny_e', ny_e) +ocp.set('dims_nbx', 0) +ocp.set('dims_nbu', nu) +ocp.set('dims_nu', model.u.size()[0]) +ocp.set('dims_N', N) + +# set weighting matrices +Q = np.eye(4) +Q[0,0] = 1e3 +Q[1,1] = 1e-2 +Q[2,2] = 1e3 +Q[3,3] = 1e-2 + +R = np.eye(1) +R[0,0] = 1e-2 + +ocp.set('cost_W', scipy.linalg.block_diag(Q, R)) + +Vx = np.zeros((ny, nx)) +Vx[0,0] = 1.0 +Vx[1,1] = 1.0 +Vx[2,2] = 1.0 +Vx[3,3] = 1.0 + +ocp.set('cost_Vx', Vx) + +Vu = np.zeros((ny, nu)) +Vu[4,0] = 1.0 +ocp.set('cost_Vu', Vu) + +ocp.set('cost_W_e', Q) + +Vx_e = np.zeros((ny_e, nx)) +Vx_e[0,0] = 1.0 +Vx_e[1,1] = 1.0 +Vx_e[2,2] = 1.0 +Vx_e[3,3] = 1.0 + +ocp.set('cost_Vx_e', Vx_e) + +ocp.set('cost_yref', np.zeros((ny, ))) +ocp.set('cost_yref_e', np.zeros((ny_e, ))) + +# setting bounds +Fmax = 80.0 +ocp.set('constraints_lbu', np.array([-Fmax])) +ocp.set('constraints_ubu', np.array([-Fmax])) +ocp.set('constraints_x0', np.array([0.0, 0.0, 3.14, 0.0]) +ocp.set('constraints_idxbu', np.array([0]) + +# set constants +# ocp.constants['PI'] = 3.1415926535897932 + +# set QP solver +# ocp.solver_config.qp_solver = 'PARTIAL_CONDENSING_HPIPM' +ocp.set('solver_config_qp_solver', 'FULL_CONDENSING_QPOASES') +ocp.set('solver_config_hessian_approx', 'GAUSS_NEWTON') +ocp.set('solver_config_integrator_type', 'ERK') + +# set prediction horizon +ocp.set('solver_config_tf', Tf) +ocp.set('solver_config_nlp_solver_type', 'SQP') + +# set header path +ocp.set('acados_include_path', '/usr/local/include') +ocp.set('acados_lib_path', '/usr/local/lib') + +# json_layout = acados_ocp2json_layout(ocp) +# with open('acados_layout.json', 'w') as f: +# json.dump(json_layout, f, default=np_array_to_list) +# exit() + +acados_solver = generate_solver(model, ocp, json_file = 'acados_ocp.json') + +Nsim = 100 + +simX = np.ndarray((Nsim, nx)) +simU = np.ndarray((Nsim, nu)) + +for i in range(Nsim): + status = acados_solver.solve() + + # get solution + x0 = acados_solver.get(0, "x") + u0 = acados_solver.get(0, "u") + + for j in range(nx): + simX[i,j] = x0[j] + + for j in range(nu): + simU[i,j] = u0[j] + + # update initial condition + x0 = acados_solver.get(1, "x") + + acados_solver.set(0, "lbx", x0) + acados_solver.set(0, "ubx", x0) + +# plot results +import matplotlib +import matplotlib.pyplot as plt +t = np.linspace(0.0, Tf/N, Nsim) +plt.subplot(2, 1, 1) +plt.step(t, simU, 'r') +plt.title('closed-loop simulation') +plt.ylabel('u') +plt.xlabel('t') +plt.grid(True) +plt.subplot(2, 1, 2) +plt.plot(t, simX[:,2]) +plt.ylabel('theta') +plt.xlabel('t') +plt.grid(True) +plt.show() + From cd815177b7a46754db3aae676a1cb3e559097a24 Mon Sep 17 00:00:00 2001 From: giaf Date: Thu, 6 Jun 2019 22:17:32 +0200 Subject: [PATCH 58/78] convexification: correctly compute all dual solution --- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 24 +++++++++++++++++++----- acados/ocp_nlp/ocp_nlp_reg_convexify.h | 1 + 2 files changed, 20 insertions(+), 5 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index 0df49e3603..80cffdbde0 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -138,7 +138,7 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims size += 2*nuxM*sizeof(double); // d e size += nuxM*nuxM*sizeof(double); // reg_hess - size += (N+1)*sizeof(struct blasfeo_dmat); // original_RSQrq + size += 2*(N+1)*sizeof(struct blasfeo_dmat); // original_RSQrq tmp_RSQrq size += 1 * 64; @@ -152,7 +152,7 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims for (ii=0; ii<=N; ii++) { - size += blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq + size += 2*blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq tmp_RSQrq } // size += 2*blasfeo_memsize_dvec(nxM); // grad b2 @@ -227,6 +227,9 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, mem->original_RSQrq = (struct blasfeo_dmat *) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dmat); + mem->tmp_RSQrq = (struct blasfeo_dmat *) c_ptr; + c_ptr += (N+1)*sizeof(struct blasfeo_dmat); + mem->RSQrq = (struct blasfeo_dmat **) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); @@ -264,9 +267,10 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, assign_and_advance_blasfeo_dmat_mem(nxM, nuM, &mem->St_copy, &c_ptr); for (ii=0; ii<=N; ii++) - { assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], &c_ptr); - } + + for (ii=0; ii<=N; ii++) + assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->tmp_RSQrq[ii], &c_ptr); assign_and_advance_blasfeo_dvec_mem(nuxM, &mem->tmp_nuxM, &c_ptr); assign_and_advance_blasfeo_dvec_mem(nbgM, &mem->tmp_nbgM, &c_ptr); @@ -557,6 +561,7 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di double delta = opts->delta; // Algorithm 6 from Verschueren2017 + blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0); blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0); @@ -610,10 +615,19 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di if (needs_regularization) { + blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQrq[ii], 0, 0); // TODO project only nu instead ??????????? + // TODO compute correction as a separate matrix, and apply to original_RSQrq too (TODO change this name then) blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); - acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); +// acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); + acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0); +// blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]); +// acados_mirror(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); +// acados_project(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); +// blasfeo_pack_dmat(nu[ii], nu[ii], mem->reg_hess, nu[ii], mem->RSQrq[ii], 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQrq[ii], 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); } // printf("QSR_hat\n"); diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/acados/ocp_nlp/ocp_nlp_reg_convexify.h index 8307666bcb..24ddf7c634 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -86,6 +86,7 @@ typedef struct { struct blasfeo_dmat St_copy; struct blasfeo_dmat *original_RSQrq; + struct blasfeo_dmat *tmp_RSQrq; struct blasfeo_dvec tmp_nuxM; struct blasfeo_dvec tmp_nbgM; From 8d745b45477ae8aefe871698655f6108899e2274 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 09:57:49 +0200 Subject: [PATCH 59/78] S-Function: adding support for time-varying y_ref --- .../c_templates/acados_solver_sfun.in.c | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/interfaces/acados_template/acados_template/c_templates/acados_solver_sfun.in.c b/interfaces/acados_template/acados_template/c_templates/acados_solver_sfun.in.c index c5082a3a82..2786998a20 100644 --- a/interfaces/acados_template/acados_template/c_templates/acados_solver_sfun.in.c +++ b/interfaces/acados_template/acados_template/c_templates/acados_solver_sfun.in.c @@ -89,7 +89,7 @@ static void mdlInitializeSizes (SimStruct *S) // specify dimension information for the input ports ssSetInputPortVectorDimension(S, 0, {{ ocp.dims.nx }}); - ssSetInputPortVectorDimension(S, 1, {{ ocp.dims.ny }}); + ssSetInputPortVectorDimension(S, 1, {{ ocp.dims.ny * ocp.dims.N }}); ssSetInputPortVectorDimension(S, 2, {{ ocp.dims.ny_e }}); {% if ocp.dims.np > 0 %} ssSetInputPortVectorDimension(S, 3, {{ ocp.dims.np }}); @@ -97,10 +97,10 @@ static void mdlInitializeSizes (SimStruct *S) // specify dimension information for the output ports ssSetOutputPortVectorDimension(S, 0, {{ ocp.dims.nu }} ); // optimal input - ssSetOutputPortVectorDimension(S, 1, 1 ); // solver status - ssSetOutputPortVectorDimension(S, 2, 1 ); // KKT residuals + ssSetOutputPortVectorDimension(S, 1, 1 ); // solver status + ssSetOutputPortVectorDimension(S, 2, 1 ); // KKT residuals ssSetOutputPortVectorDimension(S, 3, {{ ocp.dims.nx }} ); // first state - ssSetOutputPortVectorDimension(S, 4, 1); // computation times + ssSetOutputPortVectorDimension(S, 4, 1); // computation times // specify the direct feedthrough status ssSetInputPortDirectFeedThrough(S, 0, 1); // current state x0 @@ -159,7 +159,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // local buffers real_t in_x0[{{ ocp.dims.nx }}]; - real_t in_y_ref[{{ ocp.dims.ny }}]; + real_t in_y_ref[{{ ocp.dims.ny * ocp.dims.N}}]; real_t in_y_ref_e[{{ ocp.dims.ny_e }}]; {% if ocp.dims.np > 0 %} real_t in_p[{{ ocp.dims.np }}]; @@ -174,7 +174,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) // copy signals into local buffers for (int i = 0; i < {{ ocp.dims.nx }}; i++) in_x0[i] = (double)(*in_x0_sign[i]); - for (int i = 0; i < {{ ocp.dims.ny }}; i++) in_y_ref[i] = (double)(*in_y_ref_sign[i]); + for (int i = 0; i < {{ ocp.dims.ny * ocp.dims.N }}; i++) in_y_ref[i] = (double)(*in_y_ref_sign[i]); for (int i = 0; i < {{ ocp.dims.ny_e }}; i++) in_y_ref_e[i] = (double)(*in_y_ref_e_sign[i]); {% if ocp.dims.np > 0 %} for (int i = 0; i < {{ ocp.dims.np }}; i++) in_p[i] = (double)(*in_p_sign[i]); @@ -188,10 +188,12 @@ static void mdlOutputs(SimStruct *S, int_T tid) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", in_x0); // update reference - for (int ii = 0; ii < {{ocp.dims.N}}; ii++) - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) in_y_ref); + for (int ii = 0; ii < {{ ocp.dims.N }}; ii++) { + ocp_nlp_cost_model_set(nlp_config, nlp_dims, + nlp_in, ii, "yref", (void *) (in_y_ref + ii*{{ ocp.dims.ny }}); - ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ocp.dims.N}}, "yref", (void *) in_y_ref_e); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, + {{ ocp.dims.N }}, "yref", (void *) in_y_ref_e); // update value of parameters {% if ocp.dims.np > 0%} From 239a9b825680e370acb8af20c06dbd05e514ef9f Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 11:34:49 +0200 Subject: [PATCH 60/78] fix to ocp_nlp_reg_common --- acados/ocp_nlp/ocp_nlp_reg_common.c | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/acados/ocp_nlp/ocp_nlp_reg_common.c b/acados/ocp_nlp/ocp_nlp_reg_common.c index 3af5381e91..478fca95e3 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_common.c +++ b/acados/ocp_nlp/ocp_nlp_reg_common.c @@ -76,6 +76,7 @@ ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory) c_ptr += (N+1)*sizeof(int); // nbu dims->nbu = (int *) c_ptr; + c_ptr += (N+1)*sizeof(int); // nbx dims->nbx = (int *) c_ptr; c_ptr += (N+1)*sizeof(int); @@ -85,6 +86,24 @@ ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory) dims->N = N; + // initialize to zero by default + int ii; + // nx + for(ii=0; ii<=N; ii++) + dims->nx[ii] = 0; + // nu + for(ii=0; ii<=N; ii++) + dims->nu[ii] = 0; + // nbx + for(ii=0; ii<=N; ii++) + dims->nbx[ii] = 0; + // nbu + for(ii=0; ii<=N; ii++) + dims->nbu[ii] = 0; + // ng + for(ii=0; ii<=N; ii++) + dims->ng[ii] = 0; + assert((char *) raw_memory + ocp_nlp_reg_dims_calculate_size(N) >= c_ptr); return dims; From 551eaaf3fc5fcfbb9264bb952e195f38b5351c4e Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 11:35:48 +0200 Subject: [PATCH 61/78] reg_convex: use just one matrix for tmp_RSQ --- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 17 +++++++---------- acados/ocp_nlp/ocp_nlp_reg_convexify.h | 2 +- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index 80cffdbde0..7924d03dd9 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -138,7 +138,7 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims size += 2*nuxM*sizeof(double); // d e size += nuxM*nuxM*sizeof(double); // reg_hess - size += 2*(N+1)*sizeof(struct blasfeo_dmat); // original_RSQrq tmp_RSQrq + size += (N+1)*sizeof(struct blasfeo_dmat); // original_RSQrq size += 1 * 64; @@ -152,8 +152,9 @@ int ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims for (ii=0; ii<=N; ii++) { - size += 2*blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq tmp_RSQrq + size += 2*blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq } + size += blasfeo_memsize_dmat(nuxM, nuxM); // tmp_RSQ // size += 2*blasfeo_memsize_dvec(nxM); // grad b2 @@ -227,9 +228,6 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, mem->original_RSQrq = (struct blasfeo_dmat *) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dmat); - mem->tmp_RSQrq = (struct blasfeo_dmat *) c_ptr; - c_ptr += (N+1)*sizeof(struct blasfeo_dmat); - mem->RSQrq = (struct blasfeo_dmat **) c_ptr; c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); @@ -269,8 +267,7 @@ void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, for (ii=0; ii<=N; ii++) assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], &c_ptr); - for (ii=0; ii<=N; ii++) - assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->tmp_RSQrq[ii], &c_ptr); + assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->tmp_RSQ, &c_ptr); assign_and_advance_blasfeo_dvec_mem(nuxM, &mem->tmp_nuxM, &c_ptr); assign_and_advance_blasfeo_dvec_mem(nbgM, &mem->tmp_nbgM, &c_ptr); @@ -615,7 +612,7 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di if (needs_regularization) { - blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQrq[ii], 0, 0); + blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); // TODO project only nu instead ??????????? // TODO compute correction as a separate matrix, and apply to original_RSQrq too (TODO change this name then) blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]); @@ -626,8 +623,8 @@ void ocp_nlp_reg_convexify_regularize_hessian(void *config, ocp_nlp_reg_dims *di // acados_mirror(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); // acados_project(nu[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4); // blasfeo_pack_dmat(nu[ii], nu[ii], mem->reg_hess, nu[ii], mem->RSQrq[ii], 0, 0); - blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQrq[ii], 0, 0); - blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0); + blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0); } // printf("QSR_hat\n"); diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/acados/ocp_nlp/ocp_nlp_reg_convexify.h index 24ddf7c634..c79dcc9290 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.h +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -86,7 +86,7 @@ typedef struct { struct blasfeo_dmat St_copy; struct blasfeo_dmat *original_RSQrq; - struct blasfeo_dmat *tmp_RSQrq; + struct blasfeo_dmat tmp_RSQ; struct blasfeo_dvec tmp_nuxM; struct blasfeo_dvec tmp_nbgM; From 64a5c81b001dab6674c7bada004c4d44095f186f Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 11:37:30 +0200 Subject: [PATCH 62/78] get_dims->dims_get, adding dimension getters for nbx and nbu in constratins bgh --- acados/ocp_nlp/ocp_nlp_common.c | 8 +++--- acados/ocp_nlp/ocp_nlp_constraints_bgh.c | 26 ++++++++++++++++++- acados/ocp_nlp/ocp_nlp_constraints_bghp.c | 2 +- acados/ocp_nlp/ocp_nlp_constraints_common.h | 2 +- interfaces/acados_c/ocp_nlp_interface.c | 16 +++++++++++- .../acados_template/generate_solver.py | 14 +++++++--- 6 files changed, 57 insertions(+), 11 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 8e480519ba..c8543141b1 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -535,7 +535,7 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c config->constraints[i]->dims_set(config->constraints[i], dims->constraints[i], field, int_value); // update ni in ocp_nlp dimensions - config->constraints[i]->get_dims(config->constraints[i], dims->constraints[i], + config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "ni", &dims->ni[i]); // update qp_solver dims @@ -556,7 +556,7 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c // { // // update nb in qp_solver // int nb; - // config->constraints[i]->get_dims(config->constraints[i], + // config->constraints[i]->dims_get(config->constraints[i], // dims->constraints[i], "nb", &nb); // config->qp_solver->dims_set(config->qp_solver, dims->qp_solver, i, "nb", &nb); // } @@ -565,9 +565,9 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c { // update ng_qp_solver in qp_solver int ng, nh, ng_qp_solver; - config->constraints[i]->get_dims(config->constraints[i], + config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "ng", &ng); - config->constraints[i]->get_dims(config->constraints[i], + config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "nh", &nh); ng_qp_solver = ng + nh; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c index 4672169733..fcec7752f7 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bgh.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bgh.c @@ -252,6 +252,22 @@ static void ocp_nlp_constraints_bgh_get_nb(void *config_, void *dims_, int* valu +static void ocp_nlp_constraints_bgh_get_nbx(void *config_, void *dims_, int* value) +{ + ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + *value = dims->nbx; +} + + + +static void ocp_nlp_constraints_bgh_get_nbu(void *config_, void *dims_, int* value) +{ + ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + *value = dims->nbu; +} + + + static void ocp_nlp_constraints_bgh_get_ng(void *config_, void *dims_, int* value) { ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; @@ -286,6 +302,14 @@ void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *fi { ocp_nlp_constraints_bgh_get_nb(config_, dims_, value); } + else if (!strcmp(field, "nbx")) + { + ocp_nlp_constraints_bgh_get_nbx(config_, dims_, value); + } + else if (!strcmp(field, "nbu")) + { + ocp_nlp_constraints_bgh_get_nbu(config_, dims_, value); + } else if (!strcmp(field, "ng")) { ocp_nlp_constraints_bgh_get_ng(config_, dims_, value); @@ -1006,7 +1030,7 @@ void ocp_nlp_constraints_bgh_config_initialize_default(void *config_) config->dims_assign = &ocp_nlp_constraints_bgh_dims_assign; config->dims_initialize = &ocp_nlp_constraints_bgh_dims_initialize; config->dims_set = &ocp_nlp_constraints_bgh_dims_set; - config->get_dims = &ocp_nlp_constraints_bgh_dims_get; + config->dims_get = &ocp_nlp_constraints_bgh_dims_get; config->model_calculate_size = &ocp_nlp_constraints_bgh_model_calculate_size; config->model_assign = &ocp_nlp_constraints_bgh_model_assign; config->model_set = &ocp_nlp_constraints_bgh_model_set; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c index b40018250c..e123d572eb 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c @@ -1022,7 +1022,7 @@ void ocp_nlp_constraints_bghp_config_initialize_default(void *config_) config->dims_assign = &ocp_nlp_constraints_bghp_dims_assign; config->dims_initialize = &ocp_nlp_constraints_bghp_dims_initialize; config->dims_set = &ocp_nlp_constraints_bghp_dims_set; - config->get_dims = &ocp_nlp_constraints_bghp_dims_get; + config->dims_get = &ocp_nlp_constraints_bghp_dims_get; config->model_calculate_size = &ocp_nlp_constraints_bghp_model_calculate_size; config->model_assign = &ocp_nlp_constraints_bghp_model_assign; config->model_set = &ocp_nlp_constraints_bghp_model_set; diff --git a/acados/ocp_nlp/ocp_nlp_constraints_common.h b/acados/ocp_nlp/ocp_nlp_constraints_common.h index 0ed91efdf7..969b2e3e98 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_common.h +++ b/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -72,7 +72,7 @@ typedef struct void (*config_initialize_default)(void *config); // dimension setters void (*dims_set)(void *config_, void *dims_, const char *field, const int *value); - void (*get_dims)(void *config_, void *dims_, const char *field, int* value); + void (*dims_get)(void *config_, void *dims_, const char *field, int* value); } ocp_nlp_constraints_config; // diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 6956a779e6..2e11ace125 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -501,6 +501,8 @@ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field) { + int dims_value = -1; + if (!strcmp(field, "x")) { return dims->nx[stage]; @@ -509,9 +511,21 @@ int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou { return dims->nx[stage]; } + else if (!strcmp(field, "lbx") || !strcmp(field, "ubx")) + { + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "nbx", &dims_value); + return dims_value; + } + else if (!strcmp(field, "lbu") || !strcmp(field, "ubu")) + { + config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], + "nbu", &dims_value); + return dims_value; + } else { - printf("\nerror: ocp_nlp_out_set: field %s not available\n", field); + printf("\nerror: ocp_nlp_dims_get: field %s not available\n", field); exit(1); } } diff --git a/interfaces/acados_template/acados_template/generate_solver.py b/interfaces/acados_template/acados_template/generate_solver.py index 779855d429..d0cc87a3a7 100644 --- a/interfaces/acados_template/acados_template/generate_solver.py +++ b/interfaces/acados_template/acados_template/generate_solver.py @@ -286,7 +286,7 @@ def solve(self): status = self.shared_lib.acados_solve() return status - def get(self, stage, field_): + def get(self, stage_, field_): field = field_ field = field.encode('utf-8') @@ -294,13 +294,13 @@ def get(self, stage, field_): self.shared_lib.ocp_nlp_dims_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] self.shared_lib.ocp_nlp_dims_get.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field) + dims = self.shared_lib.ocp_nlp_dims_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) self.shared_lib.ocp_nlp_out_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_out_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, out_data); + self.shared_lib.ocp_nlp_out_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field, out_data); # out = cast((out), POINTER(c_double)) @@ -311,6 +311,14 @@ def set(self, stage_, field_, value_): field = field_ field = field.encode('utf-8') + self.shared_lib.ocp_nlp_dims_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) + + if value_.shape[0] != dims: + raise Exception('acados_solver.set(): mismatching dimension for field "{}" with dimension {} (you have {})'.format(field_,dims, value_.shape[0])) + value_data = cast(value_.ctypes.data, POINTER(c_double)) value_data_p = cast((value_data), c_void_p) From f4fba9e39ff0b31a4a1fc09a1217e8d987b31d4f Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 11:38:00 +0200 Subject: [PATCH 63/78] add option to compute qp residuals in sqp loop (external qp res, after (part) cond & reg) --- acados/ocp_nlp/ocp_nlp_sqp.c | 61 ++++++++++++------- acados/ocp_nlp/ocp_nlp_sqp.h | 3 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 47 +++++++++++++- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 3 + .../linear_mass_spring_model/example_ocp.m | 31 ++++++++++ .../masses_chain_model/example_ocp.m | 26 ++++++-- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 26 ++++++-- interfaces/acados_matlab/acados_ocp_opts.m | 2 + interfaces/acados_matlab/ocp_create.c | 12 ++++ 9 files changed, 178 insertions(+), 33 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 149f233b9d..d8cf28dc16 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -184,6 +184,8 @@ void ocp_nlp_sqp_opts_initialize_default(void *config_, void *dims_, void *opts_ opts->num_threads = ACADOS_NUM_THREADS; #endif + opts->ext_qp_res = 0; + opts->qp_warm_start = 0; // submodules opts @@ -354,6 +356,11 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v // for (ii=0; ii<=N; ii++) // config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); } + else if (!strcmp(field, "ext_qp_res")) + { + int* ext_qp_res = (int *) value; + opts->ext_qp_res = *ext_qp_res; + } else { printf("\nerror: ocp_nlp_sqp_opts_set: wrong field: %s\n", field); @@ -466,13 +473,17 @@ int ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void *opts_) } // nlp res - size += ocp_nlp_res_calculate_size(dims); + size += ocp_nlp_res_calculate_size(dims); // nlp mem size += ocp_nlp_memory_calculate_size(config, dims); // stat - size += 5*(opts->max_iter+1)*sizeof(double); + int stat_m = opts->max_iter+1; + int stat_n = 5; + if(opts->ext_qp_res) + stat_n += 4; + size += stat_n*stat_m*sizeof(double); size += 8; // initial align @@ -559,6 +570,8 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *r mem->stat = (double *) c_ptr; mem->stat_m = opts->max_iter+1; mem->stat_n = 5; + if(opts->ext_qp_res) + mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); mem->status = ACADOS_READY; @@ -612,11 +625,14 @@ int ocp_nlp_sqp_workspace_calculate_size(void *config_, void *dims_, void *opts_ // qp out size += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); - // qp res - size += ocp_qp_res_calculate_size(dims->qp_solver); + if(opts->ext_qp_res) + { + // qp res + size += ocp_qp_res_calculate_size(dims->qp_solver); - // qp res ws - size += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + // qp res ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + } if (opts->reuse_workspace) { @@ -754,13 +770,16 @@ static void ocp_nlp_sqp_cast_workspace(void *config_, ocp_nlp_dims *dims, ocp_nl work->qp_out = ocp_qp_out_assign(qp_solver, dims->qp_solver, c_ptr); c_ptr += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); - // qp res - work->qp_res = ocp_qp_res_assign(dims->qp_solver, c_ptr); - c_ptr += ocp_qp_res_calculate_size(dims->qp_solver); + if(opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver); - // qp res ws - work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver, c_ptr); - c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + } if (opts->reuse_workspace) { @@ -1339,17 +1358,13 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; - // TODO add flag to decide to compute or not -// double inf_norm_qp_res[4] = {}; -// ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); -// ocp_qp_res_compute_nrm_inf(work->qp_res, inf_norm_qp_res); -// printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); -// printf("\nres g\n"); -// for(int ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(work->qp_in->dim->nu[ii]+work->qp_in->dim->nx[ii], &work->qp_res->res_g[ii], 0); -// printf("\nlam\n"); -// for(int ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(2*work->qp_in->dim->nb[ii]+2*work->qp_in->dim->ng[ii], &work->qp_out->lam[ii], 0); + // compute external QP residuals (for debugging) + if(opts->ext_qp_res) + { + ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); + ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+5)); +// printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); + } // printf("\n------- qp_out (sqp iter %d) ---------\n", sqp_iter); // print_ocp_qp_out(work->qp_out); diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index ee2de62b9a..94b86b6ea5 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -57,6 +57,7 @@ typedef struct int max_iter; int reuse_workspace; int num_threads; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) int qp_warm_start; } ocp_nlp_sqp_opts; @@ -73,6 +74,8 @@ void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* v // void ocp_nlp_sqp_dyanimcs_opts_set(void *config, void *opts, int stage, const char *field, void *value); + + /************************************************ * memory ************************************************/ diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index eec42919a8..bd12512066 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -179,6 +179,8 @@ void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *o opts->num_threads = ACADOS_NUM_THREADS; #endif + opts->ext_qp_res = 0; + // submodules opts // do not compute adjoint in dynamics and constraints @@ -309,6 +311,11 @@ void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, voi // for (ii=0; ii<=N; ii++) // config->constraints[ii]->opts_set(config->constraints[ii], opts->constraints[ii], "compute_hess", value); } + else if (!strcmp(field, "ext_qp_res")) + { + int* ext_qp_res = (int *) value; + opts->ext_qp_res = *ext_qp_res; + } else { printf("\nerror: ocp_nlp_sqp_rti_opts_set: wrong field: %s\n", field); @@ -422,7 +429,11 @@ int ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts size += ocp_nlp_memory_calculate_size(config, dims); // stat - size += 1*2*sizeof(double); + int stat_m = 1+1; + int stat_n = 1; + if(opts->ext_qp_res) + stat_n += 4; + size += stat_n*stat_m*sizeof(double); size += 8; // initial align @@ -503,8 +514,10 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, voi // stat mem->stat = (double *) c_ptr; - mem->stat_m = 2; + mem->stat_m = 1+1; mem->stat_n = 1; + if(opts->ext_qp_res) + mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); mem->status = ACADOS_READY; @@ -558,6 +571,15 @@ int ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *o // qp out size += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); + if(opts->ext_qp_res) + { + // qp res + size += ocp_qp_res_calculate_size(dims->qp_solver); + + // qp res ws + size += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + } + if (opts->reuse_workspace) { @@ -695,6 +717,17 @@ static void ocp_nlp_sqp_rti_cast_workspace(void *config_, ocp_nlp_dims *dims, work->qp_out = ocp_qp_out_assign(qp_solver, dims->qp_solver, c_ptr); c_ptr += ocp_qp_out_calculate_size(qp_solver, dims->qp_solver); + if(opts->ext_qp_res) + { + // qp res + work->qp_res = ocp_qp_res_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_calculate_size(dims->qp_solver); + + // qp res ws + work->qp_res_ws = ocp_qp_res_workspace_assign(dims->qp_solver, c_ptr); + c_ptr += ocp_qp_res_workspace_calculate_size(dims->qp_solver); + } + if (opts->reuse_workspace) { @@ -1207,6 +1240,14 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; + // compute external QP residuals (for debugging) + if(opts->ext_qp_res) + { + ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); + ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+1)); +// printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); + } + // printf("\n------- qp_out (sqp iter %d) ---------\n", sqp_iter); // print_ocp_qp_out(work->qp_out); // if(sqp_iter==1) @@ -1259,6 +1300,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } + int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_) { @@ -1303,6 +1345,7 @@ int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_, void *nlp_in_, void * } + void ocp_nlp_sqp_rti_get(void *config_, void *mem_, const char *field, void *return_value_) { // ocp_nlp_config *config = config_; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 16cfe2488f..22d8644e1a 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -52,6 +52,7 @@ typedef struct int compute_dual_sol; int reuse_workspace; int num_threads; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) int qp_warm_start; } ocp_nlp_sqp_rti_opts; @@ -114,6 +115,8 @@ typedef struct ocp_qp_in *qp_in; ocp_qp_out *qp_out; void *qp_work; + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; void **dynamics; // dynamics_workspace void **cost; // cost_workspace diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index 36ab777621..7eae93d22c 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -291,6 +291,37 @@ fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +stat = ocp.get('stat'); +if (strcmp(nlp_solver, 'sqp')) + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); + if size(stat,2)>6 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); + for ii=1:size(stat,1) + fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + if size(stat,2)>6 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + end + fprintf('\n'); + end + fprintf('\n'); +else % sqp_rti + fprintf('\niter\tqp_iter'); + if size(stat,2)>2 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); + for ii=1:size(stat,1) + fprintf('%d\t%d', stat(ii,1), stat(ii,2)); + if size(stat,2)>2 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); + end + fprintf('\n'); +end + if status==0 diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 0fe12eff14..4391938eab 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -29,6 +29,7 @@ %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; +nlp_solver_ext_qp_res = 1; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_cond_N = 5; @@ -181,6 +182,7 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); +ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); if (strcmp(nlp_solver, 'sqp')) ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); end @@ -254,15 +256,31 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); + if size(stat,2)>6 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + if size(stat,2)>6 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + end + fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter\n'); + fprintf('\niter\tqp_iter'); + if size(stat,2)>2 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + fprintf('%d\t%d', stat(ii,1), stat(ii,2)); + if size(stat,2)>2 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); end fprintf('\n'); end diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 3354789f40..1779d5e9ff 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -28,6 +28,7 @@ %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; +nlp_solver_ext_qp_res = 1; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_cond_N = 5; @@ -230,6 +231,7 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); +ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); if (strcmp(nlp_solver, 'sqp')) ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); end @@ -308,15 +310,31 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); + if size(stat,2)>6 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + if size(stat,2)>6 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + end + fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter\n'); + fprintf('\niter\tqp_iter'); + if size(stat,2)>2 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + fprintf('%d\t%d', stat(ii,1), stat(ii,2)); + if size(stat,2)>2 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); end fprintf('\n'); end diff --git a/interfaces/acados_matlab/acados_ocp_opts.m b/interfaces/acados_matlab/acados_ocp_opts.m index 1dc3ccfb65..753dfefea6 100644 --- a/interfaces/acados_matlab/acados_ocp_opts.m +++ b/interfaces/acados_matlab/acados_ocp_opts.m @@ -50,6 +50,8 @@ obj.opts_struct.nlp_solver_tol_ineq = value; elseif (strcmp(field, 'nlp_solver_tol_comp')) obj.opts_struct.nlp_solver_tol_comp = value; + elseif (strcmp(field, 'nlp_solver_ext_qp_res')) + obj.opts_struct.nlp_solver_ext_qp_res = value; elseif (strcmp(field, 'qp_solver')) obj.opts_struct.qp_solver = value; elseif (strcmp(field, 'qp_solver_cond_N')) diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 18e83953d4..3e561efd86 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -623,6 +623,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double nlp_solver_tol_eq; bool set_nlp_solver_tol_eq = false; double nlp_solver_tol_ineq; bool set_nlp_solver_tol_ineq = false; double nlp_solver_tol_comp; bool set_nlp_solver_tol_comp = false; + int nlp_solver_ext_qp_res; bool set_nlp_solver_ext_qp_res = false; char *qp_solver; int qp_solver_cond_N; bool set_qp_solver_cond_N = false; int qp_solver_cond_ric_alg; bool set_qp_solver_cond_ric_alg = false; @@ -681,6 +682,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) set_nlp_solver_tol_comp = true; nlp_solver_tol_comp = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_tol_comp" ) ); } + // nlp solver ext qp res + if(mxGetField( prhs[1], 0, "nlp_solver_ext_qp_res" )!=NULL) + { + set_nlp_solver_ext_qp_res = true; + nlp_solver_ext_qp_res = mxGetScalar( mxGetField( prhs[1], 0, "nlp_solver_ext_qp_res" ) ); + } // qp_solver // TODO check qp_solver = mxArrayToString( mxGetField( prhs[1], 0, "qp_solver" ) ); @@ -1116,6 +1123,11 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { ocp_nlp_opts_set(config, opts, "tol_comp", &nlp_solver_tol_comp); } + // nlp_solver_max_iter + if(set_nlp_solver_ext_qp_res) + { + ocp_nlp_opts_set(config, opts, "ext_qp_res", &nlp_solver_ext_qp_res); + } // qp_solver_cond_N if(set_qp_solver_cond_N) { From 0c91e8cbda0af61cc9c6c0f6390fb1fb87c4b657 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 11:38:35 +0200 Subject: [PATCH 64/78] adding dimension getters for nbx and nbu in constratins bghp --- acados/ocp_nlp/ocp_nlp_constraints_bghp.c | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c index e123d572eb..b248ca22b7 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c @@ -266,6 +266,22 @@ static void ocp_nlp_constraints_bghp_get_nb(void *config_, void *dims_, int* val +static void ocp_nlp_constraints_bgh_get_nbx(void *config_, void *dims_, int* value) +{ + ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + *value = dims->nbx; +} + + + +static void ocp_nlp_constraints_bgh_get_nbu(void *config_, void *dims_, int* value) +{ + ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + *value = dims->nbu; +} + + + static void ocp_nlp_constraints_bghp_get_ng(void *config_, void *dims_, int* value) { ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_; From f9a2c90797cbbc109afaf7ce3a2402c1179fc6b7 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 11:46:51 +0200 Subject: [PATCH 65/78] fix bug in bhgp dims getter --- acados/ocp_nlp/ocp_nlp_constraints_bghp.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c index b248ca22b7..cb8c97a7fd 100644 --- a/acados/ocp_nlp/ocp_nlp_constraints_bghp.c +++ b/acados/ocp_nlp/ocp_nlp_constraints_bghp.c @@ -266,17 +266,17 @@ static void ocp_nlp_constraints_bghp_get_nb(void *config_, void *dims_, int* val -static void ocp_nlp_constraints_bgh_get_nbx(void *config_, void *dims_, int* value) +static void ocp_nlp_constraints_bghp_get_nbx(void *config_, void *dims_, int* value) { - ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_; *value = dims->nbx; } -static void ocp_nlp_constraints_bgh_get_nbu(void *config_, void *dims_, int* value) +static void ocp_nlp_constraints_bghp_get_nbu(void *config_, void *dims_, int* value) { - ocp_nlp_constraints_bgh_dims *dims = (ocp_nlp_constraints_bgh_dims *) dims_; + ocp_nlp_constraints_bghp_dims *dims = (ocp_nlp_constraints_bghp_dims *) dims_; *value = dims->nbu; } From c662369c9f502eaac1f40ea79310971590d46bcd Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 12:16:40 +0200 Subject: [PATCH 66/78] adding dims getter for cost, adding setter for y_ref to Python acados_solver --- acados/ocp_nlp/ocp_nlp_cost_common.h | 1 + acados/ocp_nlp/ocp_nlp_cost_ls.c | 22 ++++++++++++++++ acados/ocp_nlp/ocp_nlp_cost_ls.h | 2 ++ acados/ocp_nlp/ocp_nlp_cost_nls.c | 25 +++++++++++++++++++ acados/ocp_nlp/ocp_nlp_cost_nls.h | 2 ++ interfaces/acados_c/ocp_nlp_interface.c | 9 +++++++ .../acados_template/generate_solver.py | 14 +++++++++-- .../pendulum_example/generate_c_code.py | 9 +++---- 8 files changed, 77 insertions(+), 7 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_cost_common.h b/acados/ocp_nlp/ocp_nlp_cost_common.h index 5f27df8352..6b03ba7008 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_common.h +++ b/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -49,6 +49,7 @@ typedef struct void *(*dims_assign)(void *config, void *raw_memory); void (*dims_initialize)(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); void (*dims_set)(void *config_, void *dims_, const char *field, int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int *value); int (*model_calculate_size)(void *config, void *dims); void *(*model_assign)(void *config, void *dims, void *raw_memory); int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_); diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index a9f9d9adeb..1472087ab9 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -179,6 +179,27 @@ void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int +/* dimension getters */ +static void ocp_nlp_cost_ls_get_ny(void *config_, void *dims_, int* value) +{ + ocp_nlp_cost_ls_dims *dims = (ocp_nlp_cost_ls_dims *) dims_; + *value = dims->ny; +} + + + +void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value) +{ + if (!strcmp(field, "ny")) + { + ocp_nlp_cost_ls_get_ny(config_, dims_, value); + } + else + { + printf("error: attempt to get dimension from constraint model, that is not there"); + exit(1); + } +} //////////////////////////////////////////////////////////////////////////////// // model // //////////////////////////////////////////////////////////////////////////////// @@ -772,6 +793,7 @@ void ocp_nlp_cost_ls_config_initialize_default(void *config_) config->dims_assign = &ocp_nlp_cost_ls_dims_assign; config->dims_initialize = &ocp_nlp_cost_ls_dims_initialize; config->dims_set = &ocp_nlp_cost_ls_dims_set; + config->dims_get = &ocp_nlp_cost_ls_dims_get; config->model_calculate_size = &ocp_nlp_cost_ls_model_calculate_size; config->model_assign = &ocp_nlp_cost_ls_model_assign; config->model_set = &ocp_nlp_cost_ls_model_set; diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.h b/acados/ocp_nlp/ocp_nlp_cost_ls.h index 4be565aa0c..3f3a45f1e8 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -92,6 +92,8 @@ void ocp_nlp_cost_ls_dims_initialize(void *config, void *dims, int nx, // void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value); //////////////////////////////////////////////////////////////////////////////// diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index a8245fcb4a..be591b927c 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -139,6 +139,30 @@ void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, in +/* dimension getters */ +static void ocp_nlp_cost_nls_get_ny(void *config_, void *dims_, int* value) +{ + ocp_nlp_cost_nls_dims *dims = (ocp_nlp_cost_nls_dims *) dims_; + *value = dims->ny; +} + + + +void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value) +{ + if (!strcmp(field, "ny")) + { + ocp_nlp_cost_nls_get_ny(config_, dims_, value); + } + else + { + printf("error: attempt to get dimension from constraint model, that is not there"); + exit(1); + } +} + + + /************************************************ * model ************************************************/ @@ -723,6 +747,7 @@ void ocp_nlp_cost_nls_config_initialize_default(void *config_) config->dims_assign = &ocp_nlp_cost_nls_dims_assign; config->dims_initialize = &ocp_nlp_cost_nls_dims_initialize; config->dims_set = &ocp_nlp_cost_nls_dims_set; + config->dims_get = &ocp_nlp_cost_nls_dims_get; config->model_calculate_size = &ocp_nlp_cost_nls_model_calculate_size; config->model_assign = &ocp_nlp_cost_nls_model_assign; config->model_set = &ocp_nlp_cost_nls_model_set; diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.h b/acados/ocp_nlp/ocp_nlp_cost_nls.h index 5ca1b1159d..4b6368c5c3 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.h +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -63,6 +63,8 @@ void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory); void ocp_nlp_cost_nls_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); // void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value); diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 2e11ace125..0d5e0349e8 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -503,6 +503,7 @@ int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou { int dims_value = -1; + // ocp_nlp_dims if (!strcmp(field, "x")) { return dims->nx[stage]; @@ -511,6 +512,7 @@ int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou { return dims->nx[stage]; } + // ocp_nlp_constraints_dims else if (!strcmp(field, "lbx") || !strcmp(field, "ubx")) { config->constraints[stage]->dims_get(config->constraints[stage], dims->constraints[stage], @@ -523,6 +525,13 @@ int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou "nbu", &dims_value); return dims_value; } + // ocp_nlp_cost_dims + else if (!strcmp(field, "y_ref") || !strcmp(field, "yref")) + { + config->cost[stage]->dims_get(config->cost[stage], dims->cost[stage], + "ny", &dims_value); + return dims_value; + } else { printf("\nerror: ocp_nlp_dims_get: field %s not available\n", field); diff --git a/interfaces/acados_template/acados_template/generate_solver.py b/interfaces/acados_template/acados_template/generate_solver.py index d0cc87a3a7..b72d6f207c 100644 --- a/interfaces/acados_template/acados_template/generate_solver.py +++ b/interfaces/acados_template/acados_template/generate_solver.py @@ -307,6 +307,12 @@ def get(self, stage_, field_): return out def set(self, stage_, field_, value_): + + cost = ['y_ref', 'yref'] + constraints = ['lbx', 'ubx', 'lbu', 'ubu'] + + # cast value_ to avoid conversion issues + value_ = value_.astype(float) field = field_ field = field.encode('utf-8') @@ -323,8 +329,12 @@ def set(self, stage_, field_, value_): value_data_p = cast((value_data), c_void_p) stage = c_int(stage_) - self.shared_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value_data_p); + if field_ in constraints: + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value_data_p); + if field_ in cost: + self.shared_lib.ocp_nlp_cost_model_set.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, self.nlp_dims, self.nlp_in, stage, field, value_data_p); return diff --git a/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py index 126e0c0f3f..19b22667d9 100644 --- a/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py +++ b/interfaces/acados_template/examples/python/pendulum_example/generate_c_code.py @@ -94,11 +94,6 @@ ra.acados_include_path = '/usr/local/include' ra.acados_lib_path = '/usr/local/lib' -# json_layout = acados_ocp2json_layout(ra) -# with open('acados_layout.json', 'w') as f: -# json.dump(json_layout, f, default=np_array_to_list) -# exit() - acados_solver = generate_solver(model, ra, json_file = 'acados_ocp.json') Nsim = 100 @@ -125,6 +120,10 @@ acados_solver.set(0, "lbx", x0) acados_solver.set(0, "ubx", x0) + # update reference + for j in range(N): + acados_solver.set(j, "yref", np.array([0, 0, 0, 0, 0])) + # plot results import matplotlib import matplotlib.pyplot as plt From 19a77717ad25fa492da3c21e5de6812491664802 Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 13:11:50 +0200 Subject: [PATCH 67/78] add measure for regularization time --- acados/ocp_nlp/ocp_nlp_sqp.c | 22 ++++++++++---- acados/ocp_nlp/ocp_nlp_sqp.h | 1 + acados/ocp_nlp/ocp_nlp_sqp_rti.c | 20 ++++++++++--- acados/ocp_nlp/ocp_nlp_sqp_rti.h | 1 + .../linear_mass_spring_model/example_ocp.m | 3 +- .../masses_chain_model/example_ocp.m | 3 +- .../pendulum_on_cart_model/example_ocp.m | 29 +++++++++++++++---- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 3 +- interfaces/acados_matlab/ocp_get.c | 6 ++++ 9 files changed, 71 insertions(+), 17 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index d8cf28dc16..07b7358e60 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -1156,6 +1156,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, double total_time = 0.0; mem->time_qp_sol = 0.0; mem->time_lin = 0.0; + mem->time_reg = 0.0; mem->time_tot = 0.0; // extract dims @@ -1164,6 +1165,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int ii; int qp_iter = 0; + int qp_status = 0; #if defined(ACADOS_WITH_OPENMP) // backup number of threads @@ -1320,8 +1322,12 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, } + // start timer + acados_tic(&timer1); // regularize Hessian config->regularize->regularize_hessian(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); + // stop timer + mem->time_reg += acados_toc(&timer1); // printf("\n------- qp_in (sqp iter %d) --------\n", sqp_iter); // print_ocp_qp_in(work->qp_in); @@ -1337,16 +1343,17 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // start timer acados_tic(&timer1); - // TODO move qp_out in memory !!!!! (it has to be preserved to do warm start) - int qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, - opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); + qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); + // stop timer + mem->time_qp_sol += acados_toc(&timer1); + // start timer + acados_tic(&timer1); // compute correct dual solution in case of Hessian regularization config->regularize->correct_dual_sol(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); - // stop timer - mem->time_qp_sol += acados_toc(&timer1); + mem->time_reg += acados_toc(&timer1); // restore default warm start if(sqp_iter==0) @@ -1518,6 +1525,11 @@ void ocp_nlp_sqp_get(void *config_, void *mem_, const char *field, void *return_ double *value = return_value_; *value = mem->time_lin; } + else if (!strcmp("time_reg", field)) + { + double *value = return_value_; + *value = mem->time_reg; + } else if (!strcmp("nlp_res", field)) { ocp_nlp_res **value = return_value_; diff --git a/acados/ocp_nlp/ocp_nlp_sqp.h b/acados/ocp_nlp/ocp_nlp_sqp.h index 94b86b6ea5..74a61c62d9 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.h +++ b/acados/ocp_nlp/ocp_nlp_sqp.h @@ -103,6 +103,7 @@ typedef struct double time_qp_sol; double time_lin; + double time_reg; double time_tot; double *stat; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index bd12512066..e503009a18 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -1100,6 +1100,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, double total_time = 0.0; mem->time_qp_sol = 0.0; mem->time_lin = 0.0; + mem->time_reg = 0.0; mem->time_tot = 0.0; // extract dims @@ -1108,6 +1109,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, int ii; int qp_iter = 0; + int qp_status = 0; #if defined(ACADOS_WITH_OPENMP) // backup number of threads @@ -1212,8 +1214,12 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // save statistics mem->stat[mem->stat_n*1+0] = qp_iter; + // start timer + acados_tic(&timer1); // regularize Hessian config->regularize->regularize_hessian(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); + // stop timer + mem->time_reg += acados_toc(&timer1); // printf("\n------- qp_in (sqp iter %d) --------\n", sqp_iter); // print_ocp_qp_in(work->qp_in); @@ -1225,16 +1231,17 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // start timer acados_tic(&timer1); - // TODO move qp_out in memory !!!!! (it has to be preserved to do warm start) - int qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, - mem->qp_solver_mem, work->qp_work); - + qp_status = qp_solver->evaluate(qp_solver, work->qp_in, work->qp_out, opts->qp_solver_opts, mem->qp_solver_mem, work->qp_work); // stop timer mem->time_qp_sol += acados_toc(&timer1); + // start timer + acados_tic(&timer1); // compute correct dual solution in case of Hessian regularization config->regularize->correct_dual_sol(config->regularize, dims->regularize, opts->regularize, mem->regularize_mem); + // stop timer + mem->time_reg += acados_toc(&timer1); // TODO move into QP solver memory ??? nlp_out->qp_iter = ((ocp_qp_info *) work->qp_out->misc)->num_iter; @@ -1376,6 +1383,11 @@ void ocp_nlp_sqp_rti_get(void *config_, void *mem_, const char *field, void *ret double *value = return_value_; *value = mem->time_lin; } + else if (!strcmp("time_reg", field)) + { + double *value = return_value_; + *value = mem->time_reg; + } else if (!strcmp("stat", field)) { double **value = return_value_; diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/acados/ocp_nlp/ocp_nlp_sqp_rti.h index 22d8644e1a..cabf62149b 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.h +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -91,6 +91,7 @@ typedef struct double time_qp_sol; double time_lin; + double time_reg; double time_tot; double *stat; diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index 7eae93d22c..33b5dcb72c 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -287,9 +287,10 @@ sqp_iter = ocp.get('sqp_iter'); time_tot = ocp.get('time_tot'); time_lin = ocp.get('time_lin'); +time_reg = ocp.get('time_reg'); time_qp_sol = ocp.get('time_qp_sol'); -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 4391938eab..32705dba86 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -250,9 +250,10 @@ sqp_iter = ocp.get('sqp_iter'); time_tot = ocp.get('time_tot'); time_lin = ocp.get('time_lin'); +time_reg = ocp.get('time_reg'); time_qp_sol = ocp.get('time_qp_sol'); -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 0de410aa38..c83813444a 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -33,6 +33,7 @@ nlp_solver_tol_eq = 1e-8; nlp_solver_tol_ineq = 1e-8; nlp_solver_tol_comp = 1e-8; +nlp_solver_ext_qp_res = 1; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_cond_N = 5; @@ -186,6 +187,7 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); +ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); if (strcmp(nlp_solver, 'sqp')) ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('nlp_solver_tol_stat', nlp_solver_tol_stat); @@ -260,21 +262,38 @@ sqp_iter = ocp.get('sqp_iter'); time_tot = ocp.get('time_tot'); time_lin = ocp.get('time_lin'); +time_reg = ocp.get('time_reg'); time_qp_sol = ocp.get('time_qp_sol'); -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter\n'); + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); + if size(stat,2)>6 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d\n', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + if size(stat,2)>6 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + end + fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter\n'); + fprintf('\niter\tqp_iter'); + if size(stat,2)>2 + fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); + end + fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d\n', stat(ii,1), stat(ii,2)); + fprintf('%d\t%d', stat(ii,1), stat(ii,2)); + if size(stat,2)>2 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + end + fprintf('\n'); end fprintf('\n'); end diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 1779d5e9ff..240c76de6c 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -304,9 +304,10 @@ sqp_iter = ocp.get('sqp_iter'); time_tot = ocp.get('time_tot'); time_lin = ocp.get('time_lin'); +time_reg = ocp.get('time_reg'); time_qp_sol = ocp.get('time_qp_sol'); -fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3); +fprintf('\nstatus = %d, sqp_iter = %d, time_ext = %f [ms], time_int = %f [ms] (time_lin = %f [ms], time_qp_sol = %f [ms], time_reg = %f [ms])\n', status, sqp_iter, time_ext*1e3, time_tot*1e3, time_lin*1e3, time_qp_sol*1e3, time_reg*1e3); stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) diff --git a/interfaces/acados_matlab/ocp_get.c b/interfaces/acados_matlab/ocp_get.c index ebf1fff1f7..3a4de63577 100644 --- a/interfaces/acados_matlab/ocp_get.c +++ b/interfaces/acados_matlab/ocp_get.c @@ -147,6 +147,12 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) double *mat_ptr = mxGetPr( plhs[0] ); ocp_nlp_get(config, solver, "time_lin", mat_ptr); } + else if(!strcmp(field, "time_reg")) + { + plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); + double *mat_ptr = mxGetPr( plhs[0] ); + ocp_nlp_get(config, solver, "time_reg", mat_ptr); + } else if(!strcmp(field, "time_qp_sol")) { plhs[0] = mxCreateNumericMatrix(1, 1, mxDOUBLE_CLASS, mxREAL); From ae5f6c3a246fc4a8934944d1e1c876411a3d649d Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 13:54:43 +0200 Subject: [PATCH 68/78] fixed bug convexification correct dual sol --- acados/ocp_nlp/ocp_nlp_reg_convexify.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/acados/ocp_nlp/ocp_nlp_reg_convexify.c b/acados/ocp_nlp/ocp_nlp_reg_convexify.c index 7924d03dd9..9e8ba43c04 100644 --- a/acados/ocp_nlp/ocp_nlp_reg_convexify.c +++ b/acados/ocp_nlp/ocp_nlp_reg_convexify.c @@ -742,7 +742,7 @@ void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims // TODO avoid to multiply by R ??? blasfeo_dsymv_l(nu[N-1-ii]+nx[N-1-ii], nu[N-1-ii]+nx[N-1-ii], 1.0, mem->RSQrq[N-1-ii], 0, 0, mem->ux[N-1-ii], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0); blasfeo_dgemv_n(nx[N-1-ii], nx[N-ii], 1.0, mem->BAbt[N-1-ii], nu[N-1-ii], 0, mem->pi[N-1-ii], 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); - blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); + blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]); blasfeo_dveccp(nx[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii], mem->pi[N-2-ii], 0); } #endif From a90f6f8c8ba13ea897ae4a7d3b554c86425c4916 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 15:47:58 +0200 Subject: [PATCH 69/78] fix error messages --- acados/ocp_nlp/ocp_nlp_cost_ls.c | 2 +- acados/ocp_nlp/ocp_nlp_cost_nls.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_cost_ls.c b/acados/ocp_nlp/ocp_nlp_cost_ls.c index 1472087ab9..bbfe1313a2 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_ls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_ls.c @@ -196,7 +196,7 @@ void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int } else { - printf("error: attempt to get dimension from constraint model, that is not there"); + printf("error: ocp_nlp_cost_ls_dims_get: attempt to get dimensions of non-existing field %s\n", field); exit(1); } } diff --git a/acados/ocp_nlp/ocp_nlp_cost_nls.c b/acados/ocp_nlp/ocp_nlp_cost_nls.c index be591b927c..04fa323d51 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_nls.c +++ b/acados/ocp_nlp/ocp_nlp_cost_nls.c @@ -156,7 +156,7 @@ void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, in } else { - printf("error: attempt to get dimension from constraint model, that is not there"); + printf("error: ocp_nlp_cost_nls_dims_get: attempt to get dimensions of non-existing field %s\n", field); exit(1); } } From 1691f2c099d754579c345af7a1adfa8d879cede6 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 15:50:36 +0200 Subject: [PATCH 70/78] fix bug (nx -> nu) --- interfaces/acados_c/ocp_nlp_interface.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 0d5e0349e8..8d2e9f136a 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -510,7 +510,7 @@ int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou } else if (!strcmp(field, "u")) { - return dims->nx[stage]; + return dims->nu[stage]; } // ocp_nlp_constraints_dims else if (!strcmp(field, "lbx") || !strcmp(field, "ubx")) From 90f0d9c1b9f798e3f779c360e3b633d630e8a5ab Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 16:06:39 +0200 Subject: [PATCH 71/78] changing getter name (ocp_nlp_dims_get -> ocp_nlp_dims_of_attr_get) --- interfaces/acados_c/ocp_nlp_interface.c | 2 +- interfaces/acados_c/ocp_nlp_interface.h | 2 +- .../acados_template/generate_solver.py | 12 ++++++------ 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index 8d2e9f136a..d4b359510d 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -498,7 +498,7 @@ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou -int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +int ocp_nlp_dims_of_attr_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field) { int dims_value = -1; diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index be7c1b47ce..cf54f0af2d 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -249,7 +249,7 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, void *value); // -int ocp_nlp_dims_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +int ocp_nlp_dims_of_attr_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field); /* opts */ diff --git a/interfaces/acados_template/acados_template/generate_solver.py b/interfaces/acados_template/acados_template/generate_solver.py index b72d6f207c..4707857fbf 100644 --- a/interfaces/acados_template/acados_template/generate_solver.py +++ b/interfaces/acados_template/acados_template/generate_solver.py @@ -291,10 +291,10 @@ def get(self, stage_, field_): field = field_ field = field.encode('utf-8') - self.shared_lib.ocp_nlp_dims_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] - self.shared_lib.ocp_nlp_dims_get.restype = c_int + self.shared_lib.ocp_nlp_dims_of_attr_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_of_attr_get.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) + dims = self.shared_lib.ocp_nlp_dims_of_attr_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) @@ -317,10 +317,10 @@ def set(self, stage_, field_, value_): field = field_ field = field.encode('utf-8') - self.shared_lib.ocp_nlp_dims_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] - self.shared_lib.ocp_nlp_dims_get.restype = c_int + self.shared_lib.ocp_nlp_dims_of_attr_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_of_attr_get.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) + dims = self.shared_lib.ocp_nlp_dims_of_attr_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) if value_.shape[0] != dims: raise Exception('acados_solver.set(): mismatching dimension for field "{}" with dimension {} (you have {})'.format(field_,dims, value_.shape[0])) From d678181e813840a0ff38d775b77c2a85488c02d3 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 16:16:26 +0200 Subject: [PATCH 72/78] changing getter name (ocp_nlp_dims_of_attr_get -> ocp_nlp_dims_get_from_attr) --- interfaces/acados_c/ocp_nlp_interface.h | 2 +- .../acados_template/generate_solver.py | 12 ++++++------ .../rel_sync_machine_example/generate_c_code.py | 2 +- 3 files changed, 8 insertions(+), 8 deletions(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.h b/interfaces/acados_c/ocp_nlp_interface.h index cf54f0af2d..75e62953a9 100644 --- a/interfaces/acados_c/ocp_nlp_interface.h +++ b/interfaces/acados_c/ocp_nlp_interface.h @@ -249,7 +249,7 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, void *value); // -int ocp_nlp_dims_of_attr_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field); /* opts */ diff --git a/interfaces/acados_template/acados_template/generate_solver.py b/interfaces/acados_template/acados_template/generate_solver.py index 4707857fbf..80b7049eba 100644 --- a/interfaces/acados_template/acados_template/generate_solver.py +++ b/interfaces/acados_template/acados_template/generate_solver.py @@ -291,10 +291,10 @@ def get(self, stage_, field_): field = field_ field = field.encode('utf-8') - self.shared_lib.ocp_nlp_dims_of_attr_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] - self.shared_lib.ocp_nlp_dims_of_attr_get.restype = c_int + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_of_attr_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) @@ -317,10 +317,10 @@ def set(self, stage_, field_, value_): field = field_ field = field.encode('utf-8') - self.shared_lib.ocp_nlp_dims_of_attr_get.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] - self.shared_lib.ocp_nlp_dims_of_attr_get.restype = c_int + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int - dims = self.shared_lib.ocp_nlp_dims_of_attr_get(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage_, field) if value_.shape[0] != dims: raise Exception('acados_solver.set(): mismatching dimension for field "{}" with dimension {} (you have {})'.format(field_,dims, value_.shape[0])) diff --git a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py index b99c63dd3e..a00cbd1e7b 100644 --- a/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py +++ b/interfaces/acados_template/examples/python/rel_sync_machine_example/generate_c_code.py @@ -271,7 +271,7 @@ def get_general_constraints_DC(u_max): nlp_dims.nu = nu nlp_dims.np = np nlp_dims.N = N -# nlp_dims.npd_e = 0 +# nlp_dims.npd_e = -1 # nlp_dims.nh = 1 # set weighting matrices From 7f8f4b6fb0bab3e1e82a23442a3ef8226e6521c5 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 16:32:59 +0200 Subject: [PATCH 73/78] adding dummmy ocp_nlp_cost_external_dims_get --- acados/ocp_nlp/ocp_nlp_cost_external.c | 8 ++++++++ acados/ocp_nlp/ocp_nlp_cost_external.h | 2 ++ 2 files changed, 10 insertions(+) diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index d79d17d08f..3b489ccd8e 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -107,6 +107,14 @@ void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *fiel +void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value) +{ + printf("error: ocp_nlp_cost_external_dims_get: attempt to get dimensions of non-existing field %s\n", field); + exit(1); +} + + + /************************************************ * model ************************************************/ diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.h b/acados/ocp_nlp/ocp_nlp_cost_external.h index 98bc9751bd..85c48d0466 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.h +++ b/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -50,6 +50,8 @@ void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory); // void ocp_nlp_cost_external_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value); /************************************************ * model From 1ccdc74dd1d1b1ed04fb30f1e0f34667ac2fecc7 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Fri, 7 Jun 2019 18:06:13 +0200 Subject: [PATCH 74/78] fix config setup --- acados/ocp_nlp/ocp_nlp_cost_external.c | 1 + 1 file changed, 1 insertion(+) diff --git a/acados/ocp_nlp/ocp_nlp_cost_external.c b/acados/ocp_nlp/ocp_nlp_cost_external.c index 3b489ccd8e..a0918e0f4a 100644 --- a/acados/ocp_nlp/ocp_nlp_cost_external.c +++ b/acados/ocp_nlp/ocp_nlp_cost_external.c @@ -576,6 +576,7 @@ void ocp_nlp_cost_external_config_initialize_default(void *config_) config->dims_assign = &ocp_nlp_cost_external_dims_assign; config->dims_initialize = &ocp_nlp_cost_external_dims_initialize; config->dims_set = &ocp_nlp_cost_external_dims_set; + config->dims_get = &ocp_nlp_cost_external_dims_get; config->model_calculate_size = &ocp_nlp_cost_external_model_calculate_size; config->model_assign = &ocp_nlp_cost_external_model_assign; config->model_set = &ocp_nlp_cost_external_model_set; From dd1b1fe47bda6efb5761ce1cca31ff10305d8795 Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 18:11:55 +0200 Subject: [PATCH 75/78] do not exit sqp when qp's max iter is reached --- acados/ocp_nlp/ocp_nlp_sqp.c | 11 +++++----- acados/ocp_nlp/ocp_nlp_sqp_rti.c | 14 +++++++----- .../linear_mass_spring_model/example_ocp.m | 22 ++++++++++--------- .../masses_chain_model/example_ocp.m | 20 ++++++++--------- .../pendulum_on_cart_model/example_ocp.m | 20 ++++++++--------- .../matlab_mex/wind_turbine_nx6/example_ocp.m | 20 ++++++++--------- 6 files changed, 56 insertions(+), 51 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_sqp.c b/acados/ocp_nlp/ocp_nlp_sqp.c index 07b7358e60..5095a30625 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp.c +++ b/acados/ocp_nlp/ocp_nlp_sqp.c @@ -480,7 +480,7 @@ int ocp_nlp_sqp_memory_calculate_size(void *config_, void *dims_, void *opts_) // stat int stat_m = opts->max_iter+1; - int stat_n = 5; + int stat_n = 6; if(opts->ext_qp_res) stat_n += 4; size += stat_n*stat_m*sizeof(double); @@ -569,7 +569,7 @@ void *ocp_nlp_sqp_memory_assign(void *config_, void *dims_, void *opts_, void *r // stat mem->stat = (double *) c_ptr; mem->stat_m = opts->max_iter+1; - mem->stat_n = 5; + mem->stat_n = 6; if(opts->ext_qp_res) mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -1290,7 +1290,8 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, mem->stat[mem->stat_n*sqp_iter+1] = mem->nlp_res->inf_norm_res_b; mem->stat[mem->stat_n*sqp_iter+2] = mem->nlp_res->inf_norm_res_d; mem->stat[mem->stat_n*sqp_iter+3] = mem->nlp_res->inf_norm_res_m; - mem->stat[mem->stat_n*sqp_iter+4] = qp_iter; + mem->stat[mem->stat_n*sqp_iter+4] = qp_status; + mem->stat[mem->stat_n*sqp_iter+5] = qp_iter; } // exit conditions on residuals @@ -1369,7 +1370,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, if(opts->ext_qp_res) { ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); - ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+5)); + ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*(sqp_iter+1)+6)); // printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); } @@ -1379,7 +1380,7 @@ int ocp_nlp_sqp(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // exit(1); - if (qp_status != 0) + if (qp_status!=ACADOS_SUCCESS & qp_status!=ACADOS_MAXITER) { // print_ocp_qp_in(work->qp_in); diff --git a/acados/ocp_nlp/ocp_nlp_sqp_rti.c b/acados/ocp_nlp/ocp_nlp_sqp_rti.c index e503009a18..4ab3d45f1d 100644 --- a/acados/ocp_nlp/ocp_nlp_sqp_rti.c +++ b/acados/ocp_nlp/ocp_nlp_sqp_rti.c @@ -430,7 +430,7 @@ int ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts // stat int stat_m = 1+1; - int stat_n = 1; + int stat_n = 2; if(opts->ext_qp_res) stat_n += 4; size += stat_n*stat_m*sizeof(double); @@ -515,7 +515,7 @@ void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, voi // stat mem->stat = (double *) c_ptr; mem->stat_m = 1+1; - mem->stat_n = 1; + mem->stat_n = 2; if(opts->ext_qp_res) mem->stat_n += 4; c_ptr += mem->stat_m*mem->stat_n*sizeof(double); @@ -1212,7 +1212,8 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, sqp_update_qp_vectors(config, dims, nlp_in, nlp_out, opts, mem, work); // save statistics - mem->stat[mem->stat_n*1+0] = qp_iter; +// mem->stat[mem->stat_n*1+0] = qp_status; +// mem->stat[mem->stat_n*1+1] = qp_iter; // start timer acados_tic(&timer1); @@ -1251,7 +1252,7 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, if(opts->ext_qp_res) { ocp_qp_res_compute(work->qp_in, work->qp_out, work->qp_res, work->qp_res_ws); - ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+1)); + ocp_qp_res_compute_nrm_inf(work->qp_res, mem->stat+(mem->stat_n*1+2)); // printf("\nsqp_iter %d, res %e %e %e %e\n", sqp_iter, inf_norm_qp_res[0], inf_norm_qp_res[1], inf_norm_qp_res[2], inf_norm_qp_res[3]); } @@ -1261,9 +1262,10 @@ int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, // exit(1); // save statistics - mem->stat[mem->stat_n*1+0] = qp_iter; + mem->stat[mem->stat_n*1+0] = qp_status; + mem->stat[mem->stat_n*1+1] = qp_iter; - if (qp_status != 0) + if (qp_status!=ACADOS_SUCCESS & qp_status!=ACADOS_MAXITER) { // print_ocp_qp_in(work->qp_in); diff --git a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m index 33b5dcb72c..609483b474 100644 --- a/examples/matlab_mex/linear_mass_spring_model/example_ocp.m +++ b/examples/matlab_mex/linear_mass_spring_model/example_ocp.m @@ -28,6 +28,7 @@ %regularize_method = 'mirror'; %regularize_method = 'convexify'; nlp_solver_max_iter = 100; +nlp_solver_ext_qp_res = 1; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; qp_solver_cond_N = 5; @@ -208,6 +209,7 @@ ocp_opts.set('nlp_solver', nlp_solver); ocp_opts.set('nlp_solver_exact_hessian', nlp_solver_exact_hessian); ocp_opts.set('regularize_method', regularize_method); +ocp_opts.set('nlp_solver_ext_qp_res', nlp_solver_ext_qp_res); ocp_opts.set('nlp_solver_max_iter', nlp_solver_max_iter); ocp_opts.set('qp_solver', qp_solver); if (strcmp(qp_solver, 'partial_condensing_hpipm')) @@ -294,29 +296,29 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); - if size(stat,2)>6 + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_stat\tqp_iter'); + if size(stat,2)>7 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); - if size(stat,2)>6 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); + if size(stat,2)>7 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,8), stat(ii,9), stat(ii,10), stat(ii,11)); end fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter'); - if size(stat,2)>2 + fprintf('\niter\tqp_stat\tqp_iter'); + if size(stat,2)>3 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d', stat(ii,1), stat(ii,2)); - if size(stat,2)>2 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); end fprintf('\n'); end diff --git a/examples/matlab_mex/masses_chain_model/example_ocp.m b/examples/matlab_mex/masses_chain_model/example_ocp.m index 32705dba86..afc5dbeb3e 100644 --- a/examples/matlab_mex/masses_chain_model/example_ocp.m +++ b/examples/matlab_mex/masses_chain_model/example_ocp.m @@ -257,29 +257,29 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); - if size(stat,2)>6 + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_stat\tqp_iter'); + if size(stat,2)>7 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); - if size(stat,2)>6 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); + if size(stat,2)>7 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,8), stat(ii,9), stat(ii,10), stat(ii,11)); end fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter'); - if size(stat,2)>2 + fprintf('\niter\tqp_stat\tqp_iter'); + if size(stat,2)>3 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d', stat(ii,1), stat(ii,2)); - if size(stat,2)>2 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); end fprintf('\n'); end diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index c83813444a..824fc66906 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -269,29 +269,29 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); - if size(stat,2)>6 + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_stat\tqp_iter'); + if size(stat,2)>7 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); - if size(stat,2)>6 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); + if size(stat,2)>7 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,8), stat(ii,9), stat(ii,10), stat(ii,11)); end fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter'); - if size(stat,2)>2 + fprintf('\niter\tqp_stat\tqp_iter'); + if size(stat,2)>3 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d', stat(ii,1), stat(ii,2)); - if size(stat,2)>2 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); end fprintf('\n'); end diff --git a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m index 240c76de6c..1863324122 100644 --- a/examples/matlab_mex/wind_turbine_nx6/example_ocp.m +++ b/examples/matlab_mex/wind_turbine_nx6/example_ocp.m @@ -311,29 +311,29 @@ stat = ocp.get('stat'); if (strcmp(nlp_solver, 'sqp')) - fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_iter'); - if size(stat,2)>6 + fprintf('\niter\tres_g\t\tres_b\t\tres_d\t\tres_m\t\tqp_stat\tqp_iter'); + if size(stat,2)>7 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%e\t%e\t%e\t%e\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); - if size(stat,2)>6 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,7), stat(ii,8), stat(ii,9), stat(ii,10)); + fprintf('%d\t%e\t%e\t%e\t%e\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); + if size(stat,2)>7 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,8), stat(ii,9), stat(ii,10), stat(ii,11)); end fprintf('\n'); end fprintf('\n'); else % sqp_rti - fprintf('\niter\tqp_iter'); - if size(stat,2)>2 + fprintf('\niter\tqp_stat\tqp_iter'); + if size(stat,2)>3 fprintf('\tqp_res_g\tqp_res_b\tqp_res_d\tqp_res_m'); end fprintf('\n'); for ii=1:size(stat,1) - fprintf('%d\t%d', stat(ii,1), stat(ii,2)); - if size(stat,2)>2 - fprintf('\t%e\t%e\t%e\t%e', stat(ii,3), stat(ii,4), stat(ii,5), stat(ii,6)); + fprintf('%d\t%d\t%d', stat(ii,1), stat(ii,2), stat(ii,3)); + if size(stat,2)>3 + fprintf('\t%e\t%e\t%e\t%e', stat(ii,4), stat(ii,5), stat(ii,6), stat(ii,7)); end fprintf('\n'); end From 11b2b84b58ba213ddae0cc4aa96b602b09130abc Mon Sep 17 00:00:00 2001 From: giaf Date: Fri, 7 Jun 2019 18:52:48 +0200 Subject: [PATCH 76/78] add full_condensing_qpoases to mex interface --- .../pendulum_on_cart_model/example_ocp.m | 1 + interfaces/acados_matlab/acados_ocp.m | 2 +- interfaces/acados_matlab/ocp_compile_mex.m | 17 ++++++++++++++--- interfaces/acados_matlab/ocp_create.c | 4 ++++ 4 files changed, 20 insertions(+), 4 deletions(-) diff --git a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m index 824fc66906..a3bd4aaf35 100644 --- a/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m +++ b/examples/matlab_mex/pendulum_on_cart_model/example_ocp.m @@ -36,6 +36,7 @@ nlp_solver_ext_qp_res = 1; qp_solver = 'partial_condensing_hpipm'; %qp_solver = 'full_condensing_hpipm'; +%qp_solver = 'full_condensing_qpoases'; qp_solver_cond_N = 5; qp_solver_cond_ric_alg = 0; qp_solver_ric_alg = 0; diff --git a/interfaces/acados_matlab/acados_ocp.m b/interfaces/acados_matlab/acados_ocp.m index 211c4249c7..47227d16f0 100644 --- a/interfaces/acados_matlab/acados_ocp.m +++ b/interfaces/acados_matlab/acados_ocp.m @@ -18,7 +18,7 @@ % compile mex without model dependency if (strcmp(obj.opts_struct.compile_mex, 'true')) - ocp_compile_mex(); + ocp_compile_mex(obj.opts_struct); end obj.C_ocp = ocp_create(obj.model_struct, obj.opts_struct); diff --git a/interfaces/acados_matlab/ocp_compile_mex.m b/interfaces/acados_matlab/ocp_compile_mex.m index e8a46293e9..7eff219a09 100644 --- a/interfaces/acados_matlab/ocp_compile_mex.m +++ b/interfaces/acados_matlab/ocp_compile_mex.m @@ -1,4 +1,4 @@ -function ocp_compile_mex() +function ocp_compile_mex(opts) % get acados folder acados_folder = getenv('ACADOS_INSTALL_DIR'); @@ -33,6 +33,9 @@ function ocp_compile_mex() cflags_tmp = fscanf(input_file, '%[^\n]s'); fclose(input_file); cflags_tmp = [cflags_tmp, ' -std=c99 -fopenmp']; + if (strcmp(opts.qp_solver, 'full_condensing_qpoases')) + cflags_tmp = [cflags_tmp, ' -DACADOS_WITH_QPOASES']; + end input_file = fopen('cflags_octave.txt', 'w'); fprintf(input_file, '%s', cflags_tmp); fclose(input_file); @@ -47,8 +50,16 @@ function ocp_compile_mex() disp(['compiling ', mex_files{ii}]) if is_octave() % mkoctfile -p CFLAGS - mex(acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', mex_files{ii}) + if (strcmp(opts.qp_solver, 'full_condensing_qpoases')) + mex(acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', '-lqpOASES_e', mex_files{ii}) + else + mex(acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', mex_files{ii}) + end else - mex(mex_flags, 'CFLAGS=\$CFLAGS -std=c99 -fopenmp', acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', mex_files{ii}) + if (strcmp(opts.qp_solver, 'full_condensing_qpoases')) + mex(mex_flags, 'CFLAGS=\$CFLAGS -std=c99 -fopenmp -DACADOS_WITH_QPOASES', acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', '-lqpOASES_e', mex_files{ii}) + else + mex(mex_flags, 'CFLAGS=\$CFLAGS -std=c99 -fopenmp', acados_include, acados_interfaces_include, external_include, blasfeo_include, acados_lib_path, '-lacados_c', '-lacore', '-lhpipm', '-lblasfeo', mex_files{ii}) + end end end diff --git a/interfaces/acados_matlab/ocp_create.c b/interfaces/acados_matlab/ocp_create.c index 3e561efd86..d2ae3c2c5d 100644 --- a/interfaces/acados_matlab/ocp_create.c +++ b/interfaces/acados_matlab/ocp_create.c @@ -911,6 +911,10 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) { plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_HPIPM; } + else if(!strcmp(qp_solver, "full_condensing_qpoases")) + { + plan->ocp_qp_solver_plan.qp_solver = FULL_CONDENSING_QPOASES; + } else { mexPrintf("\nqp_solver not supported: %s\n", qp_solver); From 4aad75222095b33d852c942c1d8aee32f32f7568 Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Tue, 11 Jun 2019 13:26:50 +0200 Subject: [PATCH 77/78] forgot to add changes to ocp_nlp_interface.c --- interfaces/acados_c/ocp_nlp_interface.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/interfaces/acados_c/ocp_nlp_interface.c b/interfaces/acados_c/ocp_nlp_interface.c index d4b359510d..2c0e96f181 100644 --- a/interfaces/acados_c/ocp_nlp_interface.c +++ b/interfaces/acados_c/ocp_nlp_interface.c @@ -498,7 +498,7 @@ void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou -int ocp_nlp_dims_of_attr_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, +int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field) { int dims_value = -1; From d7a009f6d2703d07cdba1401e2b19ca2a0597d3b Mon Sep 17 00:00:00 2001 From: "andrea.zanelli" Date: Tue, 11 Jun 2019 13:31:21 +0200 Subject: [PATCH 78/78] get_dims -> dims_get --- acados/ocp_nlp/ocp_nlp_common.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/acados/ocp_nlp/ocp_nlp_common.c b/acados/ocp_nlp/ocp_nlp_common.c index 3552fc8d96..cc0e2f0629 100644 --- a/acados/ocp_nlp/ocp_nlp_common.c +++ b/acados/ocp_nlp/ocp_nlp_common.c @@ -569,8 +569,8 @@ void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, const c { // update ng_qp_solver in qp_solver int ng, nh, ng_qp_solver; - config->constraints[i]->get_dims(config->constraints[i], dims->constraints[i], "ng", &ng); - config->constraints[i]->get_dims(config->constraints[i], dims->constraints[i], "nh", &nh); + config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "ng", &ng); + config->constraints[i]->dims_get(config->constraints[i], dims->constraints[i], "nh", &nh); ng_qp_solver = ng + nh;