diff --git a/third_party/SConscript b/third_party/SConscript new file mode 100644 index 0000000..e5bbfaa --- /dev/null +++ b/third_party/SConscript @@ -0,0 +1,6 @@ +Import('env') + +env.Library('json11', ['json11/json11.cpp'], CCFLAGS=env['CCFLAGS'] + ['-Wno-unqualified-std-cast-call']) +env.Append(CPPPATH=[Dir('json11')]) + +env.Library('kaitai', ['kaitai/kaitaistream.cpp'], CPPDEFINES=['KS_STR_ENCODING_NONE']) diff --git a/third_party/acados/Darwin/t_renderer b/third_party/acados/Darwin/t_renderer new file mode 100644 index 0000000..fa8e993 Binary files /dev/null and b/third_party/acados/Darwin/t_renderer differ diff --git a/third_party/maplibre-native-qt/aarch64~HEAD b/third_party/acados/aarch64 similarity index 100% rename from third_party/maplibre-native-qt/aarch64~HEAD rename to third_party/acados/aarch64 diff --git a/third_party/acados/acados_template/.gitignore b/third_party/acados/acados_template/.gitignore new file mode 100644 index 0000000..63b741b --- /dev/null +++ b/third_party/acados/acados_template/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ + +# Cython intermediates +*_pyx.c +*_pyx.o +*_pyx.so diff --git a/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h new file mode 100644 index 0000000..59aee62 --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -0,0 +1,101 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SIM_{{ model.name }}_H_ +#define ACADOS_SIM_{{ model.name }}_H_ + +#include "acados_c/sim_interface.h" +#include "acados_c/external_function_interface.h" + +#define {{ model.name | upper }}_NX {{ dims.nx }} +#define {{ model.name | upper }}_NZ {{ dims.nz }} +#define {{ model.name | upper }}_NU {{ dims.nu }} +#define {{ model.name | upper }}_NP {{ dims.np }} + +#ifdef __cplusplus +extern "C" { +#endif + + +// ** capsule for solver data ** +typedef struct sim_solver_capsule +{ + // acados objects + sim_in *acados_sim_in; + sim_out *acados_sim_out; + sim_solver *acados_sim_solver; + sim_opts *acados_sim_opts; + sim_config *acados_sim_config; + void *acados_sim_dims; + + /* external functions */ + // ERK + external_function_param_{{ model.dyn_ext_fun_type }} * sim_forw_vde_casadi; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_vde_adj_casadi; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_fun_casadi; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_expl_ode_hess; + + // IRK + external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_fun_jac_x_xdot_z; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_jac_x_xdot_u_z; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_impl_dae_hess; + + // GNSF + external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_fun_jac_y; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_phi_jac_y_uhat; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_{{ model.dyn_ext_fun_type }} * sim_gnsf_get_matrices_fun; + +} sim_solver_capsule; + + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_create(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *value, int np); + +ACADOS_SYMBOL_EXPORT sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule); +ACADOS_SYMBOL_EXPORT sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule); + + +ACADOS_SYMBOL_EXPORT sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_SIM_{{ model.name }}_H_ diff --git a/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h new file mode 100644 index 0000000..5cf38aa --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/acados_solver.in.h @@ -0,0 +1,241 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_SOLVER_{{ model.name }}_H_ +#define ACADOS_SOLVER_{{ model.name }}_H_ + +#include "acados/utils/types.h" + +#include "acados_c/ocp_nlp_interface.h" +#include "acados_c/external_function_interface.h" + +#define {{ model.name | upper }}_NX {{ dims.nx }} +#define {{ model.name | upper }}_NZ {{ dims.nz }} +#define {{ model.name | upper }}_NU {{ dims.nu }} +#define {{ model.name | upper }}_NP {{ dims.np }} +#define {{ model.name | upper }}_NBX {{ dims.nbx }} +#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }} +#define {{ model.name | upper }}_NBU {{ dims.nbu }} +#define {{ model.name | upper }}_NSBX {{ dims.nsbx }} +#define {{ model.name | upper }}_NSBU {{ dims.nsbu }} +#define {{ model.name | upper }}_NSH {{ dims.nsh }} +#define {{ model.name | upper }}_NSG {{ dims.nsg }} +#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }} +#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }} +#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }} +#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }} +#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }} +#define {{ model.name | upper }}_NS {{ dims.ns }} +#define {{ model.name | upper }}_NSN {{ dims.ns_e }} +#define {{ model.name | upper }}_NG {{ dims.ng }} +#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }} +#define {{ model.name | upper }}_NGN {{ dims.ng_e }} +#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }} +#define {{ model.name | upper }}_NY {{ dims.ny }} +#define {{ model.name | upper }}_NYN {{ dims.ny_e }} +#define {{ model.name | upper }}_N {{ dims.N }} +#define {{ model.name | upper }}_NH {{ dims.nh }} +#define {{ model.name | upper }}_NPHI {{ dims.nphi }} +#define {{ model.name | upper }}_NHN {{ dims.nh_e }} +#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }} +#define {{ model.name | upper }}_NR {{ dims.nr }} + +#ifdef __cplusplus +extern "C" { +#endif + +{%- if not solver_options.custom_update_filename %} + {%- set custom_update_filename = "" %} +{% else %} + {%- set custom_update_filename = solver_options.custom_update_filename %} +{%- endif %} + +// ** capsule for solver data ** +typedef struct {{ model.name }}_solver_capsule +{ + // acados objects + ocp_nlp_in *nlp_in; + ocp_nlp_out *nlp_out; + ocp_nlp_out *sens_out; + ocp_nlp_solver *nlp_solver; + void *nlp_opts; + ocp_nlp_plan_t *nlp_solver_plan; + ocp_nlp_config *nlp_config; + ocp_nlp_dims *nlp_dims; + + // number of expected runtime parameters + unsigned int nlp_np; + + /* external functions */ + // dynamics +{% if solver_options.integrator_type == "ERK" %} + external_function_param_casadi *forw_vde_casadi; + external_function_param_casadi *expl_ode_fun; +{% if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi *hess_vde_casadi; +{%- endif %} +{% elif solver_options.integrator_type == "IRK" %} + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun; + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_z; + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_jac_x_xdot_u_z; +{% if solver_options.hessian_approx == "EXACT" %} + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_hess; +{%- endif %} +{% elif solver_options.integrator_type == "LIFTED_IRK" %} + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun; + external_function_param_{{ model.dyn_ext_fun_type }} *impl_dae_fun_jac_x_xdot_u; +{% elif solver_options.integrator_type == "GNSF" %} + external_function_param_casadi *gnsf_phi_fun; + external_function_param_casadi *gnsf_phi_fun_jac_y; + external_function_param_casadi *gnsf_phi_jac_y_uhat; + external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z; + external_function_param_casadi *gnsf_get_matrices_fun; +{% elif solver_options.integrator_type == "DISCRETE" %} + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun; + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt; +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess; +{%- endif %} +{%- endif %} + + + // cost +{% if cost.cost_type == "NONLINEAR_LS" %} + external_function_param_casadi *cost_y_fun; + external_function_param_casadi *cost_y_fun_jac_ut_xt; + external_function_param_casadi *cost_y_hess; +{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} + external_function_param_casadi *conl_cost_fun; + external_function_param_casadi *conl_cost_fun_jac_hess; +{%- elif cost.cost_type == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun; + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess; +{% endif %} + +{% if cost.cost_type_0 == "NONLINEAR_LS" %} + external_function_param_casadi cost_y_0_fun; + external_function_param_casadi cost_y_0_fun_jac_ut_xt; + external_function_param_casadi cost_y_0_hess; +{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + external_function_param_casadi conl_cost_0_fun; + external_function_param_casadi conl_cost_0_fun_jac_hess; +{% elif cost.cost_type_0 == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun; + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess; +{%- endif %} + +{% if cost.cost_type_e == "NONLINEAR_LS" %} + external_function_param_casadi cost_y_e_fun; + external_function_param_casadi cost_y_e_fun_jac_ut_xt; + external_function_param_casadi cost_y_e_hess; +{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} + external_function_param_casadi conl_cost_e_fun; + external_function_param_casadi conl_cost_e_fun_jac_hess; +{% elif cost.cost_type_e == "EXTERNAL" %} + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun; + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac; + external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess; +{%- endif %} + + // constraints +{%- if constraints.constr_type == "BGP" %} + external_function_param_casadi *phi_constraint; +{% elif constraints.constr_type == "BGH" and dims.nh > 0 %} + external_function_param_casadi *nl_constr_h_fun_jac; + external_function_param_casadi *nl_constr_h_fun; +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi *nl_constr_h_fun_jac_hess; +{%- endif %} +{%- endif %} + + +{% if constraints.constr_type_e == "BGP" %} + external_function_param_casadi phi_e_constraint; +{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %} + external_function_param_casadi nl_constr_h_e_fun_jac; + external_function_param_casadi nl_constr_h_e_fun; +{%- if solver_options.hessian_approx == "EXACT" %} + external_function_param_casadi nl_constr_h_e_fun_jac_hess; +{%- endif %} +{%- endif %} + +{%- if custom_update_filename != "" %} + void * custom_update_memory; +{%- endif %} + +} {{ model.name }}_solver_capsule; + +ACADOS_SYMBOL_EXPORT {{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule); + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule); + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_reset({{ model.name }}_solver_capsule* capsule, int reset_qp_solver_mem); + +/** + * Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than + * the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code + * generation, the time-steps from code generation is used. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps); +/** + * Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the + * nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps); +/** + * This function is used for updating an already initialized solver with a different number of qp_cond_N. + */ +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_qp_solver_cond_N({{ model.name }}_solver_capsule * capsule, int qp_solver_cond_N); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_update_params_sparse({{ model.name }}_solver_capsule * capsule, int stage, int *idx, double *p, int n_update); + +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT int {{ model.name }}_acados_custom_update({{ model.name }}_solver_capsule* capsule, double* data, int data_len); + + +ACADOS_SYMBOL_EXPORT ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_out *{{ model.name }}_acados_get_sens_out({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule); +ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SOLVER_{{ model.name }}_H_ diff --git a/third_party/acados/acados_template/c_templates_tera/constraints.in.h b/third_party/acados/acados_template/c_templates_tera/constraints.in.h new file mode 100644 index 0000000..d71ce5c --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/constraints.in.h @@ -0,0 +1,110 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_CONSTRAINTS +#define {{ model.name }}_CONSTRAINTS + +#ifdef __cplusplus +extern "C" { +#endif + +{% if dims.nphi > 0 %} +int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_constraint_sparsity_out(int); +int {{ model.name }}_phi_constraint_n_in(void); +int {{ model.name }}_phi_constraint_n_out(void); +{% endif %} + +{% if dims.nphi_e > 0 %} +int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); +const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); +const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); +int {{ model.name }}_phi_e_constraint_n_in(void); +int {{ model.name }}_phi_e_constraint_n_out(void); +{% endif %} + +{% if dims.nh > 0 %} +int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_sparsity_out(int); +int {{ model.name }}_constr_h_fun_n_in(void); +int {{ model.name }}_constr_h_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +{% if dims.nh_e > 0 %} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void); + +int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_n_in(void); +int {{ model.name }}_constr_h_e_fun_n_out(void); + +{% if solver_options.hessian_approx == "EXACT" -%} +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); +const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); +{% endif %} +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_CONSTRAINTS diff --git a/third_party/acados/acados_template/c_templates_tera/cost.in.h b/third_party/acados/acados_template/c_templates_tera/cost.in.h new file mode 100644 index 0000000..45eb09c --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/cost.in.h @@ -0,0 +1,238 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef {{ model.name }}_COST +#define {{ model.name }}_COST + +#ifdef __cplusplus +extern "C" { +#endif + + +// Cost at initial shooting node +{% if cost.cost_type_0 == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_n_in(void); +int {{ model.name }}_cost_y_0_fun_n_out(void); + +int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); +int {{ model.name }}_cost_y_0_hess_n_in(void); +int {{ model.name }}_cost_y_0_hess_n_out(void); +{% elif cost.cost_type_0 == "CONVEX_OVER_NONLINEAR" %} + +int {{ model.name }}_conl_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_0_fun_sparsity_in(int); +const int *{{ model.name }}_conl_cost_0_fun_sparsity_out(int); +int {{ model.name }}_conl_cost_0_fun_n_in(void); +int {{ model.name }}_conl_cost_0_fun_n_out(void); + +int {{ model.name }}_conl_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_0_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_conl_cost_0_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_conl_cost_0_fun_jac_hess_n_in(void); +int {{ model.name }}_conl_cost_0_fun_jac_hess_n_out(void); + +{% elif cost.cost_type_0 == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_0 == "casadi" %} +int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); + {%- else %} +int {{ cost.cost_function_ext_cost_0 }}(void **, void **, void *); + {%- endif %} +{% endif %} + + +// Cost at path shooting node +{% if cost.cost_type == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_sparsity_out(int); +int {{ model.name }}_cost_y_fun_n_in(void); +int {{ model.name }}_cost_y_fun_n_out(void); + +int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_hess_sparsity_out(int); +int {{ model.name }}_cost_y_hess_n_in(void); +int {{ model.name }}_cost_y_hess_n_out(void); + +{% elif cost.cost_type == "CONVEX_OVER_NONLINEAR" %} +int {{ model.name }}_conl_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_fun_sparsity_in(int); +const int *{{ model.name }}_conl_cost_fun_sparsity_out(int); +int {{ model.name }}_conl_cost_fun_n_in(void); +int {{ model.name }}_conl_cost_fun_n_out(void); + +int {{ model.name }}_conl_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_conl_cost_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_conl_cost_fun_jac_hess_n_in(void); +int {{ model.name }}_conl_cost_fun_jac_hess_n_out(void); +{% elif cost.cost_type == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type == "casadi" %} +int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); + {%- else %} +int {{ cost.cost_function_ext_cost }}(void **, void **, void *); + {%- endif %} +{% endif %} + +// Cost at terminal shooting node +{% if cost.cost_type_e == "NONLINEAR_LS" %} +int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_n_in(void); +int {{ model.name }}_cost_y_e_fun_n_out(void); + +int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); + +int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); +const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); +int {{ model.name }}_cost_y_e_hess_n_in(void); +int {{ model.name }}_cost_y_e_hess_n_out(void); +{% elif cost.cost_type_e == "CONVEX_OVER_NONLINEAR" %} +int {{ model.name }}_conl_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_e_fun_sparsity_in(int); +const int *{{ model.name }}_conl_cost_e_fun_sparsity_out(int); +int {{ model.name }}_conl_cost_e_fun_n_in(void); +int {{ model.name }}_conl_cost_e_fun_n_out(void); + +int {{ model.name }}_conl_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_conl_cost_e_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_conl_cost_e_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_conl_cost_e_fun_jac_hess_n_in(void); +int {{ model.name }}_conl_cost_e_fun_jac_hess_n_out(void); +{% elif cost.cost_type_e == "EXTERNAL" %} + {%- if cost.cost_ext_fun_type_e == "casadi" %} +int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); + +int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); +const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); + {%- else %} +int {{ cost.cost_function_ext_cost_e }}(void **, void **, void *); + {%- endif %} +{% endif %} + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_COST diff --git a/third_party/acados/acados_template/c_templates_tera/model.in.h b/third_party/acados/acados_template/c_templates_tera/model.in.h new file mode 100644 index 0000000..e5059df --- /dev/null +++ b/third_party/acados/acados_template/c_templates_tera/model.in.h @@ -0,0 +1,218 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef {{ model.name }}_MODEL +#define {{ model.name }}_MODEL + +#ifdef __cplusplus +extern "C" { +#endif + +{%- if solver_options.hessian_approx %} + {%- set hessian_approx = solver_options.hessian_approx %} +{%- elif solver_options.sens_hess %} + {%- set hessian_approx = "EXACT" %} +{%- else %} + {%- set hessian_approx = "GAUSS_NEWTON" %} +{%- endif %} + +{% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %} + {% if model.dyn_ext_fun_type == "casadi" %} +// implicit ODE: function +int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_n_in(void); +int {{ model.name }}_impl_dae_fun_n_out(void); + +// implicit ODE: function + jacobians +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void); + +// implicit ODE: jacobians only +int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int); +const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void); + +// implicit ODE - for lifted_irk +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void); + + {%- if hessian_approx == "EXACT" %} +// implicit ODE - hessian +int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_hess_sparsity_in(int); +const int *{{ model.name }}_impl_dae_hess_sparsity_out(int); +int {{ model.name }}_impl_dae_hess_n_in(void); +int {{ model.name }}_impl_dae_hess_n_out(void); + {% endif %} + {% else %}{# ext_fun_type #} + {%- if hessian_approx == "EXACT" %} +int {{ model.dyn_impl_dae_hess }}(void **, void **, void *); + {% endif %} +int {{ model.dyn_impl_dae_fun_jac }}(void **, void **, void *); +int {{ model.dyn_impl_dae_jac }}(void **, void **, void *); +int {{ model.dyn_impl_dae_fun }}(void **, void **, void *); + {% endif %}{# ext_fun_type #} + +{% elif solver_options.integrator_type == "GNSF" %} +/* GNSF Functions */ + {% if model.gnsf.purely_linear != 1 %} +// phi_fun +int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_n_in(void); +int {{ model.name }}_gnsf_phi_fun_n_out(void); + +// phi_fun_jac_y +int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void); + +// phi_jac_y_uhat +int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int); +const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void); + {% if model.gnsf.nontrivial_f_LO == 1 %} +// f_lo_fun_jac_x1k1uz +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int); +const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void); + {%- endif %} + {%- endif %} +// used to import model matrices +int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** res, int* iw, double* w, void *mem); +int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int); +const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int); +int {{ model.name }}_gnsf_get_matrices_fun_n_in(void); +int {{ model.name }}_gnsf_get_matrices_fun_n_out(void); + +{% elif solver_options.integrator_type == "ERK" %} +/* explicit ODE */ + +// explicit ODE +int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_fun_sparsity_in(int); +const int *{{ model.name }}_expl_ode_fun_sparsity_out(int); +int {{ model.name }}_expl_ode_fun_n_in(void); +int {{ model.name }}_expl_ode_fun_n_out(void); + +// explicit forward VDE +int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_forw_sparsity_in(int); +const int *{{ model.name }}_expl_vde_forw_sparsity_out(int); +int {{ model.name }}_expl_vde_forw_n_in(void); +int {{ model.name }}_expl_vde_forw_n_out(void); + +// explicit adjoint VDE +int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_vde_adj_sparsity_in(int); +const int *{{ model.name }}_expl_vde_adj_sparsity_out(int); +int {{ model.name }}_expl_vde_adj_n_in(void); +int {{ model.name }}_expl_vde_adj_n_out(void); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_expl_ode_hess_sparsity_in(int); +const int *{{ model.name }}_expl_ode_hess_sparsity_out(int); +int {{ model.name }}_expl_ode_hess_n_in(void); +int {{ model.name }}_expl_ode_hess_n_out(void); +{%- endif %} + +{% elif solver_options.integrator_type == "DISCRETE" %} + +{% if model.dyn_ext_fun_type == "casadi" %} +int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_n_out(void); + +int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void); + +{%- if hessian_approx == "EXACT" %} +int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int); +const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void); +{%- endif %} +{% else %} + {%- if hessian_approx == "EXACT" %} +int {{ model.dyn_disc_fun_jac_hess }}(void **, void **, void *); + {% endif %} +int {{ model.dyn_disc_fun_jac }}(void **, void **, void *); +int {{ model.dyn_disc_fun }}(void **, void **, void *); +{% endif %} + + +{% endif %} + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // {{ model.name }}_MODEL diff --git a/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h new file mode 100644 index 0000000..9611ea2 --- /dev/null +++ b/third_party/acados/acados_template/custom_update_templates/custom_update_function_zoro_template.in.h @@ -0,0 +1,44 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#include "acados_solver_{{ model.name }}.h" + +// Called at the end of solver creation. +// This is allowed to allocate memory and store the pointer to it into capsule->custom_update_memory. +int custom_update_init_function({{ model.name }}_solver_capsule* capsule); + + +// Custom update function that can be called between solver calls +int custom_update_function({{ model.name }}_solver_capsule* capsule, double* data, int data_len); + + +// Called just before destroying the solver. +// Responsible to free allocated memory, stored at capsule->custom_update_memory. +int custom_update_terminate_function({{ model.name }}_solver_capsule* capsule); diff --git a/third_party/acados/build.sh b/third_party/acados/build.sh new file mode 100644 index 0000000..b45c167 --- /dev/null +++ b/third_party/acados/build.sh @@ -0,0 +1,56 @@ +#!/usr/bin/env bash +set -e + +DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)" + +ARCHNAME="x86_64" +BLAS_TARGET="X64_AUTOMATIC" +if [ -f /TICI ]; then + ARCHNAME="larch64" + BLAS_TARGET="ARMV8A_ARM_CORTEX_A57" +fi + +ACADOS_FLAGS="-DACADOS_WITH_QPOASES=ON -UBLASFEO_TARGET -DBLASFEO_TARGET=$BLAS_TARGET" + +if [[ "$OSTYPE" == "darwin"* ]]; then + ACADOS_FLAGS="$ACADOS_FLAGS -DCMAKE_OSX_ARCHITECTURES=arm64;x86_64 -DCMAKE_MACOSX_RPATH=1" + ARCHNAME="Darwin" +fi + +if [ ! -d acados_repo/ ]; then + git clone https://github.com/acados/acados.git $DIR/acados_repo + # git clone https://github.com/commaai/acados.git $DIR/acados_repo +fi +cd acados_repo +git fetch --all +git checkout 8af9b0ad180940ef611884574a0b27a43504311d # v0.2.2 +git submodule update --depth=1 --recursive --init + +# build +mkdir -p build +cd build +cmake $ACADOS_FLAGS .. +make -j20 install + +INSTALL_DIR="$DIR/$ARCHNAME" +rm -rf $INSTALL_DIR +mkdir -p $INSTALL_DIR + +rm $DIR/acados_repo/lib/*.json + +rm -rf $DIR/include $DIR/acados_template +cp -r $DIR/acados_repo/include $DIR +cp -r $DIR/acados_repo/lib $INSTALL_DIR +cp -r $DIR/acados_repo/interfaces/acados_template/acados_template $DIR/ +#pip3 install -e $DIR/acados/interfaces/acados_template + +# build tera +cd $DIR/acados_repo/interfaces/acados_template/tera_renderer/ +if [[ "$OSTYPE" == "darwin"* ]]; then + cargo build --verbose --release --target aarch64-apple-darwin + cargo build --verbose --release --target x86_64-apple-darwin + lipo -create -output target/release/t_renderer target/x86_64-apple-darwin/release/t_renderer target/aarch64-apple-darwin/release/t_renderer +else + cargo build --verbose --release +fi +cp target/release/t_renderer $INSTALL_DIR/ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_common.h b/third_party/acados/include/acados/dense_qp/dense_qp_common.h new file mode 100644 index 0000000..2a9a974 --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_common.h @@ -0,0 +1,147 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_COMMON_H_ +#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_dense_qp.h" +#include "hpipm/include/hpipm_d_dense_qp_res.h" +#include "hpipm/include/hpipm_d_dense_qp_sol.h" +// acados +#include "acados/utils/types.h" + +typedef struct d_dense_qp_dim dense_qp_dims; +typedef struct d_dense_qp dense_qp_in; +typedef struct d_dense_qp_sol dense_qp_out; +typedef struct d_dense_qp_res dense_qp_res; +typedef struct d_dense_qp_res_ws dense_qp_res_ws; + + + +#ifndef QP_SOLVER_CONFIG_ +#define QP_SOLVER_CONFIG_ +typedef struct +{ + void (*dims_set)(void *config_, void *dims_, const char *field, const int* value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + 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); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args); + void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args); + int (*evaluate)(void *config, void *qp_in, void *qp_out, void *args, void *mem, void *work); + void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} qp_solver_config; +#endif + + + +#ifndef QP_INFO_ +#define QP_INFO_ +typedef struct +{ + double solve_QP_time; + double condensing_time; + double interface_time; + double total_time; + int num_iter; + int t_computed; +} qp_info; +#endif + + + +/* config */ +// +acados_size_t dense_qp_solver_config_calculate_size(); +// +qp_solver_config *dense_qp_solver_config_assign(void *raw_memory); + +/* dims */ +// +acados_size_t dense_qp_dims_calculate_size(); +// +dense_qp_dims *dense_qp_dims_assign(void *raw_memory); +// +void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value); +// + +/* in */ +// +acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims); +// +dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory); + +/* out */ +// +acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims); +// +dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_out_get(dense_qp_out *out, const char *field, void *value); + +/* res */ +// +acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims); +// +dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory); +// +acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims); +// +dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory); +// +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_nrm_inf(dense_qp_res *qp_res, double res[4]); + +/* misc */ +// +void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out); +// +void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out); +// +void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h new file mode 100644 index 0000000..b262089 --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_daqp.h @@ -0,0 +1,110 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ +#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// daqp +#include "daqp/include/types.h" + +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + + +typedef struct dense_qp_daqp_opts_ +{ + DAQPSettings* daqp_opts; + int warm_start; +} dense_qp_daqp_opts; + + +typedef struct dense_qp_daqp_memory_ +{ + double* lb_tmp; + double* ub_tmp; + int* idxb; + int* idxv_to_idxb; + int* idxs; + int* idxdaqp_to_idxs; + + double* Zl; + double* Zu; + double* zl; + double* zu; + double* d_ls; + double* d_us; + + double time_qp_solver_call; + int iter; + DAQPWorkspace * daqp_work; + +} dense_qp_daqp_memory; + +// opts +acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +// memory +acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +// functions +int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_daqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h new file mode 100644 index 0000000..136279d --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_hpipm.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_ +#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_dense_qp.h" +#include "hpipm/include/hpipm_d_dense_qp_ipm.h" +#include "hpipm/include/hpipm_d_dense_qp_sol.h" +// acados +#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_ws *hpipm_workspace; + double time_qp_solver_call; + int iter; + +} dense_qp_hpipm_memory; + + + +// +acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims); +// +void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory); +// +void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_); +// +void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_); +// +acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_); +// +void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_); +// +int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_hpipm_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h new file mode 100644 index 0000000..d051cb1 --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_ooqp.h @@ -0,0 +1,125 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_OOQP_H_ +#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +enum dense_qp_ooqp_termination_code +{ + DENSE_SUCCESSFUL_TERMINATION = 0, + DENSE_NOT_FINISHED, + DENSE_MAX_ITS_EXCEEDED, + DENSE_INFEASIBLE, + DENSE_UNKNOWN +}; + +typedef struct dense_qp_ooqp_opts_ +{ + int printLevel; + int useDiagonalWeights; // TODO(dimitris): implement option + int fixHessian; + int fixDynamics; + int fixInequalities; +} dense_qp_ooqp_opts; + +typedef struct dense_qp_ooqp_workspace_ +{ + double *x; + double *gamma; + double *phi; + double *y; + double *z; + double *lambda; + double *pi; + double objectiveValue; +} dense_qp_ooqp_workspace; + +typedef struct dense_qp_ooqp_memory_ +{ + int firstRun; + int nx; + int my; + int mz; + double *c; + double *dQ; + double *xlow; + char *ixlow; + double *xupp; + char *ixupp; + double *dA; + double *bA; + double *dC; + double *clow; + char *iclow; + double *cupp; + char *icupp; + double time_qp_solver_call; + int iter; + +} dense_qp_ooqp_memory; + +// +acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims); +// +void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_); +// +void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, + void *raw_memory); +// +acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_); +// +int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, + void *memory_, void *work_); +// +void dense_qp_ooqp_destroy(void *mem_, void *work); +// +void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_ooqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qore.h b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h new file mode 100644 index 0000000..392e472 --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qore.h @@ -0,0 +1,124 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_QORE_H_ +#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// qore +#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h" +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +typedef struct dense_qp_qore_opts_ +{ + int nsmax; // maximum size of Schur complement + int print_freq; // print frequency, + // prtfreq < 0: disable printing; + // prtfreq == 0: print on each call and include working set changes; + // prtfreq > 0: print on every prtfreq seconds, but do not include working set + // changes; + int warm_start; // warm start with updated matrices H and C + int warm_strategy; // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous + // solution + int hot_start; // hot start with unchanged matrices H and C + int max_iter; // maximum number of iterations + int compute_t; // compute t in qp_out (to have correct residuals in NLP) +} dense_qp_qore_opts; + +typedef struct dense_qp_qore_memory_ +{ + double *H; + double *HH; + double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; + double *A; + double *b; + double *C; + double *CC; + double *Ct; + double *CCt; + double *d_lb0; + double *d_ub0; + double *d_lb; + double *d_ub; + double *d_lg; + double *d_ug; + double *d_ls; + double *d_us; + double *lb; + double *ub; + int *idxb; + int *idxb_stacked; + int *idxs; + double *prim_sol; + double *dual_sol; + QoreProblemDense *QP; + int num_iter; + dense_qp_in *qp_stacked; + double time_qp_solver_call; + int iter; + +} dense_qp_qore_memory; + +acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_qore_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_QORE_H_ diff --git a/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h new file mode 100644 index 0000000..0e13d3e --- /dev/null +++ b/third_party/acados/include/acados/dense_qp/dense_qp_qpoases.h @@ -0,0 +1,126 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_ +#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/utils/types.h" + +typedef struct dense_qp_qpoases_opts_ +{ + double max_cputime; // maximum cpu time in seconds + int max_nwsr; // maximum number of working set recalculations + int warm_start; // warm start with dual_sol in memory + int use_precomputed_cholesky; + int hotstart; // this option requires constant data matrices! (eg linear MPC, inexact schemes + // with frozen sensitivities) + int set_acado_opts; // use same options as in acado code generation + int compute_t; // compute t in qp_out (to have correct residuals in NLP) + double tolerance; // terminationTolerance +} dense_qp_qpoases_opts; + +typedef struct dense_qp_qpoases_memory_ +{ + double *H; + double *HH; + double *R; + double *g; + double *gg; + double *Zl; + double *Zu; + double *zl; + double *zu; + double *A; + double *b; + double *d_lb0; + double *d_ub0; + double *d_lb; + double *d_ub; + double *C; + double *CC; + double *d_lg0; + double *d_ug0; + double *d_lg; + double *d_ug; + double *d_ls; + double *d_us; + int *idxb; + int *idxb_stacked; + int *idxs; + double *prim_sol; + double *dual_sol; + void *QPB; // NOTE(giaf): cast to QProblemB to use + void *QP; // NOTE(giaf): cast to QProblem to use + double cputime; // cputime of qpoases + int nwsr; // performed number of working set recalculations + int first_it; // to be used with hotstart + dense_qp_in *qp_stacked; + double time_qp_solver_call; // equal to cputime + int iter; + +} dense_qp_qpoases_memory; + +acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims); +// +void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory); +// +void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_); +// +void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_); +// +acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_); +// +void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void dense_qp_qpoases_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h new file mode 100644 index 0000000..ba97db9 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h @@ -0,0 +1,446 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + + +/// \defgroup ocp_nlp ocp_nlp +/// @{ +/// @} + +/// \defgroup ocp_nlp_solver ocp_nlp_solver +/// @{ +/// @} + +/// \ingroup ocp_nlp +/// @{ + +/// \ingroup ocp_nlp_solver +/// @{ + +/// \defgroup ocp_nlp_common ocp_nlp_common +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_xcond_solver.h" +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct ocp_nlp_config +{ + int N; // number of stages + + // solver-specific implementations of memory management functions + acados_size_t (*opts_calculate_size)(void *config, void *dims); + 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_); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_); + void *(*memory_assign)(void *config, void *dims, void *opts_, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value); + // evaluate solver // TODO rename into solve + int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); + void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work, + char *field, int stage, int index, void *sens_nlp_out); + // prepare memory + int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); + void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work); + // initalize this struct with default values + void (*config_initialize_default)(void *config); + // general getter + void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_); + void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_); + void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_); + // config structs of submodules + ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver + ocp_nlp_dynamics_config **dynamics; + ocp_nlp_cost_config **cost; + ocp_nlp_constraints_config **constraints; + ocp_nlp_reg_config *regularize; + +} ocp_nlp_config; + +// +acados_size_t ocp_nlp_config_calculate_size(int N); +// +ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory); + + + +/************************************************ + * dims + ************************************************/ + +/// Structure to store dimensions/number of variables. +typedef struct ocp_nlp_dims +{ + void **cost; + void **dynamics; + void **constraints; + ocp_qp_xcond_solver_dims *qp_solver; // xcond solver instead ?? + ocp_nlp_reg_dims *regularize; + + int *nv; // number of primal variables (states+controls+slacks) + int *nx; // number of differential states + int *nu; // number of inputs + int *ni; // number of two-sided inequality constraints: nb+ng+nh+ns + int *nz; // number of algebraic variables + int *ns; // number of slack variables + int N; // number of shooting nodes + + void *raw_memory; // Pointer to allocated memory, to be used for freeing +} ocp_nlp_dims; + +// +acados_size_t ocp_nlp_dims_calculate_size(void *config); +// +ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory); + +/// Sets the dimension of optimization variables +/// (states, constrols, algebraic variables, slack variables). +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param field The type of optimization variables, either nx, nu, nz, or ns. +/// \param value_array Number of variables for each stage. +void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_, + const char *field, const void* value_array); + +/// Sets the dimensions of constraints functions for a stage +/// (bounds on states, bounds on controls, equality constraints, +/// inequality constraints). +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh. +/// \param value_field Number of constraints/bounds for the given stage. +void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage, + const char *field, const void* value_field); + +/// Sets the dimensions of the cost terms for a stage. +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field Type of cost term, can be eiter ny. +/// \param value_field Number of cost terms/residuals for the given stage. +void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field, + const void* value_field); + +/// Sets the dimensions of the dynamics for a stage. +/// +/// \param config_ The configuration struct. +/// \param dims_ The dimension struct. +/// \param stage Stage number. +/// \param field TBD +/// \param value TBD +void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field, + const void* value); + +/************************************************ + * Inputs + ************************************************/ + +/// Struct for storing the inputs of an OCP NLP solver +typedef struct ocp_nlp_in +{ + /// Length of sampling intervals/timesteps. + double *Ts; + + /// Pointers to cost functions (TBC). + void **cost; + + /// Pointers to dynamics functions (TBC). + void **dynamics; + + /// Pointers to constraints functions (TBC). + void **constraints; + + /// Pointer to allocated memory, to be used for freeing. + void *raw_memory; + +} ocp_nlp_in; + +// +acados_size_t ocp_nlp_in_calculate_size_self(int N); +// +acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims); +// +ocp_nlp_in *ocp_nlp_in_assign_self(int N, void *raw_memory); +// +ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory); + + +/************************************************ + * out + ************************************************/ + +typedef struct ocp_nlp_out +{ + struct blasfeo_dvec *ux; // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs? + struct blasfeo_dvec *z; // algebraic vairables + struct blasfeo_dvec *pi; // multipliers for dynamics + struct blasfeo_dvec *lam; // inequality mulitpliers + struct blasfeo_dvec *t; // slack variables corresponding to evaluation of all inequalities (at the solution) + + // NOTE: the inequalities are internally organized in the following order: + // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + double inf_norm_res; + + void *raw_memory; // Pointer to allocated memory, to be used for freeing + +} ocp_nlp_out; + +// +acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims); +// +ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + void *raw_memory); + + + +/************************************************ + * options + ************************************************/ + +/// Globalization types +typedef enum +{ + FIXED_STEP, + MERIT_BACKTRACKING, +} ocp_nlp_globalization_t; + +typedef struct ocp_nlp_opts +{ + ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ??? + void *regularize; + void **dynamics; // dynamics_opts + void **cost; // cost_opts + void **constraints; // constraints_opts + double step_length; // step length in case of FIXED_STEP + double levenberg_marquardt; // LM factor to be added to the hessian before regularization + int reuse_workspace; + int num_threads; + int print_level; + + // TODO: move to separate struct? + ocp_nlp_globalization_t globalization; + int full_step_dual; + int line_search_use_sufficient_descent; + int globalization_use_SOC; + double alpha_min; + double alpha_reduction; + double eps_sufficient_descent; +} ocp_nlp_opts; + +// +acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value); +// +void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value); + + +/************************************************ + * residuals + ************************************************/ + +typedef struct ocp_nlp_res +{ + struct blasfeo_dvec *res_stat; // stationarity + struct blasfeo_dvec *res_eq; // dynamics + struct blasfeo_dvec *res_ineq; // inequality constraints + struct blasfeo_dvec *res_comp; // complementarity + struct blasfeo_dvec tmp; // tmp + double inf_norm_res_stat; + double inf_norm_res_eq; + double inf_norm_res_ineq; + double inf_norm_res_comp; + acados_size_t memsize; +} ocp_nlp_res; + +// +acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims); +// +ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory); +// +void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out); + +/************************************************ + * memory + ************************************************/ + +typedef struct ocp_nlp_memory +{ +// void *qp_solver_mem; // xcond solver mem instead ??? + ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ??? + void *regularize_mem; + void **dynamics; // dynamics memory + void **cost; // cost memory + void **constraints; // constraints memory + + // residuals + ocp_nlp_res *nlp_res; + + // qp in & out + ocp_qp_in *qp_in; + ocp_qp_out *qp_out; + // QP stuff not entering the qp_in struct + struct blasfeo_dmat *dzduxt; // dzdux transposed + struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables + + struct blasfeo_dvec *cost_grad; + struct blasfeo_dvec *ineq_fun; + struct blasfeo_dvec *ineq_adj; + struct blasfeo_dvec *dyn_fun; + struct blasfeo_dvec *dyn_adj; + + double cost_value; + + bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables + struct blasfeo_dvec *sim_guess; + + int *sqp_iter; // pointer to iteration number + +} ocp_nlp_memory; + +// +acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts); +// +ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_opts *opts, void *raw_memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct ocp_nlp_workspace +{ + + void *qp_work; + void **dynamics; // dynamics_workspace + void **cost; // cost_workspace + void **constraints; // constraints_workspace + + // for globalization: -> move to module?! + ocp_nlp_out *tmp_nlp_out; + ocp_nlp_out *weight_merit_fun; + struct blasfeo_dvec tmp_nxu; + struct blasfeo_dvec tmp_ni; + struct blasfeo_dvec dxnext_dy; + +} ocp_nlp_workspace; + +// +acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts); +// +ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_opts *opts, ocp_nlp_memory *mem, void *raw_memory); + + + +/************************************************ + * function + ************************************************/ + +void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_initialize_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_embed_initial_value(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_update_variables_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, double alpha); +// +int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +double ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work, + int check_early_termination); +// +double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +void ocp_nlp_initialize_t_slacks(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); +// +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); +// +void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h new file mode 100644 index 0000000..9dbf16f --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgh.h @@ -0,0 +1,238 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; + int nu; + int nz; + int nb; // nbx + nbu + int nbu; // number of input box constraints + int nbx; // number of state box constraints + int ng; // number of general linear constraints + int nh; // number of nonlinear path constraints + int ns; // nsbu + nsbx + nsg + nsh + int nsbu; // number of softened input bounds + int nsbx; // number of softened state bounds + int nsg; // number of softened general linear constraints + int nsh; // number of softened nonlinear constraints + int nbue; // number of input box constraints which are equality + int nbxe; // number of state box constraints which are equality + int nge; // number of general linear constraints which are equality + int nhe; // number of nonlinear path constraints which are equality +} ocp_nlp_constraints_bgh_dims; + +// +acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config); +// +void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_, + const char *field, const int* value); + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + int *idxb; + int *idxs; + int *idxe; + struct blasfeo_dvec d; // gathers bounds + struct blasfeo_dmat DCt; // general linear constraint matrix + // lg <= [D, C] * [u; x] <= ug + external_function_generic *nl_constr_h_fun; // nonlinear: lh <= h(x,u) <= uh + external_function_generic *nl_constr_h_fun_jac; // nonlinear: lh <= h(x,u) <= uh + external_function_generic *nl_constr_h_fun_jac_hess; // nonlinear: lh <= h(x,u) <= uh +} ocp_nlp_constraints_bgh_model; + +// +acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_, + void *model_, const char *field, void *value); + +// +void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_, + void *model_, const char *field, void *value); + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_constraints_bgh_opts; + +// +acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dvec *lam; // pointer to lam in nlp_out + struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out + struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dmat *DCt; // pointer to DCt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory + int *idxb; // pointer to idxb[ii] in qp_in + int *idxs_rev; // pointer to idxs_rev[ii] in qp_in + int *idxe; // pointer to idxe[ii] in qp_in +} ocp_nlp_constraints_bgh_memory; + +// +acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); +// +void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_nv; + struct blasfeo_dmat tmp_nz_nh; + struct blasfeo_dmat tmp_nv_nh; + struct blasfeo_dmat tmp_nz_nv; + struct blasfeo_dmat hess_z; + struct blasfeo_dvec tmp_ni; + struct blasfeo_dvec tmp_nh; +} ocp_nlp_constraints_bgh_workspace; + +// +acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts); + +/* functions */ + +// +void ocp_nlp_constraints_bgh_config_initialize_default(void *config); +// +void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts, + void *mem, void *work); +// +void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + +// +void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h new file mode 100644 index 0000000..eb05edf --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_bgp.h @@ -0,0 +1,218 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/* dims */ + +typedef struct +{ + int nx; + int nu; + int nz; + int nb; // nbx + nbu + int nbu; + int nbx; + int ng; // number of general linear constraints + int nphi; // dimension of convex outer part + int ns; // nsbu + nsbx + nsg + nsphi + int nsbu; // number of softened input bounds + int nsbx; // number of softened state bounds + int nsg; // number of softened general linear constraints + int nsphi; // number of softened nonlinear constraints + int nr; // dimension of nonlinear function in convex_over_nonlinear constraint + int nbue; // number of input box constraints which are equality + int nbxe; // number of state box constraints which are equality + int nge; // number of general linear constraints which are equality + int nphie; // number of nonlinear path constraints which are equality +} ocp_nlp_constraints_bgp_dims; + +// +acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config); +// +void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value); + + +/* model */ + +typedef struct +{ + // ocp_nlp_constraints_bgp_dims *dims; + int *idxb; + int *idxs; + int *idxe; + struct blasfeo_dvec d; + struct blasfeo_dmat DCt; + external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux; + external_function_generic *nl_constr_phi_o_r_fun; + external_function_generic *nl_constr_r_fun_jac; +} ocp_nlp_constraints_bgp_model; + +// +acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_, + void *model_, const char *field, void *value); +// +void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_, + void *model_, const char *field, void *value); + +/* options */ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_constraints_bgp_opts; + +// +acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value); + +/* memory */ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dvec *lam; // pointer to lam in nlp_out + struct blasfeo_dvec *tmp_lam;// pointer to lam in tmp_nlp_out + struct blasfeo_dvec *z_alg; // pointer to z_alg in ocp_nlp memory + struct blasfeo_dmat *DCt; // pointer to DCt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory + int *idxb; // pointer to idxb[ii] in qp_in + int *idxs_rev; // pointer to idxs_rev[ii] in qp_in + int *idxe; // pointer to idxe[ii] in qp_in +} ocp_nlp_constraints_bgp_memory; + +// +acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts, + void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_tmp_lam_ptr(struct blasfeo_dvec *tmp_lam, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory); +// +void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_); +// +void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_); +// +void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_); + +/* workspace */ + +typedef struct +{ + struct blasfeo_dvec tmp_ni; + struct blasfeo_dmat jac_r_ux_tran; + struct blasfeo_dmat tmp_nr_nphi_nr; + struct blasfeo_dmat tmp_nv_nr; + struct blasfeo_dmat tmp_nv_nphi; + struct blasfeo_dmat tmp_nz_nphi; +} ocp_nlp_constraints_bgp_workspace; + +// +acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts); + +/* functions */ + +// +void ocp_nlp_constraints_bgp_config_initialize_default(void *config); +// +void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model, + void *opts, void *mem, void *work); +// +void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims, + void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims, + void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_constraints_bgp_bounds_update(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h new file mode 100644 index 0000000..bb73c46 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_constraints_common.h @@ -0,0 +1,108 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_constraints ocp_nlp_constraints +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + acados_size_t (*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); + void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + 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, char *field, void *value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory); + struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); + void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory); + void (*memory_set_tmp_lam_ptr)(struct blasfeo_dvec *tmp_lam, void *memory); + void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); + void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory); + void (*memory_set_idxb_ptr)(int *idxb, void *memory); + void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory); + void (*memory_set_idxe_ptr)(int *idxe, void *memory); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*bounds_update)(void *config, void *dims, void *model, void *opts, void *mem, void *work); + void (*config_initialize_default)(void *config); + // dimension setters + void (*dims_set)(void *config_, void *dims_, const char *field, const int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int* value); +} ocp_nlp_constraints_config; + +// +acados_size_t ocp_nlp_constraints_config_calculate_size(); +// +ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h new file mode 100644 index 0000000..eb40564 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_common.h @@ -0,0 +1,107 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// +/// \defgroup ocp_nlp_cost ocp_nlp_cost +/// + +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_set)(void *config_, void *dims_, const char *field, int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int *value); + acados_size_t (*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_); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + 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, void *value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + double *(*memory_get_fun_ptr)(void *memory); + struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory); + void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory); + void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + + // computes the function value, gradient and hessian (approximation) of the cost function + void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + // computes the cost function value (intended for globalization) + void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*config_initialize_default)(void *config); + void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); + +} ocp_nlp_cost_config; + +// +acados_size_t ocp_nlp_cost_config_calculate_size(); +// +ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h new file mode 100644 index 0000000..2eb3f5d --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_conl.h @@ -0,0 +1,207 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl +/// \brief This module implements convex-over-nonlinear costs of the form +/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$, + + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states + int nz; // number of algebraic variables + int nu; // number of inputs + int ny; // number of outputs + int ns; // number of slacks +} ocp_nlp_cost_conl_dims; + +// +acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config); +// +void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz); +// +void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value); +// +void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + // slack penalty has the form z^T * s + .5 * s^T * Z * s + external_function_generic *conl_cost_fun; + external_function_generic *conl_cost_fun_jac_hess; + struct blasfeo_dvec y_ref; + struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector + struct blasfeo_dvec z; // gradient of slacks as vector + double scaling; +} ocp_nlp_cost_conl_model; + +// +acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + bool gauss_newton_hess; // dummy options, we always use a gauss-newton hessian +} ocp_nlp_cost_conl_opts; + +// +acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ +typedef struct +{ + struct blasfeo_dvec grad; // gradient of cost function + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + double fun; ///< value of the cost function +} ocp_nlp_cost_conl_memory; + +// +acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_conl_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat W; // hessian of outer loss function + struct blasfeo_dmat W_chol; // cholesky factor of hessian of outer loss function + struct blasfeo_dmat Jt_ux; // jacobian of inner residual function + struct blasfeo_dmat Jt_ux_tilde; // jacobian of inner residual function plus gradient contribution of algebraic variables + struct blasfeo_dmat Jt_z; // jacobian of inner residual function wrt algebraic variables + struct blasfeo_dmat tmp_nv_ny; + struct blasfeo_dvec tmp_ny; + struct blasfeo_dvec tmp_2ns; +} ocp_nlp_cost_conl_workspace; + +// +acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_conl_config_initialize_default(void *config); +// +void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); +// +void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h new file mode 100644 index 0000000..7895827 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_external.h @@ -0,0 +1,187 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states + int nz; // number of algebraic variables + int nu; // number of inputs + int ns; // number of slacks +} ocp_nlp_cost_external_dims; + +// +acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config); +// +void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory); +// +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 + ************************************************/ + +typedef struct +{ + external_function_generic *ext_cost_fun; // function + external_function_generic *ext_cost_fun_jac_hess; // function, gradient and hessian + external_function_generic *ext_cost_fun_jac; // function, gradient + struct blasfeo_dvec Z; + struct blasfeo_dvec z; + struct blasfeo_dmat numerical_hessian; // custom hessian approximation + double scaling; +} ocp_nlp_cost_external_model; + +// +acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int use_numerical_hessian; // > 0 indicating custom hessian is used instead of CasADi evaluation +} ocp_nlp_cost_external_opts; + +// +acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec grad; // gradient of cost function + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to tmp_ux in nlp_out + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + double fun; ///< value of the cost function +} ocp_nlp_cost_external_memory; + +// +acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_external_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nunx_nunx; + struct blasfeo_dmat tmp_nz_nz; + struct blasfeo_dmat tmp_nz_nunx; + struct blasfeo_dvec tmp_nunxnz; + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension 2*ns +} ocp_nlp_cost_external_workspace; + +// +acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_external_config_initialize_default(void *config); +// +void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_, + void *opts_, void *mem_, void *work_); +// +void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_ diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h new file mode 100644 index 0000000..801e9a5 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_ls.h @@ -0,0 +1,243 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls +/// \brief This module implements linear-least squares costs of the form +/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$. +/// @{ + + + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + +//////////////////////////////////////////////////////////////////////////////// +// dims // +//////////////////////////////////////////////////////////////////////////////// + +typedef struct +{ + int nx; // number of states + int nz; // number of algebraic variables + int nu; // number of inputs + int ny; // number of outputs + int ns; // number of slacks +} ocp_nlp_cost_ls_dims; + + +/// Calculate the size of the ocp_nlp_cost_ls_dims struct +/// +/// \param[in] config_ structure containing configuration of ocp_nlp_cost +/// module +/// \param[out] [] +/// \return \c size of ocp_nlp_dims struct +acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config); + + +/// Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct +/// +/// \param[in] config structure containing configuration of ocp_nlp_cost +/// module +/// \param[in] raw_memory pointer to memory location +/// \param[out] [] +/// \return dims +void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory); +// +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); + + +//////////////////////////////////////////////////////////////////////////////// +// model // +//////////////////////////////////////////////////////////////////////////////// + + +/// structure containing the data describing the linear least-square cost +typedef struct +{ + // slack penalty has the form z^T * s + .5 * s^T * Z * s + struct blasfeo_dmat Cyt; ///< output matrix: Cy * [u,x] = y; in transposed form + struct blasfeo_dmat Vz; ///< Vz in ls cost Vx*x + Vu*u + Vz*z + struct blasfeo_dmat W; ///< ls norm corresponding to this matrix + struct blasfeo_dvec y_ref; ///< yref + struct blasfeo_dvec Z; ///< diagonal Hessian of slacks as vector (lower and upper) + struct blasfeo_dvec z; ///< gradient of slacks as vector (lower and upper) + double scaling; + int W_changed; ///< flag indicating whether W has changed and needs to be refactorized + int Cyt_or_scaling_changed; ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed +} ocp_nlp_cost_ls_model; + +// +acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_, + const char *field, void *value_); + + + +//////////////////////////////////////////////////////////////////////////////// +// options // +//////////////////////////////////////////////////////////////////////////////// + + + +typedef struct +{ + int dummy; // struct can't be void +} ocp_nlp_cost_ls_opts; + +// +acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value); + + + +//////////////////////////////////////////////////////////////////////////////// +// memory // +//////////////////////////////////////////////////////////////////////////////// + + + +/// structure containing the memory associated with cost_ls component +/// of the ocp_nlp module +typedef struct +{ + struct blasfeo_dmat hess; ///< hessian of cost function + struct blasfeo_dmat W_chol; ///< cholesky factor of weight matrix + struct blasfeo_dvec res; ///< ls residual r(x) + struct blasfeo_dvec grad; ///< gradient of cost function + struct blasfeo_dvec *ux; ///< pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; ///< pointer to ux in tmp_nlp_out + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + struct blasfeo_dmat *RSQrq; ///< pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; ///< pointer to Z in qp_in + double fun; ///< value of the cost function +} ocp_nlp_cost_ls_memory; + +// +acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + + + +//////////////////////////////////////////////////////////////////////////////// +// workspace // +//////////////////////////////////////////////////////////////////////////////// + + + +typedef struct +{ + struct blasfeo_dmat tmp_nv_ny; // temporary matrix of dimensions nv, ny + struct blasfeo_dmat Cyt_tilde; // updated Cyt (after z elimination) + struct blasfeo_dmat dzdux_tran; // derivatives of z wrt u and x (tran) + struct blasfeo_dvec tmp_ny; // temporary vector of dimension ny + struct blasfeo_dvec tmp_2ns; // temporary vector of dimension ny + struct blasfeo_dvec tmp_nz; // temporary vector of dimension nz + struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination) +} ocp_nlp_cost_ls_workspace; + +// +acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts); + + + +//////////////////////////////////////////////////////////////////////////////// +// functions // +//////////////////////////////////////////////////////////////////////////////// + + +// computations that are done once when solver is created +void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_ls_config_initialize_default(void *config); +// +void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_, + void *mem_, void *work_); +// +void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_, + void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h new file mode 100644 index 0000000..5ec68cf --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_cost_nls.h @@ -0,0 +1,211 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_cost ocp_nlp_cost +/// @{ +/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls +/// \brief This module implements nonlinear-least squares costs of the form +/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$, + +#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ +#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_cost_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states + int nz; // number of algebraic variables + int nu; // number of inputs + int ny; // number of outputs + int ns; // number of slacks +} ocp_nlp_cost_nls_dims; + +// +acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config); +// +void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory); +// +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); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + // nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost + // slack penalty has the form z^T * s + .5 * s^T * Z * s + external_function_generic *nls_y_fun; // evaluation of nls function + external_function_generic *nls_y_fun_jac; // evaluation nls function and jacobian + external_function_generic *nls_y_hess; // hessian*seeds of nls residuals + struct blasfeo_dmat W; // + struct blasfeo_dvec y_ref; + struct blasfeo_dvec Z; // diagonal Hessian of slacks as vector + struct blasfeo_dvec z; // gradient of slacks as vector + double scaling; + int W_changed; ///< flag indicating whether W has changed and needs to be refactorized +} ocp_nlp_cost_nls_model; + +// +acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory); +// +int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_); + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + bool gauss_newton_hess; // gauss-newton hessian approximation +} ocp_nlp_cost_nls_opts; + +// +acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat W_chol; // cholesky factor of weight matrix + struct blasfeo_dmat Jt; // jacobian of nls fun + struct blasfeo_dvec res; // nls residual r(x) + struct blasfeo_dvec grad; // gradient of cost function + struct blasfeo_dvec *ux; // pointer to ux in nlp_out + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *Z; // pointer to Z in qp_in + struct blasfeo_dvec *z_alg; ///< pointer to z in sim_out + struct blasfeo_dmat *dzdux_tran; ///< pointer to sensitivity of a wrt ux in sim_out + double fun; ///< value of the cost function +} ocp_nlp_cost_nls_memory; + +// +acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_); +// +struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_); +// +void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory); +// +void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory); +// +void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_); +// +void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_); + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_ny; + struct blasfeo_dmat tmp_nv_nv; + struct blasfeo_dmat Vz; + struct blasfeo_dmat Cyt_tilde; + struct blasfeo_dvec tmp_ny; + struct blasfeo_dvec tmp_2ns; + struct blasfeo_dvec tmp_nz; +} ocp_nlp_cost_nls_workspace; + +// +acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts); + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_nls_config_initialize_default(void *config); +// +void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); +// +void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); +// +void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h new file mode 100644 index 0000000..43fe71b --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_common.h @@ -0,0 +1,119 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/sim/sim_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + + + +/************************************************ + * config + ************************************************/ + +typedef struct +{ + void (*config_initialize_default)(void *config); + sim_config *sim_solver; + /* dims */ + acados_size_t (*dims_calculate_size)(void *config); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_set)(void *config_, void *dims_, const char *field, int *value); + void (*dims_get)(void *config_, void *dims_, const char *field, int* value); + /* model */ + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_); + /* opts */ + acados_size_t (*opts_calculate_size)(void *config, void *dims); + void *(*opts_assign)(void *config, void *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, void *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void *value); + void (*opts_update)(void *config, void *dims, void *opts); + /* memory */ + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + // get shooting node gap x_next(x_n, u_n) - x_{n+1} + struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_); + struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_); + void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_); + void (*memory_set_tmp_ux_ptr)(struct blasfeo_dvec *tmp_ux, void *memory_); + void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_); + void (*memory_set_tmp_ux1_ptr)(struct blasfeo_dvec *tmp_ux1, void *memory_); + void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_); + void (*memory_set_tmp_pi_ptr)(struct blasfeo_dvec *tmp_pi, void *memory_); + void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_); + void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_); + void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_); + void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_); + void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_); + void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value); + /* workspace */ + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); +} ocp_nlp_dynamics_config; + +// +acados_size_t ocp_nlp_dynamics_config_calculate_size(); +// +ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h new file mode 100644 index 0000000..3afdc9f --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_cont.h @@ -0,0 +1,209 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" +#include "acados_c/sim_interface.h" + + + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + void *sim; + int nx; // number of states at the current stage + int nz; // number of algebraic states at the current stage + int nu; // number of inputs at the current stage + int nx1; // number of states at the next stage + int nu1; // number of inputes at the next stage +} ocp_nlp_dynamics_cont_dims; + +// +acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config); +// +void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value); + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + void *sim_solver; + int compute_adj; + int compute_hess; +} ocp_nlp_dynamics_cont_opts; + +// +acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage + struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage + struct blasfeo_dvec *tmp_ux1; // pointer to ux in tmp_nlp_out at next stage + struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage + struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage + struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in + struct blasfeo_dvec *z_alg; // pointer to output z at t = 0 + bool *set_sim_guess; // indicate if initialization for integrator is set from outside + struct blasfeo_dvec *sim_guess; // initializations for integrator + // struct blasfeo_dvec *z; // pointer to (input) z in nlp_out at current stage + struct blasfeo_dmat *dzduxt; // pointer to dzdux transposed + void *sim_solver; // sim solver memory +} ocp_nlp_dynamics_cont_memory; + +// +acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); +// +void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat hess; + sim_in *sim_in; + sim_out *sim_out; + void *sim_solver; // sim solver workspace +} ocp_nlp_dynamics_cont_workspace; + +acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + void *sim_model; + // double *state_transition; // TODO + double T; // simulation time +} ocp_nlp_dynamics_cont_model; + +// +acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); + + + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_dynamics_cont_config_initialize_default(void *config); +// +void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h new file mode 100644 index 0000000..6ea26a7 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_dynamics_disc.h @@ -0,0 +1,192 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_dynamics +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ +#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h" +#include "acados/utils/external_function_generic.h" +#include "acados/utils/types.h" + +/************************************************ + * dims + ************************************************/ + +typedef struct +{ + int nx; // number of states at the current stage + int nu; // number of inputs at the current stage + int nx1; // number of states at the next stage + int nu1; // number of inputes at the next stage +} ocp_nlp_dynamics_disc_dims; + +// +acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config); +// +void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory); +// +void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value); + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + int compute_adj; + int compute_hess; +} ocp_nlp_dynamics_disc_opts; + +// +acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts); +// +void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts); +// +int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_, + void *mem_, void *work_); + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + struct blasfeo_dvec fun; + struct blasfeo_dvec adj; + struct blasfeo_dvec *ux; // pointer to ux in nlp_out at current stage + struct blasfeo_dvec *tmp_ux; // pointer to ux in tmp_nlp_out at current stage + struct blasfeo_dvec *ux1; // pointer to ux in nlp_out at next stage + struct blasfeo_dvec *tmp_ux1;// pointer to ux in tmp_nlp_out at next stage + struct blasfeo_dvec *pi; // pointer to pi in nlp_out at current stage + struct blasfeo_dvec *tmp_pi; // pointer to pi in tmp_nlp_out at current stage + struct blasfeo_dmat *BAbt; // pointer to BAbt in qp_in + struct blasfeo_dmat *RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_dynamics_disc_memory; + +// +acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts); +// +void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory); +// +struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_ux_ptr(struct blasfeo_dvec *tmp_ux, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_ux1_ptr(struct blasfeo_dvec *tmp_ux1, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_tmp_pi_ptr(struct blasfeo_dvec *tmp_pi, void *memory); +// +void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + struct blasfeo_dmat tmp_nv_nv; +} ocp_nlp_dynamics_disc_workspace; + +acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts); + + + +/************************************************ + * model + ************************************************/ + +typedef struct +{ + external_function_generic *disc_dyn_fun; + external_function_generic *disc_dyn_fun_jac; + external_function_generic *disc_dyn_fun_jac_hess; +} ocp_nlp_dynamics_disc_model; + +// +acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims); +// +void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value); + + + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_dynamics_disc_config_initialize_default(void *config); +// +void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); +// +void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h new file mode 100644 index 0000000..9388f3f --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_common.h @@ -0,0 +1,122 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \ingroup ocp_nlp +/// @{ + +/// \defgroup ocp_nlp_reg ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" + + + +/* dims */ + +//typedef ocp_qp_dims ocp_nlp_reg_dims; +typedef struct +{ + int *nx; + int *nu; + int *nbu; + int *nbx; + int *ng; + int N; +} ocp_nlp_reg_dims; + +// +acados_size_t ocp_nlp_reg_dims_calculate_size(int N); +// +ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory); +// +void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value); + + + +/* config */ + +typedef struct +{ + /* dims */ + acados_size_t (*dims_calculate_size)(int N); + ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory); + void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value); + /* opts */ + acados_size_t (*opts_calculate_size)(void); + void *(*opts_assign)(void *raw_memory); + void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts); + void (*opts_set)(void *config, ocp_nlp_reg_dims *dims, void *opts, char *field, void* value); + /* memory */ + acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts); + void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value); + void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory); + 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); +} ocp_nlp_reg_config; + +// +acados_size_t ocp_nlp_reg_config_calculate_size(void); +// +void *ocp_nlp_reg_config_assign(void *raw_memory); + + + +/* regularization help functions */ +void acados_reconstruct_A(int dim, double *A, double *V, double *d); +void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon); +void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h new file mode 100644 index 0000000..cb52352 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_convexify.h @@ -0,0 +1,146 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double delta; + double epsilon; +// double gamma; // 0.0 +} ocp_nlp_reg_convexify_opts; + +// +acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void); +// +void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_convexify_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct { + double *R; + double *V; // TODO move to workspace + double *d; // TODO move to workspace + double *e; // TODO move to workspace + double *reg_hess; // TODO move to workspace + + struct blasfeo_dmat Q_tilde; + struct blasfeo_dmat Q_bar; + struct blasfeo_dmat BAQ; + struct blasfeo_dmat L; + struct blasfeo_dmat delta_eye; + struct blasfeo_dmat St_copy; + + struct blasfeo_dmat *original_RSQrq; + struct blasfeo_dmat tmp_RSQ; + + struct blasfeo_dvec tmp_nuxM; + struct blasfeo_dvec tmp_nbgM; + +// struct blasfeo_dvec grad; +// struct blasfeo_dvec b2; + + // giaf's + struct blasfeo_dmat **RSQrq; // pointer to RSQrq in qp_in + 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; + +// +acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config); + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h new file mode 100644 index 0000000..84a023c --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_mirror.h @@ -0,0 +1,121 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_mirror_opts; + +// +acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void); +// +void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_mirror_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 **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_mirror_memory; + +// +acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h new file mode 100644 index 0000000..b571f3b --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_noreg.h @@ -0,0 +1,106 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + + +/************************************************ + * options + ************************************************/ + +// +acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void); +// +void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_noreg_opts_set(void *config_, ocp_nlp_reg_dims *dims, void *opts_, char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ +// +acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + +// not needed + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_ + +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h new file mode 100644 index 0000000..682ea20 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project.h @@ -0,0 +1,121 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_reg +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// blasfeo +#include "blasfeo/include/blasfeo_common.h" + +// acados +#include "acados/ocp_nlp/ocp_nlp_reg_common.h" + + + +/************************************************ + * dims + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double epsilon; +} ocp_nlp_reg_project_opts; + +// +acados_size_t ocp_nlp_reg_project_opts_calculate_size(void); +// +void *ocp_nlp_reg_project_opts_assign(void *raw_memory); +// +void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_); +// +void ocp_nlp_reg_project_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 **RSQrq; // pointer to RSQrq in qp_in +} ocp_nlp_reg_project_memory; + +// +acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts); +// +void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory); + +/************************************************ + * workspace + ************************************************/ + + // TODO + +/************************************************ + * functions + ************************************************/ + +// +void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config); + + + +#ifdef __cplusplus +} +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_ +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h new file mode 100644 index 0000000..7e12952 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h @@ -0,0 +1,132 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \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 + ************************************************/ + +// use the functions in ocp_nlp_reg_common + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + double thr_eig; + double min_eig; + double min_pivot; + int pivoting; +} ocp_nlp_reg_project_reduc_hess_opts; + +// +acados_size_t 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; + +// +acados_size_t 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/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h new file mode 100644 index 0000000..fdb9641 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp.h @@ -0,0 +1,170 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_solver +/// @{ +/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_ +#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/types.h" + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + ocp_nlp_opts *nlp_opts; + 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 complementarity condition + int max_iter; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) + int qp_warm_start; // qp_warm_start in all but the first sqp iterations + bool warm_start_first_qp; // to set qp_warm_start in first iteration + int rti_phase; // only phase 0 at the moment + int initialize_t_slacks; // 0-false or 1-true + +} ocp_nlp_sqp_opts; + +// +acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims); +// +void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts); +// +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_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + // nlp memory + ocp_nlp_memory *nlp_mem; + + double time_qp_sol; + double time_qp_solver_call; + double time_qp_xcond; + double time_lin; + double time_reg; + double time_tot; + double time_glob; + double time_sim; + double time_sim_la; + double time_sim_ad; + double time_solution_sensitivities; + + // statistics + double *stat; + int stat_m; + int stat_n; + + int status; + int sqp_iter; + +} ocp_nlp_sqp_memory; + +// +acados_size_t 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); +// +void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, + void *opts_, void *mem_, void *work_); + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + ocp_nlp_workspace *nlp_work; + + // temp QP in & out (to be used as workspace in param sens) + ocp_qp_in *tmp_qp_in; + ocp_qp_out *tmp_qp_out; + + // qp residuals + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; + +} ocp_nlp_sqp_workspace; + +// +acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_); + + + +/************************************************ + * functions + ************************************************/ + +// +int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out, + void *args, void *mem, void *work_); +// +void ocp_nlp_sqp_config_initialize_default(void *config_); +// +int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, + void *opts_, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h new file mode 100644 index 0000000..364d0f4 --- /dev/null +++ b/third_party/acados/include/acados/ocp_nlp/ocp_nlp_sqp_rti.h @@ -0,0 +1,163 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +/// \addtogroup ocp_nlp +/// @{ +/// \addtogroup ocp_nlp_solver +/// @{ +/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti +/// @{ + +#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ +#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/utils/types.h" + + + +/************************************************ + * options + ************************************************/ + +typedef struct +{ + ocp_nlp_opts *nlp_opts; + int compute_dual_sol; + int ext_qp_res; // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging) + int qp_warm_start; // NOTE: this is not actually setting the warm_start! Just for compatibility with sqp. + bool warm_start_first_qp; // to set qp_warm_start in first iteration + int rti_phase; // phase of RTI. Possible values 1 (preparation), 2 (feedback) 0 (both) + +} ocp_nlp_sqp_rti_opts; + +// +acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_); +// +void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory); +// +void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_); +// +void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_); +// +void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value); +// +void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage, + const char *field, void* value); + + + +/************************************************ + * memory + ************************************************/ + +typedef struct +{ + // nlp memory + ocp_nlp_memory *nlp_mem; + + double time_qp_sol; + double time_qp_solver_call; + double time_qp_xcond; + double time_lin; + double time_reg; + double time_tot; + double time_glob; + double time_solution_sensitivities; + + // statistics + double *stat; + int stat_m; + int stat_n; + + int status; + +} ocp_nlp_sqp_rti_memory; + +// +acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_); +// +void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, + void *raw_memory); + + + +/************************************************ + * workspace + ************************************************/ + +typedef struct +{ + ocp_nlp_workspace *nlp_work; + + // temp QP in & out (to be used as workspace in param sens) + ocp_qp_in *tmp_qp_in; + ocp_qp_out *tmp_qp_out; + + // qp residuals + ocp_qp_res *qp_res; + ocp_qp_res_ws *qp_res_ws; + + +} ocp_nlp_sqp_rti_workspace; + +// +acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_); + + + +/************************************************ + * functions + ************************************************/ +// +int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_, + void *opts_, void *mem_, void *work_); +// +void ocp_nlp_sqp_rti_config_initialize_default(void *config_); +// +int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_, + void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_ +/// @} +/// @} +/// @} diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h new file mode 100644 index 0000000..d1a4563 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common.h @@ -0,0 +1,180 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_H_ +#define ACADOS_OCP_QP_OCP_QP_COMMON_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp.h" +#include "hpipm/include/hpipm_d_ocp_qp_dim.h" +#include "hpipm/include/hpipm_d_ocp_qp_res.h" +#include "hpipm/include/hpipm_d_ocp_qp_sol.h" +// acados +#include "acados/utils/types.h" + + + +typedef struct d_ocp_qp_dim ocp_qp_dims; +typedef struct d_ocp_qp ocp_qp_in; +typedef struct d_ocp_qp_sol ocp_qp_out; +typedef struct d_ocp_qp_res ocp_qp_res; +typedef struct d_ocp_qp_res_ws ocp_qp_res_ws; + + + +#ifndef QP_SOLVER_CONFIG_ +#define QP_SOLVER_CONFIG_ +typedef struct +{ + void (*dims_set)(void *config_, void *dims_, int stage, const char *field, int* value); + acados_size_t (*opts_calculate_size)(void *config, void *dims); + 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, void* value); + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); + void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} qp_solver_config; +#endif + + + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config, int N); + void *(*dims_assign)(void *config, int N, void *raw_memory); + void (*dims_set)(void *config, void *dims_, int stage, const char *field, int* value); + void (*dims_get)(void *config, void *dims, const char *field, void* value); + // TODO add config everywhere !!!!! + acados_size_t (*opts_calculate_size)(void *dims); + void *(*opts_assign)(void *dims, void *raw_memory); + void (*opts_initialize_default)(void *dims, void *opts); + void (*opts_update)(void *dims, void *opts); + void (*opts_set)(void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *dims, void *opts); + void *(*memory_assign)(void *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config, void *mem, const char *field, void* value); + acados_size_t (*workspace_calculate_size)(void *dims, void *opts); + int (*condensing)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*condensing_rhs)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); + int (*expansion)(void *qp_in, void *qp_out, void *opts, void *mem, void *work); +} ocp_qp_xcond_config; + + + +/// Struct containing metrics of the qp solver. +#ifndef QP_INFO_ +#define QP_INFO_ +typedef struct +{ + double solve_QP_time; + double condensing_time; + double interface_time; + double total_time; + int num_iter; + int t_computed; +} qp_info; +#endif + + + +/* config */ +// +acados_size_t ocp_qp_solver_config_calculate_size(); +// +qp_solver_config *ocp_qp_solver_config_assign(void *raw_memory); +// +acados_size_t ocp_qp_condensing_config_calculate_size(); +// +ocp_qp_xcond_config *ocp_qp_condensing_config_assign(void *raw_memory); + + +/* dims */ +// +acados_size_t ocp_qp_dims_calculate_size(int N); +// +ocp_qp_dims *ocp_qp_dims_assign(int N, void *raw_memory); +// +void ocp_qp_dims_set(void *config_, void *dims, int stage, const char *field, int* value); +// +void ocp_qp_dims_get(void *config_, void *dims, int stage, const char *field, int* value); + + +/* in */ +// +acados_size_t ocp_qp_in_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_in *ocp_qp_in_assign(ocp_qp_dims *dims, void *raw_memory); + + +/* out */ +// +acados_size_t ocp_qp_out_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_out *ocp_qp_out_assign(ocp_qp_dims *dims, void *raw_memory); + +/* res */ +// +acados_size_t ocp_qp_res_calculate_size(ocp_qp_dims *dims); +// +ocp_qp_res *ocp_qp_res_assign(ocp_qp_dims *dims, void *raw_memory); +// +acados_size_t 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_nrm_inf(ocp_qp_res *qp_res, double res[4]); + + +/* misc */ +// +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); +// +void ocp_qp_compute_t(ocp_qp_in *qp_in, ocp_qp_out *qp_out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_COMMON_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h new file mode 100644 index 0000000..f65f602 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_common_frontend.h @@ -0,0 +1,118 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ +#define ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" + +typedef struct +{ + int N; + int *nx; + int *nu; + int *nb; + int *nc; + double **A; + double **B; + double **b; + double **Q; + double **S; + double **R; + double **q; + double **r; + int **idxb; + double **lb; + double **ub; + double **Cx; + double **Cu; + double **lc; + double **uc; +} colmaj_ocp_qp_in; + +typedef struct +{ + double **x; + double **u; + double **pi; + double **lam; +} colmaj_ocp_qp_out; + +typedef struct +{ + double **res_r; + double **res_q; + double **res_ls; + double **res_us; + double **res_b; + double **res_d_lb; + double **res_d_ub; + double **res_d_lg; + double **res_d_ug; + double **res_d_ls; + double **res_d_us; + double **res_m_lb; + double **res_m_ub; + double **res_m_lg; + double **res_m_ug; + double **res_m_ls; + double **res_m_us; + double res_nrm_inf[4]; +} colmaj_ocp_qp_res; + +// +acados_size_t colmaj_ocp_qp_in_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_in(ocp_qp_dims *dims, colmaj_ocp_qp_in **qp_in, void *ptr); +// +acados_size_t colmaj_ocp_qp_out_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_out(ocp_qp_dims *dims, colmaj_ocp_qp_out **qp_out, void *ptr); +// +acados_size_t colmaj_ocp_qp_res_calculate_size(ocp_qp_dims *dims); +// +char *assign_colmaj_ocp_qp_res(ocp_qp_dims *dims, colmaj_ocp_qp_res **qp_res, void *ptr); +// +void convert_colmaj_to_ocp_qp_in(colmaj_ocp_qp_in *cm_qp_in, ocp_qp_in *qp_in); +// +void convert_ocp_qp_out_to_colmaj(ocp_qp_out *qp_out, colmaj_ocp_qp_out *cm_qp_out); +// +void convert_ocp_qp_res_to_colmaj(ocp_qp_res *qp_res, colmaj_ocp_qp_res *cm_qp_res); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_COMMON_FRONTEND_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h new file mode 100644 index 0000000..d23e658 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_full_condensing.h @@ -0,0 +1,114 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_ +#define ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_red.h" +// acados +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + ocp_qp_dims *red_dims; // dims of reduced qp + dense_qp_dims *fcond_dims; +} ocp_qp_full_condensing_dims; + + + +typedef struct ocp_qp_full_condensing_opts_ +{ + struct d_cond_qp_arg *hpipm_cond_opts; + struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts; +// dense_qp_dims *fcond_dims; // TODO(all): move to dims + 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; + int mem_qp_in; // allocate qp_in in memory +} ocp_qp_full_condensing_opts; + + + +typedef struct ocp_qp_full_condensing_memory_ +{ + struct d_cond_qp_ws *hpipm_cond_work; + struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work; + // in memory + dense_qp_in *fcond_qp_in; + dense_qp_out *fcond_qp_out; + ocp_qp_in *red_qp; // reduced qp + ocp_qp_out *red_sol; // reduced qp sol + // only pointer + ocp_qp_in *ptr_qp_in; + qp_info *qp_out_info; // info in fcond_qp_in + double time_qp_xcond; +} ocp_qp_full_condensing_memory; + + + +// +acados_size_t ocp_qp_full_condensing_opts_calculate_size(void *dims); +// +void *ocp_qp_full_condensing_opts_assign(void *dims, void *raw_memory); +// +void ocp_qp_full_condensing_opts_initialize_default(void *dims, void *opts_); +// +void ocp_qp_full_condensing_opts_update(void *dims, void *opts_); +// +void ocp_qp_full_condensing_opts_set(void *opts_, const char *field, void* value); +// +acados_size_t ocp_qp_full_condensing_memory_calculate_size(void *dims, void *opts_); +// +void *ocp_qp_full_condensing_memory_assign(void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_full_condensing_workspace_calculate_size(void *dims, void *opts_); +// +int ocp_qp_full_condensing(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_full_expansion(void *in, void *out, void *opts, void *mem, void *work); +// +void ocp_qp_full_condensing_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_FULL_CONDENSING_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h new file mode 100644 index 0000000..261606b --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpipm.h @@ -0,0 +1,100 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_HPIPM_H_ +#define ACADOS_OCP_QP_OCP_QP_HPIPM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_ipm.h" +// acados +#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_ +{ + 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_ +{ + struct d_ocp_qp_ipm_ws *hpipm_workspace; + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_hpipm_memory; + + + +// +acados_size_t ocp_qp_hpipm_opts_calculate_size(void *config, void *dims); +// +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); +// +acados_size_t 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); +// +acados_size_t 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_memory_reset(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpipm_eval_sens(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 + +#endif // ACADOS_OCP_QP_OCP_QP_HPIPM_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h new file mode 100644 index 0000000..8db53a2 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_hpmpc.h @@ -0,0 +1,127 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_HPMPC_H_ +#define ACADOS_OCP_QP_OCP_QP_HPMPC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef enum hpmpc_options_t_ { HPMPC_DEFAULT_ARGUMENTS } hpmpc_options_t; + +typedef struct ocp_qp_hpmpc_opts_ +{ + double tol; + int max_iter; + double mu0; + double alpha_min; + int warm_start; + int N2; // horizion length of the partially condensed problem + + // partial tightening + double sigma_mu; + int N; + int M; +} ocp_qp_hpmpc_opts; + +// struct of the solver memory +typedef struct ocp_qp_hpmpc_memory_ +{ + struct blasfeo_dvec *hpi; + double *stats; + + // workspace + void *hpmpc_work; // raw workspace + + // partial tightening-specific (init of extra variables) + struct blasfeo_dvec *lam0; + struct blasfeo_dvec *ux0; + struct blasfeo_dvec *pi0; + struct blasfeo_dvec *t0; + + // 2. workspace + struct blasfeo_dmat *hsL; + struct blasfeo_dmat *hsric_work_mat; + struct blasfeo_dmat sLxM; + struct blasfeo_dmat sPpM; + + struct blasfeo_dvec *hsQx; + struct blasfeo_dvec *hsqx; + struct blasfeo_dvec *hstinv; + struct blasfeo_dvec *hsrq; + struct blasfeo_dvec *hsdux; + + struct blasfeo_dvec *hsdlam; + struct blasfeo_dvec *hsdt; + struct blasfeo_dvec *hsdpi; + struct blasfeo_dvec *hslamt; + + struct blasfeo_dvec *hsPb; + + void *work_ric; + + int out_iter; + + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_hpmpc_memory; + +acados_size_t ocp_qp_hpmpc_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_hpmpc_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_hpmpc_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_hpmpc_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_hpmpc_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_hpmpc_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_hpmpc_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_hpmpc(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpmpc_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_hpmpc_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_HPMPC_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h new file mode 100644 index 0000000..a535503 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_ooqp.h @@ -0,0 +1,144 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_OOQP_H_ +#define ACADOS_OCP_QP_OCP_QP_OOQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +enum ocp_qp_ooqp_termination_code +{ + SPARSE_SUCCESSFUL_TERMINATION = 0, + SPARSE_NOT_FINISHED, + SPARSE_MAX_ITS_EXCEEDED, + SPARSE_INFEASIBLE, + SPARSE_UNKNOWN +}; + +typedef struct ocp_qp_ooqp_opts_ +{ + int printLevel; + int useDiagonalWeights; // TODO(dimitris): implement option + int fixHessian; + int fixHessianSparsity; + int fixDynamics; + int fixDynamicsSparsity; + int fixInequalities; + int fixInequalitiesSparsity; +} ocp_qp_ooqp_opts; + +typedef struct ocp_qp_ooqp_workspace_ +{ + double *x; + double *gamma; + double *phi; + double *y; + double *z; + double *lambda; + double *pi; + double objectiveValue; + int *tmpInt; // temporary vector to sort indicies sparse matrices + double *tmpReal; // temporary vector to sort data of sparse matrices + // int ierr; +} ocp_qp_ooqp_workspace; + +typedef struct ocp_qp_ooqp_memory_ +{ + int firstRun; + double *c; + int nx; + int *irowQ; + int nnzQ; + int *jcolQ; + int *orderQ; + double *dQ; + double *xlow; + char *ixlow; + double *xupp; + char *ixupp; + int *irowA; + int nnzA; + int *jcolA; + int *orderA; + double *dA; + double *bA; + int my; + int *irowC; + int nnzC; + int *jcolC; + int *orderC; + double *dC; + double *clow; + int mz; + char *iclow; + double *cupp; + char *icupp; + int nnz; // max(nnzQ, nnzA, nnzC) + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_ooqp_memory; + +// +acados_size_t ocp_qp_ooqp_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_ooqp_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_ooqp_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_ooqp_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_ooqp_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_ooqp_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_ooqp_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_ooqp(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_, + void *work_); +// +void ocp_qp_ooqp_destroy(void *mem_, void *work); +// +void ocp_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_ooqp_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_OOQP_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h new file mode 100644 index 0000000..51df1b1 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_osqp.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_OSQP_H_ +#define ACADOS_OCP_QP_OCP_QP_OSQP_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// osqp +#include "osqp/include/types.h" + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef struct ocp_qp_osqp_opts_ +{ + OSQPSettings *osqp_opts; +} ocp_qp_osqp_opts; + + +typedef struct ocp_qp_osqp_memory_ +{ + c_int first_run; + + c_float *q; + c_float *l; + c_float *u; + + c_int P_nnzmax; + c_int *P_i; + c_int *P_p; + c_float *P_x; + + c_int A_nnzmax; + c_int *A_i; + c_int *A_p; + c_float *A_x; + + OSQPData *osqp_data; + OSQPWorkspace *osqp_work; + + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_osqp_memory; + +acados_size_t ocp_qp_osqp_opts_calculate_size(void *config, void *dims); +// +void *ocp_qp_osqp_opts_assign(void *config, void *dims, void *raw_memory); +// +void ocp_qp_osqp_opts_initialize_default(void *config, void *dims, void *opts_); +// +void ocp_qp_osqp_opts_update(void *config, void *dims, void *opts_); +// +acados_size_t ocp_qp_osqp_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *ocp_qp_osqp_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_osqp_workspace_calculate_size(void *config, void *dims, void *opts_); +// +int ocp_qp_osqp(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_osqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_osqp_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_OSQP_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h new file mode 100644 index 0000000..844f604 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_partial_condensing.h @@ -0,0 +1,120 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ +#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// hpipm +#include "hpipm/include/hpipm_d_ocp_qp_red.h" +// acados +#include "acados/ocp_qp/ocp_qp_common.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + ocp_qp_dims *red_dims; // dims of reduced qp + ocp_qp_dims *pcond_dims; + int *block_size; + int N2; + int N2_bkp; +} ocp_qp_partial_condensing_dims; + + + +typedef struct ocp_qp_partial_condensing_opts_ +{ + struct d_part_cond_qp_arg *hpipm_pcond_opts; + struct d_ocp_qp_reduce_eq_dof_arg *hpipm_red_opts; +// ocp_qp_dims *pcond_dims; // TODO(all): move to dims +// int *block_size; + int N2; + int N2_bkp; +// int expand_dual_sol; // 0 primal sol only, 1 primal + dual sol + int ric_alg; + int mem_qp_in; // allocate qp_in in memory +} ocp_qp_partial_condensing_opts; + + + +typedef struct ocp_qp_partial_condensing_memory_ +{ + struct d_part_cond_qp_ws *hpipm_pcond_work; + struct d_ocp_qp_reduce_eq_dof_ws *hpipm_red_work; + // in memory + ocp_qp_in *pcond_qp_in; + ocp_qp_out *pcond_qp_out; + ocp_qp_in *red_qp; // reduced qp + ocp_qp_out *red_sol; // reduced qp sol + // only pointer + ocp_qp_in *ptr_qp_in; + ocp_qp_in *ptr_pcond_qp_in; + qp_info *qp_out_info; // info in pcond_qp_in + double time_qp_xcond; +} ocp_qp_partial_condensing_memory; + + + +// +acados_size_t ocp_qp_partial_condensing_opts_calculate_size(void *dims); +// +void *ocp_qp_partial_condensing_opts_assign(void *dims, void *raw_memory); +// +void ocp_qp_partial_condensing_opts_initialize_default(void *dims, void *opts_); +// +void ocp_qp_partial_condensing_opts_update(void *dims, void *opts_); +// +void ocp_qp_partial_condensing_opts_set(void *opts_, const char *field, void* value); +// +acados_size_t ocp_qp_partial_condensing_memory_calculate_size(void *dims, void *opts_); +// +void *ocp_qp_partial_condensing_memory_assign(void *dims, void *opts, void *raw_memory); +// +acados_size_t ocp_qp_partial_condensing_workspace_calculate_size(void *dims, void *opts_); +// +int ocp_qp_partial_condensing(void *in, void *out, void *opts, void *mem, void *work); +// +int ocp_qp_partial_expansion(void *in, void *out, void *opts, void *mem, void *work); +// +void ocp_qp_partial_condensing_config_initialize_default(void *config_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h new file mode 100644 index 0000000..3b875ca --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_qpdunes.h @@ -0,0 +1,119 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_QPDUNES_H_ +#define ACADOS_OCP_QP_OCP_QP_QPDUNES_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "qpDUNES.h" + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + +typedef enum qpdunes_options_t_ { + QPDUNES_DEFAULT_ARGUMENTS, + QPDUNES_LINEAR_MPC, // TODO(dimitris): partly implemented + QPDUNES_NONLINEAR_MPC, // TODO(dimitris): not implemented yet + QPDUNES_ACADO_SETTINGS +} qpdunes_options_t; + +typedef enum { QPDUNES_WITH_QPOASES, QPDUNES_WITH_CLIPPING } qpdunes_stage_qp_solver_t; + +typedef struct ocp_qp_qpdunes_opts_ +{ + qpOptions_t options; + qpdunes_stage_qp_solver_t stageQpSolver; + int warmstart; // warmstart = 0: all multipliers set to zero, warmstart = 1: use previous mult. + bool isLinearMPC; +} ocp_qp_qpdunes_opts; + +typedef struct ocp_qp_qpdunes_memory_ +{ + int firstRun; + int nx; + int nu; + int nz; + int nDmax; // max(dims->ng) + qpData_t qpData; + double time_qp_solver_call; + int iter; + int status; + +} ocp_qp_qpdunes_memory; + +typedef struct ocp_qp_qpdunes_workspace_ +{ + double *H; + double *Q; + double *R; + double *S; + double *g; + double *ABt; + double *b; + double *Ct; + double *lc; + double *uc; + double *zLow; + double *zUpp; +} ocp_qp_qpdunes_workspace; + +// +acados_size_t ocp_qp_qpdunes_opts_calculate_size(void *config_, ocp_qp_dims *dims); +// +void *ocp_qp_qpdunes_opts_assign(void *config_, ocp_qp_dims *dims, void *raw_memory); +// +void ocp_qp_qpdunes_opts_initialize_default(void *config_, ocp_qp_dims *dims, void *opts_); +// +void ocp_qp_qpdunes_opts_update(void *config_, ocp_qp_dims *dims, void *opts_); +// +acados_size_t ocp_qp_qpdunes_memory_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +void *ocp_qp_qpdunes_memory_assign(void *config_, ocp_qp_dims *dims, void *opts_, void *raw_memory); +// +acados_size_t ocp_qp_qpdunes_workspace_calculate_size(void *config_, ocp_qp_dims *dims, void *opts_); +// +int ocp_qp_qpdunes(void *config_, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *memory_, + void *work_); +// +void ocp_qp_qpdunes_free_memory(void *mem_); +// +void ocp_qp_qpdunes_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_); +// +void ocp_qp_qpdunes_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_QPDUNES_H_ diff --git a/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h new file mode 100644 index 0000000..a78bc65 --- /dev/null +++ b/third_party/acados/include/acados/ocp_qp/ocp_qp_xcond_solver.h @@ -0,0 +1,150 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ +#define ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + ocp_qp_dims *orig_dims; + void *xcond_dims; +} ocp_qp_xcond_solver_dims; + + + +typedef struct ocp_qp_xcond_solver_opts_ +{ + void *xcond_opts; + void *qp_solver_opts; +} ocp_qp_xcond_solver_opts; + + + +typedef struct ocp_qp_xcond_solver_memory_ +{ + void *xcond_memory; + void *solver_memory; + void *xcond_qp_in; + void *xcond_qp_out; +} ocp_qp_xcond_solver_memory; + + + +typedef struct ocp_qp_xcond_solver_workspace_ +{ + void *xcond_work; + void *qp_solver_work; +} ocp_qp_xcond_solver_workspace; + + + +typedef struct +{ + acados_size_t (*dims_calculate_size)(void *config, int N); + ocp_qp_xcond_solver_dims *(*dims_assign)(void *config, int N, void *raw_memory); + void (*dims_set)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); + void (*dims_get)(void *config_, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); + acados_size_t (*opts_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims); + void *(*opts_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory); + void (*opts_initialize_default)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void (*opts_update)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void (*opts_set)(void *config_, void *opts_, const char *field, void* value); + acados_size_t (*memory_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + void *(*memory_assign)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts, void *raw_memory); + void (*memory_get)(void *config_, void *mem_, const char *field, void* value); + void (*memory_reset)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); + acados_size_t (*workspace_calculate_size)(void *config, ocp_qp_xcond_solver_dims *dims, void *opts); + int (*evaluate)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts, void *mem, void *work); + void (*eval_sens)(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *param_qp_in, ocp_qp_out *sens_qp_out, void *opts, void *mem, void *work); + qp_solver_config *qp_solver; // either ocp_qp_solver or dense_solver + ocp_qp_xcond_config *xcond; +} ocp_qp_xcond_solver_config; // pcond - partial condensing or fcond - full condensing + + + +/* config */ +// +acados_size_t ocp_qp_xcond_solver_config_calculate_size(); +// +ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_assign(void *raw_memory); + +/* dims */ +// +acados_size_t ocp_qp_xcond_solver_dims_calculate_size(void *config, int N); +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_assign(void *config, int N, void *raw_memory); +// +void ocp_qp_xcond_solver_dims_set_(void *config, ocp_qp_xcond_solver_dims *dims, int stage, const char *field, int* value); + +/* opts */ +// +acados_size_t ocp_qp_xcond_solver_opts_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims); +// +void *ocp_qp_xcond_solver_opts_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *raw_memory); +// +void ocp_qp_xcond_solver_opts_initialize_default(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void ocp_qp_xcond_solver_opts_update(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void ocp_qp_xcond_solver_opts_set_(void *config_, void *opts_, const char *field, void* value); + +/* memory */ +// +acados_size_t ocp_qp_xcond_solver_memory_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); +// +void *ocp_qp_xcond_solver_memory_assign(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_, void *raw_memory); + +/* workspace */ +// +acados_size_t ocp_qp_xcond_solver_workspace_calculate_size(void *config, ocp_qp_xcond_solver_dims *dims, void *opts_); + +/* config */ +// +int ocp_qp_xcond_solver(void *config, ocp_qp_xcond_solver_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, void *opts_, void *mem_, void *work_); + +// +void ocp_qp_xcond_solver_config_initialize_default(void *config_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_OCP_QP_OCP_QP_PARTIAL_CONDENSING_SOLVER_H_ diff --git a/third_party/acados/include/acados/sim/sim_collocation_utils.h b/third_party/acados/include/acados/sim/sim_collocation_utils.h new file mode 100644 index 0000000..045d165 --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_collocation_utils.h @@ -0,0 +1,97 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_COLLOCATION_UTILS_H_ +#define ACADOS_SIM_SIM_COLLOCATION_UTILS_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + + + +// enum Newton_type_collocation +// { +// exact = 0, +// simplified_in, +// simplified_inis +// }; + + + +// typedef struct +// { +// enum Newton_type_collocation type; +// double *eig; +// double *low_tria; +// bool single; +// bool freeze; + +// double *transf1; +// double *transf2; + +// double *transf1_T; +// double *transf2_T; +// } Newton_scheme; + + +typedef enum +{ + GAUSS_LEGENDRE, + GAUSS_RADAU_IIA, +} sim_collocation_type; + + +// +// acados_size_t gauss_legendre_nodes_work_calculate_size(int ns); +// +// void gauss_legendre_nodes(int ns, double *nodes, void *raw_memory); +// +// acados_size_t gauss_simplified_work_calculate_size(int ns); +// // +// void gauss_simplified(int ns, Newton_scheme *scheme, void *work); +// +acados_size_t butcher_tableau_work_calculate_size(int ns); +// +// void calculate_butcher_tableau_from_nodes(int ns, double *nodes, double *b, double *A, void *work); +// +void calculate_butcher_tableau(int ns, sim_collocation_type collocation_type, double *c_vec, + double *b_vec, double *A_mat, void *work); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_COLLOCATION_UTILS_H_ diff --git a/third_party/acados/include/acados/sim/sim_common.h b/third_party/acados/include/acados/sim/sim_common.h new file mode 100644 index 0000000..c4bbd6e --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_common.h @@ -0,0 +1,221 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_COMMON_H_ +#define ACADOS_SIM_SIM_COMMON_H_ + +#include + +#include "acados/sim/sim_collocation_utils.h" +#include "acados/utils/timing.h" +#include "acados/utils/types.h" + +#include "acados/utils/external_function_generic.h" + +// maximum number of integration stages +#define NS_MAX 15 + + + +typedef enum +{ + // ERK and LIFTED_ERK + EXPL_ODE_FUN, + EXPL_ODE_HES, // wrt x and u ??? + EXPL_VDE_FOR, + EXPL_VDE_ADJ, + // IRK + IMPL_ODE_FUN, + IMPL_ODE_FUN_JAC_X_XDOT, + IMPL_ODE_JAC_X_XDOT_U, + IMPL_ODE_FUN_JAC_X_XDOT_U, + IMPL_ODE_HESS, + // gnsf + PHI_FUN, + PHI_FUN_JAC_Y, + PHI_JAC_Y_UHAT, + LO_FUN, + GET_GNSF_MATRICES +} sim_function_t; + + + +typedef struct +{ + void *dims; + + double *x; // x[NX] - initial state value for simulation + double *u; // u[NU] - control - constant over simulation time + + double *S_forw; // forward seed [Sx, Su] + double *S_adj; // backward seed + + bool identity_seed; // indicating if S_forw = [eye(nx), zeros(nx x nu)] + + void *model; + + double T; // simulation time + +} sim_in; + + + +typedef struct +{ + double CPUtime; // in seconds + double LAtime; // in seconds + double ADtime; // in seconds + +} sim_info; + + + +typedef struct +{ + double *xn; // xn[NX] + double *S_forw; // S_forw[NX*(NX+NU)] + double *S_adj; // + double *S_hess; // + + double *zn; // z - algebraic variables - reported at start of simulation interval + double *S_algebraic; // sensitivities of reported value of algebraic variables w.r.t. + // initial stat & control (x_n,u) + + double *grad; // gradient correction + + sim_info *info; + +} sim_out; + + + +typedef struct +{ + int ns; // number of integration stages + + int num_steps; + int num_forw_sens; + + int tableau_size; // check that is consistent with ns + // only update when butcher tableau is changed + // kind of private -> no setter! + double *A_mat; + double *c_vec; + double *b_vec; + + bool sens_forw; + bool sens_adj; + bool sens_hess; + + bool output_z; // 1 -- if zn should be computed + bool sens_algebraic; // 1 -- if S_algebraic should be computed + bool exact_z_output; // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations + sim_collocation_type collocation_type; + + // for explicit integrators: newton_iter == 0 && scheme == NULL + // && jac_reuse=false + int newton_iter; + bool jac_reuse; + // Newton_scheme *scheme; + + double newton_tol; // optinally used in implicit integrators + + // workspace + void *work; + +} sim_opts; + + + +typedef struct +{ + int (*evaluate)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work); + int (*precompute)(void *config_, sim_in *in, sim_out *out, void *opts, void *mem, void *work); + // opts + acados_size_t (*opts_calculate_size)(void *config_, void *dims); + 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, void *value); + void (*opts_get)(void *config_, void *opts_, const char *field, void *value); + // mem + acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts); + void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory); + int (*memory_set)(void *config, void *dims, void *mem, const char *field, void *value); + int (*memory_set_to_zero)(void *config, void *dims, void *opts, void *mem, const char *field); + void (*memory_get)(void *config, void *dims, void *mem, const char *field, void *value); + // work + acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts); + // model + acados_size_t (*model_calculate_size)(void *config, void *dims); + void *(*model_assign)(void *config, void *dims, void *raw_memory); + int (*model_set)(void *model, const char *field, void *value); + // config + void (*config_initialize_default)(void *config); + // dims + acados_size_t (*dims_calculate_size)(); + void *(*dims_assign)(void *config, void *raw_memory); + void (*dims_set)(void *config, void *dims, const char *field, const int *value); + void (*dims_get)(void *config, void *dims, const char *field, int *value); + +} sim_config; + + + +/* config */ +// +acados_size_t sim_config_calculate_size(); +// +sim_config *sim_config_assign(void *raw_memory); + +/* in */ +// +acados_size_t sim_in_calculate_size(void *config, void *dims); +// +sim_in *sim_in_assign(void *config, void *dims, void *raw_memory); +// +int sim_in_set_(void *config_, void *dims_, sim_in *in, const char *field, void *value); + +/* out */ +// +acados_size_t sim_out_calculate_size(void *config, void *dims); +// +sim_out *sim_out_assign(void *config, void *dims, void *raw_memory); +// +int sim_out_get_(void *config, void *dims, sim_out *out, const char *field, void *value); + +/* opts */ +// +void sim_opts_set_(sim_opts *opts, const char *field, void *value); +// +void sim_opts_get_(sim_config *config, sim_opts *opts, const char *field, void *value); + +#endif // ACADOS_SIM_SIM_COMMON_H_ diff --git a/third_party/acados/include/acados/sim/sim_erk_integrator.h b/third_party/acados/include/acados/sim/sim_erk_integrator.h new file mode 100644 index 0000000..fd46cb4 --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_erk_integrator.h @@ -0,0 +1,140 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_ERK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_ERK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + + + +typedef struct +{ + int nx; + int nu; + int nz; +} sim_erk_dims; + + + +typedef struct +{ + /* external functions */ + // explicit ode + external_function_generic *expl_ode_fun; + // hessian explicit ode + external_function_generic *expl_ode_hes; + // forward explicit vde + external_function_generic *expl_vde_for; + // adjoint explicit vde + external_function_generic *expl_vde_adj; + +} erk_model; + + + +typedef struct +{ + // memory + double time_sim; + double time_ad; + double time_la; + + // workspace structs +} sim_erk_memory; + + + +typedef struct +{ + // workspace mem + double *rhs_forw_in; // x + S + p + + double *K_traj; // (stages*nX) or (steps*stages*nX) for adj + double *out_forw_traj; // S or (steps+1)*nX for adj + + double *rhs_adj_in; + double *out_adj_tmp; + double *adj_traj; + +} sim_erk_workspace; + + + +// dims +acados_size_t sim_erk_dims_calculate_size(); +void *sim_erk_dims_assign(void *config_, void *raw_memory); +void sim_erk_dims_set(void *config_, void *dims_, const char *field, const int* value); +void sim_erk_dims_get(void *config_, void *dims_, const char *field, int* value); + +// model +acados_size_t sim_erk_model_calculate_size(void *config, void *dims); +void *sim_erk_model_assign(void *config, void *dims, void *raw_memory); +int sim_erk_model_set(void *model, const char *field, void *value); + +// opts +acados_size_t sim_erk_opts_calculate_size(void *config, void *dims); +// +void sim_erk_opts_update(void *config_, void *dims, void *opts_); +// +void *sim_erk_opts_assign(void *config, void *dims, void *raw_memory); +// +void sim_erk_opts_initialize_default(void *config, void *dims, void *opts_); +// +void sim_erk_opts_set(void *config_, void *opts_, const char *field, void *value); + + +// memory +acados_size_t sim_erk_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *sim_erk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +// +int sim_erk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value); + + +// workspace +acados_size_t sim_erk_workspace_calculate_size(void *config, void *dims, void *opts_); + +// +int sim_erk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_); +// +void sim_erk_config_initialize_default(void *config); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_ERK_INTEGRATOR_H_ diff --git a/third_party/acados/include/acados/sim/sim_gnsf.h b/third_party/acados/include/acados/sim/sim_gnsf.h new file mode 100644 index 0000000..404532a --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_gnsf.h @@ -0,0 +1,364 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_GNSF_H_ +#define ACADOS_SIM_SIM_GNSF_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include + +#include "acados/utils/timing.h" +#include "acados/utils/types.h" +#include "acados/sim/sim_common.h" + +#include "blasfeo/include/blasfeo_common.h" +// #include "blasfeo/include/blasfeo_d_aux.h" +// #include "blasfeo/include/blasfeo_d_aux_ext_dep.h" +// #include "blasfeo/include/blasfeo_d_blas.h" +// #include "blasfeo/include/blasfeo_d_kernel.h" +// #include "blasfeo/include/blasfeo_i_aux_ext_dep.h" +// #include "blasfeo/include/blasfeo_target.h" + +/* +GNSF - Generalized Nonlinear Static Feedback Model +has the following form +https://github.com/acados/acados/files/3359595/gnsf_structure_blo.pdf + +Details on the algorithm can be found in master thesis of Jonathan Frey, +which presents a slightly different format without the terms B_LO, c_LO. +https://github.com/acados/acados/files/2318322/gnsf_structure.pdf +https://cdn.syscop.de/publications/Frey2018.pdf +https://cdn.syscop.de/publications/Frey2019.pdf +*/ + +typedef struct +{ + int nx; // total number of differential states + int nu; // total number of inputs + int nz; // total number of algebraic states + int nx1; // number of differential states in NSF part + int nz1; // number of algebraic states in NSF part + int n_out; // output dimension of phi + int ny; // dimension of first input of phi + int nuhat; // dimension of second input of phi + +} sim_gnsf_dims; + + + +typedef struct +{ + /* external functions */ + // phi: nonlinearity function + external_function_generic *phi_fun; + external_function_generic *phi_fun_jac_y; + external_function_generic *phi_jac_y_uhat; + + // f_lo: linear output function + external_function_generic *f_lo_fun_jac_x1_x1dot_u_z; + + // to import model matrices + external_function_generic *get_gnsf_matrices; + + // flag indicating, if model defining matrices are imported via external (casadi) function, + // [default]: true -> auto; + bool auto_import_gnsf; + + // booleans from structure detection + bool nontrivial_f_LO; // indicates if f_LO is constant zero function + bool fully_linear; // indicates if model is fully linear LOS + + /* model defining matrices */ + // TODO: add setters to set manually + double *A; + double *B; + double *C; + double *E; + + double *L_x; + double *L_xdot; + double *L_z; + double *L_u; + + double *A_LO; + double *B_LO; + double *E_LO; + + /* constant vector */ + double *c; + double *c_LO; + + // permutation vector - to have GNSF order of x, z within sim_gnsf only + int *ipiv_x; + int *ipiv_z; + + double *ipiv_x_double; + double *ipiv_z_double; + +} gnsf_model; + + + +// pre_workspace - workspace used in the precomputation phase +typedef struct +{ + struct blasfeo_dmat E11; + struct blasfeo_dmat E12; + struct blasfeo_dmat E21; + struct blasfeo_dmat E22; + + struct blasfeo_dmat A1; + struct blasfeo_dmat A2; + struct blasfeo_dmat B1; + struct blasfeo_dmat B2; + struct blasfeo_dmat C1; + struct blasfeo_dmat C2; + + struct blasfeo_dmat AA1; + struct blasfeo_dmat AA2; + struct blasfeo_dmat BB1; + struct blasfeo_dmat BB2; + struct blasfeo_dmat CC1; + struct blasfeo_dmat CC2; + struct blasfeo_dmat DD1; + struct blasfeo_dmat DD2; + struct blasfeo_dmat EE1; + struct blasfeo_dmat EE2; + + struct blasfeo_dmat QQ1; + + struct blasfeo_dmat LLZ; + struct blasfeo_dmat LLx; + struct blasfeo_dmat LLK; + + int *ipivEE1; // index of pivot vector + int *ipivEE2; + int *ipivQQ1; + + // for algebraic sensitivity propagation + struct blasfeo_dmat Q1; + + // for constant term in NSF + struct blasfeo_dvec cc1; + struct blasfeo_dvec cc2; + +} gnsf_pre_workspace; + + + +// workspace +typedef struct +{ + double *Z_work; // used to perform computations to get out->zn + + int *ipiv; // index of pivot vector + + struct blasfeo_dvec *vv_traj; + struct blasfeo_dvec *yy_traj; + struct blasfeo_dmat *f_LO_jac_traj; + + struct blasfeo_dvec K2_val; + struct blasfeo_dvec x0_traj; + struct blasfeo_dvec res_val; + struct blasfeo_dvec u0; + struct blasfeo_dvec lambda; + struct blasfeo_dvec lambda_old; + + struct blasfeo_dvec yyu; + struct blasfeo_dvec yyss; + + struct blasfeo_dvec K1_val; + struct blasfeo_dvec f_LO_val; + struct blasfeo_dvec x1_stage_val; + struct blasfeo_dvec Z1_val; + + struct blasfeo_dvec K1u; + struct blasfeo_dvec Zu; + struct blasfeo_dvec ALOtimesx02; + struct blasfeo_dvec BLOtimesu0; + + struct blasfeo_dvec uhat; + + struct blasfeo_dmat J_r_vv; + struct blasfeo_dmat J_r_x1u; + + struct blasfeo_dmat dK1_dx1; + struct blasfeo_dmat dK1_du; + struct blasfeo_dmat dZ_dx1; + struct blasfeo_dmat dZ_du; + struct blasfeo_dmat J_G2_K1; + + struct blasfeo_dmat dK2_dx1; + struct blasfeo_dmat dK2_dvv; + struct blasfeo_dmat dxf_dwn; + struct blasfeo_dmat S_forw_new; + struct blasfeo_dmat S_algebraic_aux; + + struct blasfeo_dmat dPsi_dvv; + struct blasfeo_dmat dPsi_dx; + struct blasfeo_dmat dPsi_du; + + struct blasfeo_dmat dPHI_dyuhat; + struct blasfeo_dvec z0; + + // memory only available if (opts->sens_algebraic) + // struct blasfeo_dvec y_one_stage; + // struct blasfeo_dvec x0dot_1; + // struct blasfeo_dmat dz10_dx1u; // (nz1) * (nx1+nu); + // struct blasfeo_dmat dr0_dvv0; // (n_out * n_out) + // struct blasfeo_dmat f_LO_jac0; // (nx2+nz2) * (2*nx1 + nz1 + nu) + // struct blasfeo_dmat sens_z2_rhs; // (nx2 + nz2) * (nx1 + nu) + // int *ipiv_vv0; + +} gnsf_workspace; + + + +// memory +typedef struct +{ + bool first_call; + + // simulation time for one step + double dt; + + // (scaled) butcher table + double *A_dt; + double *b_dt; + double *c_butcher; + + // value used to initialize integration variables - corresponding to value of phi + double *phi_guess; // n_out + + struct blasfeo_dmat S_forw; + struct blasfeo_dmat S_algebraic; + + // precomputed matrices + struct blasfeo_dmat KKv; + struct blasfeo_dmat KKx; + struct blasfeo_dmat KKu; + + struct blasfeo_dmat YYv; + struct blasfeo_dmat YYx; + struct blasfeo_dmat YYu; + + struct blasfeo_dmat ZZv; + struct blasfeo_dmat ZZx; + struct blasfeo_dmat ZZu; + + struct blasfeo_dmat ALO; + struct blasfeo_dmat BLO; + struct blasfeo_dmat M2_LU; + int *ipivM2; + + struct blasfeo_dmat dK2_dx2; + struct blasfeo_dmat dK2_du; + struct blasfeo_dmat dx2f_dx2u; + + struct blasfeo_dmat Lu; + + // precomputed vectors for constant term in NSF + struct blasfeo_dvec KK0; + struct blasfeo_dvec YY0; + struct blasfeo_dvec ZZ0; + + // for algebraic sensitivities only; + // struct blasfeo_dmat *Z0x; + // struct blasfeo_dmat *Z0u; + // struct blasfeo_dmat *Z0v; + + // struct blasfeo_dmat *Y0x; + // struct blasfeo_dmat *Y0u; + // struct blasfeo_dmat *Y0v; + + // struct blasfeo_dmat *K0x; + // struct blasfeo_dmat *K0u; + // struct blasfeo_dmat *K0v; + + // struct blasfeo_dmat *ELO_LU; + // int *ipiv_ELO; + // struct blasfeo_dmat *ELO_inv_ALO; + + // struct blasfeo_dmat *Lx; + // struct blasfeo_dmat *Lxdot; + // struct blasfeo_dmat *Lz; + + double time_sim; + double time_ad; + double time_la; + +} sim_gnsf_memory; + + + +// gnsf dims +acados_size_t sim_gnsf_dims_calculate_size(); +void *sim_gnsf_dims_assign(void *config_, void *raw_memory); + +// get & set functions +void sim_gnsf_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_gnsf_dims_get(void *config_, void *dims_, const char *field, int* value); + +// opts +acados_size_t sim_gnsf_opts_calculate_size(void *config, void *dims); +void *sim_gnsf_opts_assign(void *config, void *dims, void *raw_memory); +void sim_gnsf_opts_initialize_default(void *config, void *dims, void *opts_); +void sim_gnsf_opts_update(void *config_, void *dims, void *opts_); +void sim_gnsf_opts_set(void *config_, void *opts_, const char *field, void *value); + +// model +acados_size_t sim_gnsf_model_calculate_size(void *config, void *dims_); +void *sim_gnsf_model_assign(void *config, void *dims_, void *raw_memory); +int sim_gnsf_model_set(void *model_, const char *field, void *value); + +// precomputation +int sim_gnsf_precompute(void *config_, sim_in *in, sim_out *out, void *opts_, void *mem_, + void *work_); + +// workspace & memory +acados_size_t sim_gnsf_workspace_calculate_size(void *config, void *dims_, void *args); +acados_size_t sim_gnsf_memory_calculate_size(void *config, void *dims_, void *opts_); +void *sim_gnsf_memory_assign(void *config, void *dims_, void *opts_, void *raw_memory); + +// interface +void sim_gnsf_config_initialize_default(void *config_); + +// integrator +int sim_gnsf(void *config, sim_in *in, sim_out *out, void *opts, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_GNSF_H_ diff --git a/third_party/acados/include/acados/sim/sim_irk_integrator.h b/third_party/acados/include/acados/sim/sim_irk_integrator.h new file mode 100644 index 0000000..5090aa0 --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_irk_integrator.h @@ -0,0 +1,183 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_IRK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_IRK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + +#include "blasfeo/include/blasfeo_common.h" + +typedef struct +{ + int nx; + int nu; + int nz; + +} sim_irk_dims; + + + +typedef struct +{ + /* external functions */ + // implicit fun - can either be fully implicit ode or dae + // - i.e. dae has z as additional last argument & nz > 0 + external_function_generic *impl_ode_fun; + // implicit ode & jac_x & jax_xdot & jac_z + external_function_generic *impl_ode_fun_jac_x_xdot_z; + // jax_x & jac_xdot & jac_u & jac_z of implicit ode + external_function_generic *impl_ode_jac_x_xdot_u_z; + // hessian of implicit ode: + external_function_generic *impl_ode_hess; +} irk_model; + + + +typedef struct +{ + struct blasfeo_dvec *rG; // residuals of G (nx*ns) + struct blasfeo_dvec *K; // internal K variables ((nx+nz)*ns) + struct blasfeo_dvec *xt; // temporary x + struct blasfeo_dvec *xn; // x at each integration step + struct blasfeo_dvec xtdot; // temporary xdot + + struct blasfeo_dvec *lambda; // adjoint sensitivities (nx + nu) + struct blasfeo_dvec *lambdaK; // auxiliary variable ((nx+nz)*ns) for adjoint propagation + + struct blasfeo_dmat df_dx; // temporary Jacobian of ode w.r.t x (nx+nz, nx) + struct blasfeo_dmat df_dxdot; // temporary Jacobian of ode w.r.t xdot (nx+nz, nx) + struct blasfeo_dmat df_du; // temporary Jacobian of ode w.r.t u (nx+nz, nu) + struct blasfeo_dmat df_dz; // temporary Jacobian of ode w.r.t z (nx+nz, nu) + + /* NOTE: the memory allocation corresponding to the following fields is CONDITIONAL */ + + // only allocated if (opts->sens_algebraic || opts->output_z) + int *ipiv_one_stage; // index of pivot vector (nx + nz) + double *Z_work; // used to perform computations to get out->zn (ns) + + // df_dxdotz, dk0_dxu, only allocated if (opts->sens_algebraic && opts->exact_z_output) + // used for algebraic sensitivity generation + struct blasfeo_dmat df_dxdotz; // temporary Jacobian of ode w.r.t. xdot,z (nx+nz, nx+nz); + struct blasfeo_dmat dk0_dxu; // intermediate result, (nx+nz, nx+nu) + + // dK_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of (num_steps) blasfeo_dmat + // to store intermediate results + struct blasfeo_dmat *dK_dxu; // jacobian of (K,Z) over x and u ((nx+nz)*ns, nx+nu); + + // S_forw: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of (num_steps + 1) blasfeo_dmat + // to store intermediate results + struct blasfeo_dmat *S_forw; // forward sensitivities (nx, nx+nu) + + // dG_dxu: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results + struct blasfeo_dmat *dG_dxu; // jacobian of G over x and u ((nx+nz)*ns, nx+nu) + + // dG_dK: if (!opts->sens_hess) - single blasfeo_dmat that is reused + // if ( opts->sens_hess) - array of blasfeo_dmat to store intermediate results + struct blasfeo_dmat *dG_dK; // jacobian of G over K ((nx+nz)*ns, (nx+nz)*ns) + + // ipiv: index of pivot vector + // if (!opts->sens_hess) - array (ns * (nx + nz)) that is reused + // if ( opts->sens_hess) - array (ns * (nx + nz)) * num_steps, to store all + // pivot vectors for dG_dxu + int *ipiv; // index of pivot vector + + // xn_traj, K_traj only available if( opts->sens_adj || opts->sens_hess ) + struct blasfeo_dvec *xn_traj; // xn trajectory + struct blasfeo_dvec *K_traj; // K trajectory + + /* the following variables are only available if (opts->sens_hess) */ + // For Hessian propagation + struct blasfeo_dmat Hess; // temporary Hessian (nx + nu, nx + nu) + // output of impl_ode_hess + struct blasfeo_dmat f_hess; // size: (nx + nu, nx + nu) + struct blasfeo_dmat dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu) + struct blasfeo_dmat tmp_dxkzu_dw0; // size (2*nx + nu + nz) x (nx + nu) + +} sim_irk_workspace; + + +typedef struct +{ + double *xdot; // xdot[NX] - initialization for state derivatives k within the integrator + double *z; // z[NZ] - initialization for algebraic variables z + + double time_sim; + double time_ad; + double time_la; +} sim_irk_memory; + + +// get & set functions +void sim_irk_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_irk_dims_get(void *config_, void *dims_, const char *field, int* value); + +// dims +acados_size_t sim_irk_dims_calculate_size(); +void *sim_irk_dims_assign(void *config_, void *raw_memory); + +// model +acados_size_t sim_irk_model_calculate_size(void *config, void *dims); +void *sim_irk_model_assign(void *config, void *dims, void *raw_memory); +int sim_irk_model_set(void *model, const char *field, void *value); + +// opts +acados_size_t sim_irk_opts_calculate_size(void *config, void *dims); +void *sim_irk_opts_assign(void *config, void *dims, void *raw_memory); +void sim_irk_opts_initialize_default(void *config, void *dims, void *opts_); +void sim_irk_opts_update(void *config_, void *dims, void *opts_); +void sim_irk_opts_set(void *config_, void *opts_, const char *field, void *value); + +// memory +acados_size_t sim_irk_memory_calculate_size(void *config, void *dims, void *opts_); +void *sim_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); +int sim_irk_memory_set(void *config_, void *dims_, void *mem_, const char *field, void *value); + +// workspace +acados_size_t sim_irk_workspace_calculate_size(void *config, void *dims, void *opts_); +void sim_irk_config_initialize_default(void *config); + +// main +int sim_irk(void *config, sim_in *in, sim_out *out, void *opts_, void *mem_, void *work_); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_IRK_INTEGRATOR_H_ diff --git a/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h new file mode 100644 index 0000000..e60bb80 --- /dev/null +++ b/third_party/acados/include/acados/sim/sim_lifted_irk_integrator.h @@ -0,0 +1,157 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ +#define ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" +#include "acados/utils/types.h" + +typedef struct +{ + int nx; + int nu; + int nz; +} sim_lifted_irk_dims; + + + +typedef struct +{ + /* external functions */ + // implicit ode + external_function_generic *impl_ode_fun; + // implicit ode & jax_x & jac_xdot & jac_u implicit ode + external_function_generic *impl_ode_fun_jac_x_xdot_u; + +} lifted_irk_model; + + + +typedef struct +{ + + struct blasfeo_dmat *J_temp_x; // temporary Jacobian of ode w.r.t x (nx, nx) + struct blasfeo_dmat *J_temp_xdot; // temporary Jacobian of ode w.r.t xdot (nx, nx) + struct blasfeo_dmat *J_temp_u; // temporary Jacobian of ode w.r.t u (nx, nu) + + struct blasfeo_dvec *rG; // residuals of G (nx*ns) + struct blasfeo_dvec *xt; // temporary x + struct blasfeo_dvec *xn; // x at each integration step (for evaluations) + struct blasfeo_dvec *xn_out; // x at each integration step (output) + struct blasfeo_dvec *dxn; // dx at each integration step + struct blasfeo_dvec *w; // stacked x and u + + int *ipiv; // index of pivot vector + +} sim_lifted_irk_workspace; + + + +typedef struct +{ + // memory for lifted integrators + struct blasfeo_dmat *S_forw; // forward sensitivities + struct blasfeo_dmat *JGK; // jacobian of G over K (nx*ns, nx*ns) + struct blasfeo_dmat *JGf; // jacobian of G over x and u (nx*ns, nx+nu); + struct blasfeo_dmat *JKf; // jacobian of K over x and u (nx*ns, nx+nu); + + struct blasfeo_dvec *K; // internal variables (nx*ns) + struct blasfeo_dvec *x; // states (nx) -- for expansion step + struct blasfeo_dvec *u; // controls (nu) -- for expansion step + + int update_sens; + // int init_K; + + double time_sim; + double time_ad; + double time_la; + +} sim_lifted_irk_memory; + + + +/* dims */ +void sim_lifted_irk_dims_set(void *config_, void *dims_, const char *field, const int *value); +void sim_lifted_irk_dims_get(void *config_, void *dims_, const char *field, int* value); + +acados_size_t sim_lifted_irk_dims_calculate_size(); +// +void *sim_lifted_irk_dims_assign(void* config_, void *raw_memory); + +/* model */ +// +acados_size_t sim_lifted_irk_model_calculate_size(void *config, void *dims); +// +void *sim_lifted_irk_model_assign(void *config, void *dims, void *raw_memory); +// +int sim_lifted_irk_model_set(void *model_, const char *field, void *value); + +/* opts */ +// +acados_size_t sim_lifted_irk_opts_calculate_size(void *config, void *dims); +// +void *sim_lifted_irk_opts_assign(void *config, void *dims, void *raw_memory); +// +void sim_lifted_irk_opts_initialize_default(void *config, void *dims, void *opts_); +// +void sim_lifted_irk_opts_update(void *config_, void *dims, void *opts_); +// +void sim_lifted_irk_opts_set(void *config_, void *opts_, const char *field, void *value); + +/* memory */ +// +acados_size_t sim_lifted_irk_memory_calculate_size(void *config, void *dims, void *opts_); +// +void *sim_lifted_irk_memory_assign(void *config, void *dims, void *opts_, void *raw_memory); + +/* workspace */ +// +acados_size_t sim_lifted_irk_workspace_calculate_size(void *config, void *dims, void *opts_); +// +void sim_lifted_irk_config_initialize_default(void *config); + +/* solver */ +// +int sim_lifted_irk(void *config, sim_in *in, sim_out *out, void *opts_, + void *mem_, void *work_); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_SIM_SIM_LIFTED_IRK_INTEGRATOR_H_ diff --git a/third_party/acados/include/acados/utils/external_function_generic.h b/third_party/acados/include/acados/utils/external_function_generic.h new file mode 100644 index 0000000..1e68dc1 --- /dev/null +++ b/third_party/acados/include/acados/utils/external_function_generic.h @@ -0,0 +1,242 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ +#define ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + +/************************************************ + * generic external function + ************************************************/ + +// type of arguments +typedef enum { + COLMAJ, + BLASFEO_DMAT, + BLASFEO_DVEC, + COLMAJ_ARGS, + BLASFEO_DMAT_ARGS, + BLASFEO_DVEC_ARGS, + IGNORE_ARGUMENT +} ext_fun_arg_t; + +struct colmaj_args +{ + double *A; + int lda; +}; + +struct blasfeo_dmat_args +{ + struct blasfeo_dmat *A; + int ai; + int aj; +}; + +struct blasfeo_dvec_args +{ + struct blasfeo_dvec *x; + int xi; +}; + +// prototype of an external function +typedef struct +{ + // public members (have to be before private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // private members + // ..... +} external_function_generic; + + + +/************************************************ + * generic external parametric function + ************************************************/ + +// prototype of a parametric external function +typedef struct +{ + // public members for core (have to be before private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // public members for interfaces + void (*get_nparam)(void *, int *); + void (*set_param)(void *, double *); + void (*set_param_sparse)(void *, int n_update, int *idx, double *); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*fun)(void **, void **, void *); + double *p; // parameters + int np; // number of parameters + // ..... +} external_function_param_generic; + +// +acados_size_t external_function_param_generic_struct_size(); +// +void external_function_param_generic_set_fun(external_function_param_generic *fun, void *value); +// +acados_size_t external_function_param_generic_calculate_size(external_function_param_generic *fun, int np); +// +void external_function_param_generic_assign(external_function_param_generic *fun, void *mem); +// +void external_function_param_generic_wrapper(void *self, ext_fun_arg_t *type_in, void **in, ext_fun_arg_t *type_out, void **out); +// +void external_function_param_generic_get_nparam(void *self, int *np); +// +void external_function_param_generic_set_param(void *self, double *p); + + +/************************************************ + * casadi external function + ************************************************/ + +typedef struct +{ + // public members (have to be the same as in the prototype, and before the private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*casadi_fun)(const double **, double **, int *, double *, void *); + int (*casadi_work)(int *, int *, int *, int *); + const int *(*casadi_sparsity_in)(int); + const int *(*casadi_sparsity_out)(int); + int (*casadi_n_in)(void); + int (*casadi_n_out)(void); + double **args; + double **res; + double *w; + int *iw; + int *args_size; // size of args[i] + int *res_size; // size of res[i] + int args_num; // number of args arrays + int args_size_tot; // total size of args arrays + int res_num; // number of res arrays + int res_size_tot; // total size of res arrays + int in_num; // number of input arrays + int out_num; // number of output arrays + int iw_size; // number of ints for worksapce + int w_size; // number of doubles for workspace +} external_function_casadi; + +// +acados_size_t external_function_casadi_struct_size(); +// +void external_function_casadi_set_fun(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_work(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_sparsity_in(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_sparsity_out(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_n_in(external_function_casadi *fun, void *value); +// +void external_function_casadi_set_n_out(external_function_casadi *fun, void *value); +// +acados_size_t external_function_casadi_calculate_size(external_function_casadi *fun); +// +void external_function_casadi_assign(external_function_casadi *fun, void *mem); +// +void external_function_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in, + ext_fun_arg_t *type_out, void **out); + +/************************************************ + * casadi external parametric function + ************************************************/ + +typedef struct +{ + // public members for core (have to be the same as in the prototype, and before the private ones) + void (*evaluate)(void *, ext_fun_arg_t *, void **, ext_fun_arg_t *, void **); + // public members for interfaces + void (*get_nparam)(void *, int *); + void (*set_param)(void *, double *); + void (*set_param_sparse)(void *, int n_update, int *idx, double *); + // private members + void *ptr_ext_mem; // pointer to external memory + int (*casadi_fun)(const double **, double **, int *, double *, void *); + int (*casadi_work)(int *, int *, int *, int *); + const int *(*casadi_sparsity_in)(int); + const int *(*casadi_sparsity_out)(int); + int (*casadi_n_in)(void); + int (*casadi_n_out)(void); + double **args; + double **res; + double *w; + int *iw; + int *args_size; // size of args[i] + int *res_size; // size of res[i] + int args_num; // number of args arrays + int args_size_tot; // total size of args arrays + int res_num; // number of res arrays + int res_size_tot; // total size of res arrays + int in_num; // number of input arrays + int out_num; // number of output arrays + int iw_size; // number of ints for worksapce + int w_size; // number of doubles for workspace + int np; // number of parameters +} external_function_param_casadi; + +// +acados_size_t external_function_param_casadi_struct_size(); +// +void external_function_param_casadi_set_fun(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_work(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_sparsity_in(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_sparsity_out(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_n_in(external_function_param_casadi *fun, void *value); +// +void external_function_param_casadi_set_n_out(external_function_param_casadi *fun, void *value); +// +acados_size_t external_function_param_casadi_calculate_size(external_function_param_casadi *fun, int np); +// +void external_function_param_casadi_assign(external_function_param_casadi *fun, void *mem); +// +void external_function_param_casadi_wrapper(void *self, ext_fun_arg_t *type_in, void **in, + ext_fun_arg_t *type_out, void **out); +// +void external_function_param_casadi_get_nparam(void *self, int *np); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_EXTERNAL_FUNCTION_GENERIC_H_ diff --git a/third_party/acados/include/acados/utils/math.h b/third_party/acados/include/acados/utils/math.h new file mode 100644 index 0000000..7156a82 --- /dev/null +++ b/third_party/acados/include/acados/utils/math.h @@ -0,0 +1,103 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + +#ifndef ACADOS_UTILS_MATH_H_ +#define ACADOS_UTILS_MATH_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/types.h" + +#if defined(__MABX2__) +double fmax(double a, double b); +int isnan(double x); +#endif + +#define MIN(a,b) (((a)<(b))?(a):(b)) +#define MAX(a,b) (((a)>(b))?(a):(b)) + +void dgemm_nn_3l(int m, int n, int k, double *A, int lda, double *B, int ldb, double *C, int ldc); +// void dgemv_n_3l(int m, int n, double *A, int lda, double *x, double *y); +// void dgemv_t_3l(int m, int n, double *A, int lda, double *x, double *y); +// void dcopy_3l(int n, double *x, int incx, double *y, int incy); +void daxpy_3l(int n, double da, double *dx, double *dy); +void dscal_3l(int n, double da, double *dx); +double twonormv(int n, double *ptrv); + +/* copies a matrix into another matrix */ +void dmcopy(int row, int col, double *ptrA, int lda, double *ptrB, int ldb); + +/* solution of a system of linear equations */ +void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info); + +/* matrix exponential */ +void expm(int row, double *A); + +int idamax_3l(int n, double *x); + +void dswap_3l(int n, double *x, int incx, double *y, int incy); + +void dger_3l(int m, int n, double alpha, double *x, int incx, double *y, int incy, double *A, + int lda); + +void dgetf2_3l(int m, int n, double *A, int lda, int *ipiv, int *info); + +void dlaswp_3l(int n, double *A, int lda, int k1, int k2, int *ipiv); + +void dtrsm_l_l_n_u_3l(int m, int n, double *A, int lda, double *B, int ldb); + +void dgetrs_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb); + +void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info); + +double onenorm(int row, int col, double *ptrA); + +// double twonormv(int n, double *ptrv); + +void padeapprox(int m, int row, double *A); + +void expm(int row, double *A); + +// void d_compute_qp_size_ocp2dense_rev(int N, int *nx, int *nu, int *nb, int **hidxb, int *ng, +// int *nvd, int *ned, int *nbd, int *ngd); + +void acados_eigen_decomposition(int dim, double *A, double *V, double *d, double *e); + +double minimum_of_doubles(double *x, int n); + +void neville_algorithm(double xx, int n, double *x, double *Q, double *out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_MATH_H_ diff --git a/third_party/acados/include/acados/utils/mem.h b/third_party/acados/include/acados/utils/mem.h new file mode 100644 index 0000000..681a371 --- /dev/null +++ b/third_party/acados/include/acados/utils/mem.h @@ -0,0 +1,110 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_MEM_H_ +#define ACADOS_UTILS_MEM_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#include "types.h" + +// blasfeo +#include "blasfeo/include/blasfeo_d_aux.h" +#include "blasfeo/include/blasfeo_d_aux_ext_dep.h" + +// TODO(dimitris): probably does not belong here +typedef struct +{ + int (*fun)(void *); + acados_size_t (*calculate_args_size)(void *); + void *(*assign_args)(void *); + void (*initialize_default_args)(void *); + acados_size_t (*calculate_memory_size)(void *); + void *(*assign_memory)(void *); + acados_size_t (*calculate_workspace_size)(void *); +} module_solver; + +// make int counter of memory multiple of a number (typically 8 or 64) +void make_int_multiple_of(acados_size_t num, acados_size_t *size); + +// align char pointer to number (typically 8 for pointers and doubles, +// 64 for blasfeo structs) and return offset +int align_char_to(int num, char **c_ptr); + +// switch between malloc and calloc (for valgrinding) +void *acados_malloc(size_t nitems, acados_size_t size); + +// uses always calloc +void *acados_calloc(size_t nitems, acados_size_t size); + +// allocate vector of pointers to vectors of doubles and advance pointer +void assign_and_advance_double_ptrs(int n, double ***v, char **ptr); + +// allocate vector of pointers to vectors of ints and advance pointer +void assign_and_advance_int_ptrs(int n, int ***v, char **ptr); + +// allocate vector of pointers to strvecs and advance pointer +void assign_and_advance_blasfeo_dvec_structs(int n, struct blasfeo_dvec **sv, char **ptr); + +// allocate vector of pointers to strmats and advance pointer +void assign_and_advance_blasfeo_dmat_structs(int n, struct blasfeo_dmat **sm, char **ptr); + +// allocate vector of pointers to vector of pointers to strmats and advance pointer +void assign_and_advance_blasfeo_dmat_ptrs(int n, struct blasfeo_dmat ***sm, char **ptr); + +// allocate vector of chars and advance pointer +void assign_and_advance_char(int n, char **v, char **ptr); + +// allocate vector of ints and advance pointer +void assign_and_advance_int(int n, int **v, char **ptr); + +// allocate vector of bools and advance pointer +void assign_and_advance_bool(int n, bool **v, char **ptr); + +// allocate vector of doubles and advance pointer +void assign_and_advance_double(int n, double **v, char **ptr); + +// allocate strvec and advance pointer +void assign_and_advance_blasfeo_dvec_mem(int n, struct blasfeo_dvec *sv, char **ptr); + +// allocate strmat and advance pointer +void assign_and_advance_blasfeo_dmat_mem(int m, int n, struct blasfeo_dmat *sA, char **ptr); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_MEM_H_ diff --git a/third_party/acados/include/acados/utils/print.h b/third_party/acados/include/acados/utils/print.h new file mode 100644 index 0000000..824d3ce --- /dev/null +++ b/third_party/acados/include/acados/utils/print.h @@ -0,0 +1,110 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_PRINT_H_ +#define ACADOS_UTILS_PRINT_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_common_frontend.h" +#include "acados/utils/types.h" + +// void print_matrix(char *file_name, const real_t *matrix, const int_t nrows, const int_t ncols); + +// void print_matrix_name(char *file_name, char *name, const real_t *matrix, const int_t nrows, +// const int_t ncols); + +// void print_int_matrix(char *file_name, const int_t *matrix, const int_t nrows, const int_t ncols); + +// void print_array(char *file_name, real_t *array, int_t size); + +// void print_int_array(char *file_name, const int_t *array, int_t size); + +void read_matrix(const char *file_name, real_t *array, const int_t nrows, const int_t ncols); + +void write_double_vector_to_txt(real_t *vec, int_t n, const char *fname); + +// ocp nlp +// TODO(andrea): inconsistent naming +void ocp_nlp_dims_print(ocp_nlp_dims *dims); +// TODO(andrea): inconsistent naming +void ocp_nlp_out_print(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out); +// TODO(andrea): inconsistent naming +void ocp_nlp_res_print(ocp_nlp_dims *dims, ocp_nlp_res *nlp_res); + +// ocp qp +// TODO: move printing routines below that print qp structures to HPIPM! +void print_ocp_qp_dims(ocp_qp_dims *dims); + +// void print_dense_qp_dims(dense_qp_dims *dims); + +void print_ocp_qp_in(ocp_qp_in *qp_in); + +void print_ocp_qp_in_to_file(FILE *file, ocp_qp_in *qp_in); + +void print_ocp_qp_out(ocp_qp_out *qp_out); + +void print_ocp_qp_out_to_file(FILE *file, ocp_qp_out *qp_out); + +void print_ocp_qp_res(ocp_qp_res *qp_res); + +void print_dense_qp_in(dense_qp_in *qp_in); +// void print_ocp_qp_in_to_string(char string_out[], ocp_qp_in *qp_in); + +// void print_ocp_qp_out_to_string(char string_out[], ocp_qp_out *qp_out); + +// void print_colmaj_ocp_qp_in(colmaj_ocp_qp_in *qp); + +// void print_colmaj_ocp_qp_in_to_file(colmaj_ocp_qp_in *qp); + +// void print_colmaj_ocp_qp_out(char *filename, colmaj_ocp_qp_in *qp, colmaj_ocp_qp_out *out); + +void print_qp_info(qp_info *info); + +// void acados_warning(char warning_string[]); + +// void acados_error(char error_string[]); + +// void acados_not_implemented(char feature_string[]); + +// blasfeo +// void print_blasfeo_target(); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_PRINT_H_ diff --git a/third_party/acados/include/acados/utils/strsep.h b/third_party/acados/include/acados/utils/strsep.h new file mode 100644 index 0000000..62bdfb4 --- /dev/null +++ b/third_party/acados/include/acados/utils/strsep.h @@ -0,0 +1,69 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_STRSEP_H_ +#define ACADOS_UTILS_STRSEP_H_ + +#ifdef __cplusplus +#include +#define STD(x) std::x +namespace std +{ +#else +#include +#define STD(x) x +#endif + +char* strsep_acados(char** stringp, const char* delim) +{ + char* result; + + if ((stringp == NULL) || (*stringp == NULL)) return NULL; + + result = *stringp; + + while (**stringp && !STD(strchr)(delim, **stringp)) ++*stringp; + + if (**stringp) + *(*stringp)++ = '\0'; + else + *stringp = NULL; + + return result; +} + +#ifdef __cplusplus +} // namespace std +#endif + +#undef STD + +#endif // ACADOS_UTILS_STRSEP_H_ diff --git a/third_party/acados/include/acados/utils/timing.h b/third_party/acados/include/acados/utils/timing.h new file mode 100644 index 0000000..b095593 --- /dev/null +++ b/third_party/acados/include/acados/utils/timing.h @@ -0,0 +1,122 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_TIMING_H_ +#define ACADOS_UTILS_TIMING_H_ + +#include "acados/utils/types.h" + +#ifdef __cplusplus +extern "C" { +#endif + +#ifdef MEASURE_TIMINGS +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + +/* Use Windows QueryPerformanceCounter for timing. */ +#include + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; +} acados_timer; + +#elif defined(__APPLE__) + +#include + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; +} acados_timer; + +#elif defined(__MABX2__) + +#include + +typedef struct acados_timer_ +{ + double time; +} acados_timer; + +#else + +/* Use POSIX clock_gettime() for timing on non-Windows machines. */ +#include + +#if (__STDC_VERSION__ >= 199901L) && !(defined __MINGW32__ || defined __MINGW64__) // C99 Mode + +#include +#include + +typedef struct acados_timer_ +{ + struct timeval tic; + struct timeval toc; +} acados_timer; + +#else // ANSI C Mode + +/** A structure for keeping internal timer data. */ +typedef struct acados_timer_ +{ + struct timespec tic; + struct timespec toc; +} acados_timer; + +#endif // __STDC_VERSION__ >= 199901L + +#endif // (defined _WIN32 || defined _WIN64) + +#else + +// Dummy type when timings are off +typedef real_t acados_timer; + +#endif // MEASURE_TIMINGS + +/** A function for measurement of the current time. */ +void acados_tic(acados_timer* t); + +/** A function which returns the elapsed time. */ +real_t acados_toc(acados_timer* t); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_TIMING_H_ diff --git a/third_party/acados/include/acados/utils/types.h b/third_party/acados/include/acados/utils/types.h new file mode 100644 index 0000000..a27ef9e --- /dev/null +++ b/third_party/acados/include/acados/utils/types.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef ACADOS_UTILS_TYPES_H_ +#define ACADOS_UTILS_TYPES_H_ + +/* Symbol visibility in DLLs */ +#ifndef ACADOS_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define ACADOS_SYMBOL_EXPORT + #else + #define ACADOS_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && ((__GNUC__ >= 4) || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) + #define ACADOS_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define ACADOS_SYMBOL_EXPORT + #endif +#endif + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +#define MAX_STR_LEN 256 +#define ACADOS_EPS 1e-12 +#define ACADOS_NEG_INFTY -1.0e9 +#define ACADOS_POS_INFTY +1.0e9 +#define UNUSED(x) ((void)(x)) + + + +typedef double real_t; +typedef int int_t; +typedef size_t acados_size_t; + + +typedef int (*casadi_function_t)(const double** arg, double** res, int* iw, double* w, void* mem); + + + +// enum of return values +enum return_values +{ + ACADOS_SUCCESS, + ACADOS_NAN_DETECTED, + ACADOS_MAXITER, + ACADOS_MINSTEP, + ACADOS_QP_FAILURE, + ACADOS_READY, +}; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // ACADOS_UTILS_TYPES_H_ diff --git a/third_party/acados/include/acados_c/condensing_interface.h b/third_party/acados/include/acados_c/condensing_interface.h new file mode 100644 index 0000000..b430207 --- /dev/null +++ b/third_party/acados/include/acados_c/condensing_interface.h @@ -0,0 +1,81 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ +#define INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_full_condensing.h" +#include "acados/ocp_qp/ocp_qp_partial_condensing.h" + +typedef enum { + PARTIAL_CONDENSING, + FULL_CONDENSING, +} condensing_t; + +typedef struct +{ + condensing_t condensing_type; +} condensing_plan; + +typedef struct +{ + ocp_qp_xcond_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} condensing_module; + +ocp_qp_xcond_config *ocp_qp_condensing_config_create(condensing_plan *plan); +// +void *ocp_qp_condensing_opts_create(ocp_qp_xcond_config *config, void *dims_); +// +acados_size_t ocp_qp_condensing_calculate_size(ocp_qp_xcond_config *config, void *dims_, void *opts_); +// +condensing_module *ocp_qp_condensing_assign(ocp_qp_xcond_config *config, void *dims_, + void *opts_, void *raw_memory); +// +condensing_module *ocp_qp_condensing_create(ocp_qp_xcond_config *config, void *dims_, + void *opts_); +// +int ocp_qp_condense(condensing_module *module, void *qp_in, void *qp_out); +// +int ocp_qp_expand(condensing_module *module, void *qp_in, void *qp_out); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_CONDENSING_INTERFACE_H_ diff --git a/third_party/acados/include/acados_c/dense_qp_interface.h b/third_party/acados/include/acados_c/dense_qp_interface.h new file mode 100644 index 0000000..b3af4bf --- /dev/null +++ b/third_party/acados/include/acados_c/dense_qp_interface.h @@ -0,0 +1,91 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/dense_qp/dense_qp_common.h" + +typedef enum { DENSE_QP_HPIPM, DENSE_QP_QORE, DENSE_QP_QPOASES, DENSE_QP_OOQP, DENSE_QP_DAQP } dense_qp_solver_t; + +typedef struct +{ + dense_qp_solver_t qp_solver; +} dense_qp_solver_plan; + +typedef struct +{ + qp_solver_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} dense_qp_solver; + +qp_solver_config *dense_qp_config_create(dense_qp_solver_plan *plan); +// +dense_qp_dims *dense_qp_dims_create(); +// +dense_qp_in *dense_qp_in_create(qp_solver_config *config, dense_qp_dims *dims); +// +dense_qp_out *dense_qp_out_create(qp_solver_config *config, dense_qp_dims *dims); +// +void *dense_qp_opts_create(qp_solver_config *config, dense_qp_dims *dims); +// +acados_size_t dense_qp_calculate_size(qp_solver_config *config, dense_qp_dims *dims, void *opts_); +// +dense_qp_solver *dense_qp_assign(qp_solver_config *config, dense_qp_dims *dims, void *opts_, + void *raw_memory); +// +dense_qp_solver *dense_qp_create(qp_solver_config *config, dense_qp_dims *dims, void *opts_); +// +int dense_qp_solve(dense_qp_solver *solver, dense_qp_in *qp_in, dense_qp_out *qp_out); +// +void dense_qp_inf_norm_residuals(dense_qp_dims *dims, dense_qp_in *qp_in, dense_qp_out *qp_out, + double *res); +// +bool dense_qp_set_field_double_array(const char *field, double *arr, dense_qp_in *qp_in); +// +bool dense_qp_set_field_int_array(const char *field, int *arr, dense_qp_in *qp_in); +// +bool dense_qp_get_field_double_array(const char *field, dense_qp_in *qp_in, double *arr); +// +bool dense_qp_get_field_int_array(const char *field, dense_qp_in *qp_in, int *arr); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_DENSE_QP_INTERFACE_H_ diff --git a/third_party/acados/include/acados_c/external_function_interface.h b/third_party/acados/include/acados_c/external_function_interface.h new file mode 100644 index 0000000..d4f52db --- /dev/null +++ b/third_party/acados/include/acados_c/external_function_interface.h @@ -0,0 +1,89 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ +#define INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/utils/external_function_generic.h" + + + +/************************************************ + * generic external parametric function + ************************************************/ + +// +void external_function_param_generic_create(external_function_param_generic *fun, int np); +// +void external_function_param_generic_free(external_function_param_generic *fun); + + + +/************************************************ + * casadi external function + ************************************************/ + +// +void external_function_casadi_create(external_function_casadi *fun); +// +void external_function_casadi_free(external_function_casadi *fun); +// +void external_function_casadi_create_array(int size, external_function_casadi *funs); +// +void external_function_casadi_free_array(int size, external_function_casadi *funs); + + + +/************************************************ + * casadi external parametric function + ************************************************/ + +// +void external_function_param_casadi_create(external_function_param_casadi *fun, int np); +// +void external_function_param_casadi_free(external_function_param_casadi *fun); +// +void external_function_param_casadi_create_array(int size, external_function_param_casadi *funs, + int np); +// +void external_function_param_casadi_free_array(int size, external_function_param_casadi *funs); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_EXTERNAL_FUNCTION_INTERFACE_H_ diff --git a/third_party/acados/include/acados_c/ocp_nlp_interface.h b/third_party/acados/include/acados_c/ocp_nlp_interface.h new file mode 100644 index 0000000..dd3e596 --- /dev/null +++ b/third_party/acados/include/acados_c/ocp_nlp_interface.h @@ -0,0 +1,428 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +// acados +#include "acados/ocp_nlp/ocp_nlp_common.h" +#include "acados/ocp_nlp/ocp_nlp_constraints_bgh.h" +#include "acados/sim/sim_erk_integrator.h" +#include "acados/sim/sim_irk_integrator.h" +#include "acados/sim/sim_lifted_irk_integrator.h" +#include "acados/sim/sim_gnsf.h" +#include "acados/utils/types.h" +// acados_c +#include "acados_c/ocp_qp_interface.h" +#include "acados_c/sim_interface.h" + + +/// Solution methods for optimal control problems. +typedef enum +{ + SQP, + SQP_RTI, + INVALID_NLP_SOLVER, +} ocp_nlp_solver_t; + + +/// Types of the cost function. +typedef enum +{ + LINEAR_LS, + NONLINEAR_LS, + CONVEX_OVER_NONLINEAR, + EXTERNAL, + INVALID_COST, +} ocp_nlp_cost_t; + + +/// Types of the system dynamics, discrete or continuous time. +typedef enum +{ + CONTINUOUS_MODEL, + DISCRETE_MODEL, + INVALID_DYNAMICS, +} ocp_nlp_dynamics_t; + + +/// Constraint types +typedef enum +{ + /// Comprises simple bounds, polytopic constraints, + /// general non-linear constraints. + BGH, + + /// Comprises simple bounds, polytopic constraints, + /// general non-linear constraints, and positive definite constraints. + BGP, + + INVALID_CONSTRAINT, +} ocp_nlp_constraints_t; + + +/// Regularization types +typedef enum +{ + NO_REGULARIZE, + MIRROR, + PROJECT, + PROJECT_REDUC_HESS, + CONVEXIFY, + INVALID_REGULARIZE, +} ocp_nlp_reg_t; + + +/// Structure to store the configuration of a non-linear program +typedef struct ocp_nlp_plan_t +{ + /// QP solver configuration. + ocp_qp_solver_plan_t ocp_qp_solver_plan; + + /// Simulation solver configuration for each stage. + sim_solver_plan_t *sim_solver_plan; + + /// Nlp solver type. + ocp_nlp_solver_t nlp_solver; + + /// Regularization type, defaults to no regularization. + ocp_nlp_reg_t regularization; + + /// Cost type for each stage. + ocp_nlp_cost_t *nlp_cost; + + /// Dynamics type for each stage. + ocp_nlp_dynamics_t *nlp_dynamics; + + /// Constraints type for each stage. + ocp_nlp_constraints_t *nlp_constraints; + + /// Horizon length. + int N; + +} ocp_nlp_plan_t; + + +/// Structure to store the state/configuration for the non-linear programming solver +typedef struct ocp_nlp_solver +{ + ocp_nlp_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} ocp_nlp_solver; + + +/// Constructs an empty plan struct (user nlp configuration), all fields are set to a +/// default/invalid state. +/// +/// \param N Horizon length +ACADOS_SYMBOL_EXPORT ocp_nlp_plan_t *ocp_nlp_plan_create(int N); + +/// Destructor for plan struct, frees memory. +/// +/// \param plan_ The plan struct to destroy. +ACADOS_SYMBOL_EXPORT void ocp_nlp_plan_destroy(void* plan_); + + +/// Constructs an nlp configuration struct from a plan. +/// +/// \param plan The plan (user nlp configuration). +ACADOS_SYMBOL_EXPORT ocp_nlp_config *ocp_nlp_config_create(ocp_nlp_plan_t plan); + +/// Desctructor of the nlp configuration. +/// +/// \param config_ The configuration struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_config_destroy(void *config_); + + +/// Constructs an struct that contains the dimensions of the variables. +/// +/// \param config_ The configuration struct. +ACADOS_SYMBOL_EXPORT ocp_nlp_dims *ocp_nlp_dims_create(void *config_); + +/// Destructor of The dimension struct. +/// +/// \param dims_ The dimension struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_dims_destroy(void *dims_); + + +/// Constructs an input struct for a non-linear programs. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +ACADOS_SYMBOL_EXPORT ocp_nlp_in *ocp_nlp_in_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the inputs struct. +/// +/// \param in The inputs struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_in_destroy(void *in); + + +/// Sets the sampling times for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field Has to be "Ts" (TBC other options). +/// \param value The sampling times (floating point). +ACADOS_SYMBOL_EXPORT void ocp_nlp_in_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage, + const char *field, void *value); + +/// +// void ocp_nlp_in_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage, +// const char *field, void *value); + +/// Sets the function pointers to the dynamics functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param fun_type The name of the function type, either impl_ode_fun, +/// impl_ode_fun_jac_x_xdot, impl_ode_jac_x_xdot_u (TBC) +/// \param fun_ptr Function pointer to the dynamics function. +ACADOS_SYMBOL_EXPORT int ocp_nlp_dynamics_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int stage, const char *fun_type, void *fun_ptr); + + +/// Sets the function pointers to the cost functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field The name of the field, either nls_res_jac, +/// y_ref, W (others TBC) +/// \param value Cost values. +ACADOS_SYMBOL_EXPORT int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int stage, const char *field, void *value); + + +/// Sets the function pointers to the constraints functions for the given stage. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param in The inputs struct. +/// \param stage Stage number. +/// \param field The name of the field, either lb, ub (others TBC) +/// \param value Constraints function or values. +ACADOS_SYMBOL_EXPORT int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_in *in, int stage, const char *field, void *value); + +/// +ACADOS_SYMBOL_EXPORT void ocp_nlp_constraints_model_get(ocp_nlp_config *config, ocp_nlp_dims *dims, + ocp_nlp_in *in, int stage, const char *field, void *value); + +/* out */ + +/// Constructs an output struct for the non-linear program. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +ACADOS_SYMBOL_EXPORT ocp_nlp_out *ocp_nlp_out_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the output struct. +/// +/// \param out The output struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_out_destroy(void *out); + + +/// Sets fields in the output struct of an nlp solver, used to initialize the solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param out The output struct. +/// \param stage Stage number. +/// \param field The name of the field, either x, u, pi. +/// \param value Initialization values. +ACADOS_SYMBOL_EXPORT void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, void *value); + + +/// Gets values of fields in the output struct of an nlp solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param out The output struct. +/// \param stage Stage number. +/// \param field The name of the field, either x, u, z, pi. +/// \param value Pointer to the output memory. +ACADOS_SYMBOL_EXPORT void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, void *value); + +// +ACADOS_SYMBOL_EXPORT void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver, + int stage, const char *field, void *value); + +// TODO(andrea): remove this once/if the MATLAB interface uses the new setters below? +ACADOS_SYMBOL_EXPORT int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field); + +ACADOS_SYMBOL_EXPORT void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out); + +ACADOS_SYMBOL_EXPORT void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out); + +ACADOS_SYMBOL_EXPORT void ocp_nlp_qp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int stage, const char *field, int *dims_out); + +/* opts */ + +/// Creates an options struct for the non-linear program. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +ACADOS_SYMBOL_EXPORT void *ocp_nlp_solver_opts_create(ocp_nlp_config *config, ocp_nlp_dims *dims); + +/// Destructor of the options. +/// +/// \param opts The options struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_opts_destroy(void *opts); + +/// Sets an option. +/// +/// \param config The configuration struct. +/// \param opts_ The options struct. +/// \param field Name of the option. +/// \param value Value of the option. +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value); + + +// ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_opts_get(ocp_nlp_config *config, void *opts_, const char *field, void* value); + + +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_opts_set_at_stage(ocp_nlp_config *config, void *opts_, int stage, const char *field, void* value); + + +/// TBC +/// Updates the options. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_opts_update(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_); + + +/* solver */ + +/// Creates an ocp solver. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +/// \return The solver. +ACADOS_SYMBOL_EXPORT ocp_nlp_solver *ocp_nlp_solver_create(ocp_nlp_config *config, ocp_nlp_dims *dims, void *opts_); + +/// Destructor of the solver. +/// +/// \param solver The solver struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_destroy(void *solver); + +/// Solves the optimal control problem. Call ocp_nlp_precompute before +/// calling this functions (TBC). +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +ACADOS_SYMBOL_EXPORT int ocp_nlp_solve(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + + +/// Resets the memory of the QP solver +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_solver_reset_qp_memory(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + +/// Performs precomputations for the solver. Needs to be called before +/// ocl_nlp_solve (TBC). +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +ACADOS_SYMBOL_EXPORT int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + +/// Computes cost function value. +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + +/// Computes the residuals. +/// +/// \param solver The solver struct. +/// \param nlp_in The inputs struct. +/// \param nlp_out The output struct. +ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out); + + +// +ACADOS_SYMBOL_EXPORT void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out); + +/* get */ +/// \param config The configuration struct. +/// \param solver The solver struct. +/// \param field Supports "sqp_iter", "status", "nlp_res", "time_tot", ... +/// \param return_value_ Pointer to the output memory. +ACADOS_SYMBOL_EXPORT void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, + const char *field, void *return_value_); + +/* set */ +/// Sets the initial guesses for the integrator for the given stage. +/// +/// \param config The configuration struct. +/// \param solver The ocp_nlp_solver struct. +/// \param stage Stage number. +/// \param field Supports "z_guess", "xdot_guess" (IRK), "phi_guess" (GNSF-IRK) +/// \param value The initial guess for the algebraic variables in the integrator (if continuous model is used). +ACADOS_SYMBOL_EXPORT void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, + int stage, const char *field, void *value); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_OCP_NLP_INTERFACE_H_ diff --git a/third_party/acados/include/acados_c/ocp_qp_interface.h b/third_party/acados/include/acados_c/ocp_qp_interface.h new file mode 100644 index 0000000..3dc3f1a --- /dev/null +++ b/third_party/acados/include/acados_c/ocp_qp_interface.h @@ -0,0 +1,263 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ +#define INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/ocp_qp/ocp_qp_common.h" +#include "acados/ocp_qp/ocp_qp_xcond_solver.h" + + +/// QP solver types (Enumeration). +/// +/// Full list of fields: +/// PARTIAL_CONDENSING_HPIPM +/// PARTIAL_CONDENSING_HPMPC +/// PARTIAL_CONDENSING_OOQP +/// PARTIAL_CONDENSING_OSQP +/// PARTIAL_CONDENSING_QPDUNES +/// FULL_CONDENSING_HPIPM +/// FULL_CONDENSING_QPOASES +/// FULL_CONDENSING_QORE +/// FULL_CONDENSING_OOQP +/// INVALID_QP_SOLVER +/// +/// Note: In this enumeration the partial condensing solvers have to be +/// specified before the full condensing solvers. +typedef enum { + PARTIAL_CONDENSING_HPIPM, +#ifdef ACADOS_WITH_HPMPC + PARTIAL_CONDENSING_HPMPC, +#else + PARTIAL_CONDENSING_HPMPC_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OOQP + PARTIAL_CONDENSING_OOQP, +#else + PARTIAL_CONDENSING_OOQP_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OSQP + PARTIAL_CONDENSING_OSQP, +#else + PARTIAL_CONDENSING_OSQP_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_QPDUNES + PARTIAL_CONDENSING_QPDUNES, +#else + PARTIAL_CONDENSING_QPDUNES_NOT_AVAILABLE, +#endif + FULL_CONDENSING_HPIPM, +#ifdef ACADOS_WITH_QPOASES + FULL_CONDENSING_QPOASES, +#else + FULL_CONDENSING_QPOASES_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_DAQP + FULL_CONDENSING_DAQP, +#else + FULL_CONDENSING_DAQP_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_QORE + FULL_CONDENSING_QORE, +#else + FULL_CONDENSING_QORE_NOT_AVAILABLE, +#endif +#ifdef ACADOS_WITH_OOQP + FULL_CONDENSING_OOQP, +#else + FULL_CONDENSING_OOQP_NOT_AVAILABLE, +#endif + INVALID_QP_SOLVER, +} ocp_qp_solver_t; + + +/// Struct containing qp solver +typedef struct +{ + ocp_qp_solver_t qp_solver; +} ocp_qp_solver_plan_t; + + +/// Linear ocp configuration. +typedef struct +{ + ocp_qp_xcond_solver_config *config; + ocp_qp_xcond_solver_dims *dims; + void *opts; + void *mem; + void *work; +} ocp_qp_solver; + + +/// Initializes the qp solver configuration. +/// TBC should this be private/static - no, used in ocp_nlp +void ocp_qp_xcond_solver_config_initialize_from_plan( + ocp_qp_solver_t solver_name, ocp_qp_xcond_solver_config *solver_config); + +/// Constructs a qp solver config and Initializes with default values. +/// +/// \param plan The qp solver plan struct. +ocp_qp_xcond_solver_config *ocp_qp_xcond_solver_config_create(ocp_qp_solver_plan_t plan); + +/// Destructor for config struct, frees memory. +/// +/// \param config The config object to destroy. +void ocp_qp_xcond_solver_config_free(ocp_qp_xcond_solver_config *config); + + +/// Constructs a struct that contains the dimensions for the variables of the qp. +/// +/// \param N The number of variables. +ocp_qp_dims *ocp_qp_dims_create(int N); + +/// Destructor of The dimension struct. +/// +/// \param dims The dimension struct. +void ocp_qp_dims_free(void *dims); + +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create(ocp_qp_xcond_solver_config *config, int N); +// +ocp_qp_xcond_solver_dims *ocp_qp_xcond_solver_dims_create_from_ocp_qp_dims( + ocp_qp_xcond_solver_config *config, ocp_qp_dims *dims); +// +void ocp_qp_xcond_solver_dims_free(ocp_qp_xcond_solver_dims *dims_); + +void ocp_qp_xcond_solver_dims_set(void *config_, ocp_qp_xcond_solver_dims *dims, + int stage, const char *field, int* value); + + +/// Constructs an input object for the qp. +/// +/// \param dims The dimension struct. +ocp_qp_in *ocp_qp_in_create(ocp_qp_dims *dims); + + +void ocp_qp_in_set(ocp_qp_xcond_solver_config *config, ocp_qp_in *in, + int stage, char *field, void *value); + +/// Destructor of the inputs struct. +/// +/// \param in_ The inputs struct. +void ocp_qp_in_free(void *in_); + + +/// Constructs an outputs object for the qp. +/// +/// \param dims The dimension struct. +ocp_qp_out *ocp_qp_out_create(ocp_qp_dims *dims); + +/// Destructor of the output struct. +/// +/// \param out_ The output struct. +void ocp_qp_out_free(void *out_); + + +/// Getter of output struct +void ocp_qp_out_get(ocp_qp_out *out, const char *field, void *value); + + +/// Constructs an options object for the qp. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +void *ocp_qp_xcond_solver_opts_create(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims); + +/// Destructor of the options struct. +/// +/// \param opts The options struct to destroy. +void ocp_qp_xcond_solver_opts_free(ocp_qp_xcond_solver_opts *opts); + + +/// Setter of the options struct. +/// +/// \param opts The options struct. +void ocp_qp_xcond_solver_opts_set(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_opts *opts, const char *field, void* value); + +/// TBC Should be private/static? +acados_size_t ocp_qp_calculate_size(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// TBC Reserves memory? TBC Should this be private? +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +/// \param raw_memory Pointer to raw memory to assign to qp solver. +ocp_qp_solver *ocp_qp_assign(ocp_qp_xcond_solver_config *config, ocp_qp_xcond_solver_dims *dims, + void *opts_, void *raw_memory); + +/// Creates a qp solver. Reserves memory. +/// +/// \param config The configuration struct. +/// \param dims The dimension struct. +/// \param opts_ The options struct. +ocp_qp_solver *ocp_qp_create(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// Destroys a qp solver. Frees memory. +/// +/// \param solver The qp solver +void ocp_qp_solver_destroy(ocp_qp_solver *solver); + +void ocp_qp_x_cond_solver_free(ocp_qp_xcond_solver_config *config, + ocp_qp_xcond_solver_dims *dims, void *opts_); + + +/// Solves the qp. +/// +/// \param solver The solver. +/// \param qp_in The inputs struct. +/// \param qp_out The output struct. +int ocp_qp_solve(ocp_qp_solver *solver, ocp_qp_in *qp_in, ocp_qp_out *qp_out); + + +/// Calculates the infinity norm of the residuals. +/// +/// \param dims The dimension struct. +/// \param qp_in The inputs struct. +/// \param qp_out The output struct. +/// \param res Output array for the residuals. +void ocp_qp_inf_norm_residuals(ocp_qp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, + double *res); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_OCP_QP_INTERFACE_H_ diff --git a/third_party/acados/include/acados_c/sim_interface.h b/third_party/acados/include/acados_c/sim_interface.h new file mode 100644 index 0000000..09a05d6 --- /dev/null +++ b/third_party/acados/include/acados_c/sim_interface.h @@ -0,0 +1,137 @@ +/* + * Copyright (c) The acados authors. + * + * This file is part of acados. + * + * The 2-Clause BSD License + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, + * this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE.; + */ + + +#ifndef INTERFACES_ACADOS_C_SIM_INTERFACE_H_ +#define INTERFACES_ACADOS_C_SIM_INTERFACE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#include "acados/sim/sim_common.h" + + + +typedef enum +{ + ERK, + IRK, + GNSF, + LIFTED_IRK, + INVALID_SIM_SOLVER, +} sim_solver_t; + + + +typedef struct +{ + sim_solver_t sim_solver; +} sim_solver_plan_t; + + + +typedef struct +{ + sim_config *config; + void *dims; + void *opts; + void *mem; + void *work; +} sim_solver; + + + +/* config */ +// +ACADOS_SYMBOL_EXPORT sim_config *sim_config_create(sim_solver_plan_t plan); +// +ACADOS_SYMBOL_EXPORT void sim_config_destroy(void *config); + +/* dims */ +// +ACADOS_SYMBOL_EXPORT void *sim_dims_create(void *config_); +// +ACADOS_SYMBOL_EXPORT void sim_dims_destroy(void *dims); +// +ACADOS_SYMBOL_EXPORT void sim_dims_set(sim_config *config, void *dims, const char *field, const int* value); +// +ACADOS_SYMBOL_EXPORT void sim_dims_get(sim_config *config, void *dims, const char *field, int* value); +// +ACADOS_SYMBOL_EXPORT void sim_dims_get_from_attr(sim_config *config, void *dims, const char *field, int *dims_out); + +/* in */ +// +ACADOS_SYMBOL_EXPORT sim_in *sim_in_create(sim_config *config, void *dims); +// +ACADOS_SYMBOL_EXPORT void sim_in_destroy(void *out); +// +ACADOS_SYMBOL_EXPORT int sim_in_set(void *config_, void *dims_, sim_in *in, const char *field, void *value); + + +/* out */ +// +ACADOS_SYMBOL_EXPORT sim_out *sim_out_create(sim_config *config, void *dims); +// +ACADOS_SYMBOL_EXPORT void sim_out_destroy(void *out); +// +ACADOS_SYMBOL_EXPORT int sim_out_get(void *config, void *dims, sim_out *out, const char *field, void *value); + +/* opts */ +// +ACADOS_SYMBOL_EXPORT void *sim_opts_create(sim_config *config, void *dims); +// +ACADOS_SYMBOL_EXPORT void sim_opts_destroy(void *opts); +// +ACADOS_SYMBOL_EXPORT void sim_opts_set(sim_config *config, void *opts, const char *field, void *value); +// +ACADOS_SYMBOL_EXPORT void sim_opts_get(sim_config *config, void *opts, const char *field, void *value); + +/* solver */ +// +ACADOS_SYMBOL_EXPORT acados_size_t sim_calculate_size(sim_config *config, void *dims, void *opts_); +// +ACADOS_SYMBOL_EXPORT sim_solver *sim_assign(sim_config *config, void *dims, void *opts_, void *raw_memory); +// +ACADOS_SYMBOL_EXPORT sim_solver *sim_solver_create(sim_config *config, void *dims, void *opts_); +// +ACADOS_SYMBOL_EXPORT void sim_solver_destroy(void *solver); +// +ACADOS_SYMBOL_EXPORT int sim_solve(sim_solver *solver, sim_in *in, sim_out *out); +// +ACADOS_SYMBOL_EXPORT int sim_precompute(sim_solver *solver, sim_in *in, sim_out *out); +// +ACADOS_SYMBOL_EXPORT int sim_solver_set(sim_solver *solver, const char *field, void *value); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // INTERFACES_ACADOS_C_SIM_INTERFACE_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo.h b/third_party/acados/include/blasfeo/include/blasfeo.h new file mode 100644 index 0000000..c854918 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#include "blasfeo_processor_features.h" +#include "blasfeo_target.h" +#include "blasfeo_block_size.h" +#include "blasfeo_stdlib.h" +#include "blasfeo_common.h" +#include "blasfeo_d_aux.h" +#include "blasfeo_d_aux_ext_dep.h" +#include "blasfeo_d_kernel.h" +#include "blasfeo_d_blas.h" +#include "blasfeo_s_aux.h" +#include "blasfeo_s_aux_ext_dep.h" +#include "blasfeo_s_kernel.h" +#include "blasfeo_s_blas.h" +#include "blasfeo_i_aux_ext_dep.h" +#include "blasfeo_v_aux_ext_dep.h" +#include "blasfeo_timing.h" +#include "blasfeo_memory.h" diff --git a/third_party/acados/include/blasfeo/include/blasfeo_block_size.h b/third_party/acados/include/blasfeo/include/blasfeo_block_size.h new file mode 100644 index 0000000..5d0907a --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_block_size.h @@ -0,0 +1,447 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_BLOCK_SIZE_H_ +#define BLASFEO_BLOCK_SIZE_H_ + + + +#define D_EL_SIZE 8 // double precision +#define S_EL_SIZE 4 // single precision + + + +#if defined( TARGET_X64_INTEL_SKYLAKE_X ) +// common +#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way +#define L2_CACHE_SIZE (256*1024) //(1024*1024) // L2 data cache size: 1 MB ; DTLB1 64*4 kB = 256 kB +#define LLC_CACHE_SIZE (6*1024*1024) //(8*1024*1024) // LLC cache size: 8 MB ; TLB 1536*4 kB = 6 MB +// double +#define D_PS 8 // panel size +#define D_PLD 8 // 4 // GCD of panel length +#define D_M_KERNEL 24 // max kernel size +#define D_N_KERNEL 8 // max kernel size +#define D_KC 128 //256 // 192 +#define D_NC 144 //72 //96 //72 // 120 // 512 +#define D_MC 2400 // 6000 +// single +#define S_PS 16 // panel size +#define S_PLD 4 // GCD of panel length TODO probably 16 when writing assebly +#define S_M_KERNEL 32 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 128 //256 +#define S_NC 128 //144 +#define S_MC 3000 + +#elif defined( TARGET_X64_INTEL_HASWELL ) +// common +#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way +#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB +#define LLC_CACHE_SIZE (6*1024*1024) // LLC cache size: 6 MB ; TLB 1024*4 kB = 4 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_N_KERNEL 8 // max kernel size +#define D_KC 256 // 192 +#define D_NC 64 //96 //72 // 120 // 512 +#define D_MC 1500 +// single +#define S_PS 8 // panel size +#define S_PLD 4 // 2 // GCD of panel length +#define S_M_KERNEL 24 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 144 +#define S_MC 3000 + +#elif defined( TARGET_X64_INTEL_SANDY_BRIDGE ) +// common +#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way +#define L2_CACHE_SIZE (256*1024) // L2 data cache size: 256 kB ; DTLB1 64*4 kB = 256 kB +#define LLC_CACHE_SIZE (4*1024*1024) // LLC cache size: 4 MB ; TLB 1024*4 kB = 4 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 //320 //256 //320 +#define D_NC 72 //64 //72 //60 // 120 +#define D_MC 1000 // 800 +// single +#define S_PS 8 // panel size +#define S_PLD 4 // 2 // GCD of panel length +#define S_M_KERNEL 16 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 144 +#define S_MC 2000 + +#elif defined( TARGET_X64_INTEL_CORE ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + +#elif defined( TARGET_X64_AMD_BULLDOZER ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_X86_AMD_JAGUAR ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_X86_AMD_BARCELONA ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_APPLE_M1) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (128*1024) // L1 data cache size (big cores): 64 kB, ?-way ; DTLB1 ? +#define LLC_CACHE_SIZE (12*1024*1024) // LLC (L2) cache size (big cores): 12 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 512 //256 +#define D_NC 128 //256 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 512 +#define S_NC 256 +#define S_MC 6000 + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A76) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (64*1024) // L1 data cache size: 64 kB, 4-way ; DTLB1 48*4 kB = 192 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 512 //256 +#define D_NC 128 //256 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 512 +#define S_NC 256 +#define S_MC 6000 + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A73) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 (64?) kB, 4-way, seen as 8-(16-)way ; DTLB1 48*4 kB = 192 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 320 +#define D_NC 256 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A57) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 2-way ; DTLB1 32*4 kB = 128 kB +#define LLC_CACHE_SIZE (1*1024*1024) // LLC cache size: 1 MB // 2 MB ??? +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 8 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 128 //224 //256 //192 +#define D_NC 72 //40 //36 //48 +#define D_MC (4*192) //512 //488 //600 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A55) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ; DTLB1 16*4 kB = 64 kB +#define LLC_CACHE_SIZE (512*1024) // LLC cache size: 512 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 224 +#define D_NC 160 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined(TARGET_ARMV8A_ARM_CORTEX_A53) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 4-way ??? ; DTLB1 10*4 kB = 40 kB +#define LLC_CACHE_SIZE (256*1024) // LLC cache size: 256 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 12 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 160 +#define D_NC 128 +#define D_MC 6000 +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 8 // max kernel size +#define S_N_KERNEL 8 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A15 ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A7 ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_ARMV7A_ARM_CORTEX_A9 ) +// common +#define CACHE_LINE_SIZE 32 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#elif defined( TARGET_GENERIC ) +// common +#define CACHE_LINE_SIZE 64 +#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB +// double +#define D_PS 4 // panel size +#define D_PLD 4 // 2 // GCD of panel length +#define D_M_KERNEL 4 // max kernel size +#define D_N_KERNEL 4 // max kernel size +#define D_KC 256 +#define D_NC 128 // TODO these are just dummy +#define D_MC 3000 // TODO these are just dummy + +// single +#define S_PS 4 +#define S_PLD 4 //2 +#define S_M_KERNEL 4 // max kernel size +#define S_N_KERNEL 4 // max kernel size +#define S_KC 256 +#define S_NC 128 // TODO these are just dummy +#define S_MC 3000 // TODO these are just dummy + + +#else +#error "Unknown architecture" +#endif + + + +#define D_CACHE_LINE_EL (CACHE_LINE_SIZE/D_EL_SIZE) +#define D_L1_CACHE_EL (L1_CACHE_SIZE/D_EL_SIZE) +#define D_L2_CACHE_EL (L2_CACHE_SIZE/D_EL_SIZE) +#define D_LLC_CACHE_EL (LLC_CACHE_SIZE/D_EL_SIZE) + +#define S_CACHE_LINE_EL (CACHE_LINE_SIZE/S_EL_SIZE) +#define S_L1_CACHE_EL (L1_CACHE_SIZE/S_EL_SIZE) +#define S_L2_CACHE_EL (L2_CACHE_SIZE/S_EL_SIZE) +#define S_LLC_CACHE_EL (LLC_CACHE_SIZE/S_EL_SIZE) + + + +#endif // BLASFEO_BLOCK_SIZE_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_common.h b/third_party/acados/include/blasfeo/include/blasfeo_common.h new file mode 100644 index 0000000..03b6c36 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_common.h @@ -0,0 +1,274 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_COMMON_H_ +#define BLASFEO_COMMON_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#if defined(__GNUC__) || defined(__clang__) || defined(__INTEL_COMPILER) || defined(__ICL) || defined(__ICC) || defined(__INTEL_LLVM_COMPILER) +#define ALIGNED(VEC, BYTES) VEC __attribute__ ((aligned ( BYTES ))) +#elif defined (_MSC_VER) +#define ALIGNED(VEC, BYTES) __declspec(align( BYTES )) VEC +#else +#define ALIGNED(VEC, BYTES) VEC +#endif + + + + +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_PANELMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_PANELMAJ) ) + +#include "blasfeo_block_size.h" + +// matrix structure +struct blasfeo_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +// vector structure +struct blasfeo_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a pm array of doubles, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +struct blasfeo_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a pm array of floats, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(D_PS-1)))*(sA)->cn+(aj)*D_PS+((ai)&(D_PS-1))]) +#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&(S_PS-1)))*(sA)->cn+(aj)*S_PS+((ai)&(S_PS-1))]) +#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai]) + +#elif ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) | ( defined(LA_REFERENCE) & defined(MF_COLMAJ) ) | defined(LA_EXTERNAL_BLAS_WRAPPER) + +// matrix structure +struct blasfeo_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a m*n array of doubles + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a m*n array of floats + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +// vector structure +struct blasfeo_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a m array of doubles, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + +struct blasfeo_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a m array of floats, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + +#define BLASFEO_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_SVECEL(sa,ai) ((sa)->pa[ai]) + +#else + +#error : wrong LA or MF choice + +#endif + + + +// Explicitly panel-major matrix structure +struct blasfeo_pm_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a pm*pn array of doubles, the first is aligned to cache line size + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int ps; // panel size + int memsize; // size of needed memory + }; + +struct blasfeo_pm_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a pm*pn array of floats, the first is aligned to cache line size + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int pm; // packed number or rows + int cn; // packed number or cols + int use_dA; // flag to tell if dA can be used + int ps; // panel size + int memsize; // size of needed memory + }; + +struct blasfeo_pm_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a pm array of doubles, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +struct blasfeo_pm_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a pm array of floats, the first is aligned to cache line size + int m; // size + int pm; // packed size + int memsize; // size of needed memory + }; + +// Explicitly column-major matrix structure +struct blasfeo_cm_dmat + { + double *mem; // pointer to passed chunk of memory + double *pA; // pointer to a m*n array of doubles + double *dA; // pointer to a min(m,n) (or max???) array of doubles + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_cm_smat + { + float *mem; // pointer to passed chunk of memory + float *pA; // pointer to a m*n array of floats + float *dA; // pointer to a min(m,n) (or max???) array of floats + int m; // rows + int n; // cols + int use_dA; // flag to tell if dA can be used + int memsize; // size of needed memory + }; + +struct blasfeo_cm_dvec + { + double *mem; // pointer to passed chunk of memory + double *pa; // pointer to a m array of doubles, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + +struct blasfeo_cm_svec + { + float *mem; // pointer to passed chunk of memory + float *pa; // pointer to a m array of floats, the first is aligned to cache line size + int m; // size + int memsize; // size of needed memory + }; + + +#define BLASFEO_PM_DMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))]) +#define BLASFEO_PM_SMATEL(sA,ai,aj) ((sA)->pA[((ai)-((ai)&((sA)->ps-1)))*(sA)->cn+(aj)*((sA)->ps)+((ai)&((sA)->ps-1))]) +#define BLASFEO_PM_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_PM_SVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_CM_DMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_CM_SMATEL(sA,ai,aj) ((sA)->pA[(ai)+(aj)*(sA)->m]) +#define BLASFEO_CM_DVECEL(sa,ai) ((sa)->pa[ai]) +#define BLASFEO_CM_SVECEL(sa,ai) ((sa)->pa[ai]) + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_COMMON_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h new file mode 100644 index 0000000..a6d6119 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux.h @@ -0,0 +1,255 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operations header + * + * include/blasfeo_aux_lib*.h + * + */ + +#ifndef BLASFEO_D_AUX_H_ +#define BLASFEO_D_AUX_H_ + + + +#include + +#include "blasfeo_common.h" +#include "blasfeo_d_aux_old.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// --- memory size calculations +// +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_memsize_dmat(int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a dmat +size_t blasfeo_memsize_diag_dmat(int m, int n); +// returns the memory size (in bytes) needed for a dvec +size_t blasfeo_memsize_dvec(int m); + +// --- creation +// +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- packing +// pack the column-major matrix A into the matrix struct B +void blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the lower-triangular column-major matrix A into the matrix struct B +void blasfeo_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the upper-triangular column-major matrix A into the matrix struct B +void blasfeo_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// transpose and pack the column-major matrix A into the matrix struct B +void blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the vector x into the vector structure y +void blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi); +// unpack the matrix structure A into the column-major matrix B +void blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// transpose and unpack the matrix structure A into the column-major matrix B +void blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// pack the vector structure x into the vector y +void blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y, int yi); + +// --- cast +// +//void d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO +//void d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO +//void d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO + + +// ge +// --- insert/extract +// +// sA[ai, aj] <= a +void blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// <= sA[ai, aj] +double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); + +// --- set +// A <= alpha +void blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- copy / scale +// B <= A +void blasfeo_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// A <= alpha*A +void blasfeo_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// B <= alpha*A +void blasfeo_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A, A lower triangular +void blasfeo_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- sum +// B <= B + alpha*A +void blasfeo_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// y <= y + alpha*x +void blasfeo_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); + +// --- traspositions +// B <= A' +void blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A lower triangular +void blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A upper triangular +void blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); + +// dia +// diag(A) += alpha +void blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A) <= alpha*x +void blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] <= alpha*x +void blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// x <= diag(A) +void blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// x <= diag(A)[idx] +void blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// diag(A) += alpha*x +void blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] += alpha*x +void blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// diag(A)[idx] = y + alpha*x +void blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); + +// row +void blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// col +void blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// vec +// a <= alpha +void blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// sx[xi] <= a +void blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// <= sx[xi] +double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi); +// y <= x +void blasfeo_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// x <= alpha*x +void blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// y <= alpha*x +void blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// z[idx] += alpha * x +void blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// z[idx] <= alpha * x +void blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// z <= alpha * x[idx] +void blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z += alpha * x[idx] +void blasfeo_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); + +void blasfeo_dveccl(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); + +void blasfeo_dveccl_mask(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, + struct blasfeo_dvec *sm, int mi); + +void blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +void blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_dvecnrm_2(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +void blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + + + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_pm_memsize_dmat(int ps, int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_pm_create_dmat(int ps, int m, int n, struct blasfeo_pm_dmat *sA, void *memory); +// print +void blasfeo_pm_print_dmat(int m, int n, struct blasfeo_pm_dmat *sA, int ai, int aj); + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_cm_memsize_dmat(int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_cm_create_dmat(int m, int n, struct blasfeo_pm_dmat *sA, void *memory); + + + +// +// BLAS API helper functions +// + +#if ( defined(BLAS_API) & defined(MF_PANELMAJ) ) +// aux +void blasfeo_cm_dgetr(int m, int n, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj); +#endif + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h new file mode 100644 index 0000000..bee9e98 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep.h @@ -0,0 +1,145 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operation external dependancies header + * + * include/blasfeo_d_aux_ext_dep.h + * + * - dynamic memory allocation + * - print + * + */ + +#ifndef BLASFEO_D_AUX_EXT_DEP_H_ +#define BLASFEO_D_AUX_EXT_DEP_H_ + + + +#include + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +/* column-major matrices */ + +// dynamically allocate row*col doubles of memory and set accordingly a pointer to double; set allocated memory to zero +void d_zeros(double **pA, int row, int col); +// dynamically allocate row*col doubles of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero +void d_zeros_align(double **pA, int row, int col); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to double; set allocated memory to zero +void d_zeros_align_bytes(double **pA, int size); +// free the memory allocated by d_zeros +void d_free(double *pA); +// free the memory allocated by d_zeros_align or d_zeros_align_bytes +void d_free_align(double *pA); +// print a column-major matrix +void d_print_mat(int m, int n, double *A, int lda); +// print the transposed of a column-major matrix +void d_print_tran_mat(int row, int col, double *A, int lda); +// print to file a column-major matrix +void d_print_to_file_mat(FILE *file, int row, int col, double *A, int lda); +// print to file a column-major matrix in exponential format +void d_print_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda); +// print to string a column-major matrix +void d_print_to_string_mat(char **buf_out, int row, int col, double *A, int lda); +// print to file the transposed of a column-major matrix +void d_print_tran_to_file_mat(FILE *file, int row, int col, double *A, int lda); +// print to file the transposed of a column-major matrix in exponential format +void d_print_tran_to_file_exp_mat(FILE *file, int row, int col, double *A, int lda); +// print in exponential notation a column-major matrix +void d_print_exp_mat(int m, int n, double *A, int lda); +// print in exponential notation the transposed of a column-major matrix +void d_print_exp_tran_mat(int row, int col, double *A, int lda); + +/* strmat and strvec */ + +// create a strmat for a matrix of size m*n by dynamically allocating memory +void blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA); +// create a strvec for a vector of size m by dynamically allocating memory +void blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa); +// free the memory allocated by blasfeo_allocate_dmat +void blasfeo_free_dmat(struct blasfeo_dmat *sA); +// free the memory allocated by blasfeo_allocate_dvec +void blasfeo_free_dvec(struct blasfeo_dvec *sa); +// print a strmat +void blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print in exponential notation a strmat +void blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to file a strmat +void blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to file a strmat in exponential format +void blasfeo_print_to_file_exp_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print to string a strmat +void blasfeo_print_to_string_dmat(char **buf_out, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print the transposed of a strmat +void blasfeo_print_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +// print a strvec +void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print in exponential notation a strvec +void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print to file a strvec +void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +// print to string a strvec +void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); +// print the transposed of a strvec +void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print in exponential notation the transposed of a strvec +void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +// print to file the transposed of a strvec +void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +// print to string the transposed of a strvec +void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_EXT_DEP_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h new file mode 100644 index 0000000..81b811f --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ext_dep_ref.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operation external dependancies header + * + * include/blasfeo_d_aux_ext_dep.h + * + * - dynamic memory allocation + * - print + * + */ + +#ifndef BLASFEO_D_AUX_EXT_DEP_REF_H_ +#define BLASFEO_D_AUX_EXT_DEP_REF_H_ + + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// expose reference BLASFEO for testing +// see blasfeo_d_aux_exp_dep.h for help + +void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_allocate_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA); +void blasfeo_allocate_dvec_ref(int m, struct blasfeo_dvec_ref *sa); +void blasfeo_free_dmat_ref(struct blasfeo_dmat_ref *sA); +void blasfeo_free_dvec_ref(struct blasfeo_dvec_ref *sa); +void blasfeo_print_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_exp_dmat_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_exp_dmat_ref(FILE *file, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_to_string_dmat_ref(char **buf_out, int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj); +void blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_string_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void blasfeo_print_to_string_tran_dvec(char **buf_out, int m, struct blasfeo_dvec *sa, int ai); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_EXT_DEP_REF_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h new file mode 100644 index 0000000..3a1847a --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_old.h @@ -0,0 +1,75 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * ----------- TOMOVE + * + * expecting column major matrices + * + */ + +#include "blasfeo_common.h" + + +void dtrcp_l_lib(int m, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb); +void dgead_lib(int m, int n, double alpha, int offsetA, double *A, int sda, int offsetB, double *B, int sdb); +// TODO remove ??? +void ddiain_sqrt_lib(int kmax, double *x, int offset, double *pD, int sdd); +// TODO ddiaad1 +void ddiareg_lib(int kmax, double reg, int offset, double *pD, int sdd); + + +void dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x); +void ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +void ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +void ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x); +void ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +void ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd); +void drowin_lib(int kmax, double alpha, double *x, double *pD); +void drowex_lib(int kmax, double alpha, double *pD, double *x); +void drowad_lib(int kmax, double alpha, double *x, double *pD); +void drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD); +void drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD); +void drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD); +void dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd); +void dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +void dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd); +void dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd); +void dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +void dvecin_libsp(int kmax, int *idx, double *x, double *y); +void dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y); diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h new file mode 100644 index 0000000..1e007fa --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_ref.h @@ -0,0 +1,208 @@ + +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_AUX_REF_H_ +#define BLASFEO_D_AUX_REF_H_ + + + +#include + +#include "blasfeo_common.h" +#include "blasfeo_d_aux_old.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// --- memory calculations +// +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_ref_memsize_dmat(int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a dmat +size_t blasfeo_ref_memsize_diag_dmat(int m, int n); +// returns the memory size (in bytes) needed for a dvec +size_t blasfeo_ref_memsize_dvec(int m); + +// --- creation +// +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- packing +// pack the column-major matrix A into the matrix struct B +void blasfeo_ref_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the lower-triangular column-major matrix A into the matrix struct B +void blasfeo_ref_pack_l_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the upper-triangular column-major matrix A into the matrix struct B +void blasfeo_ref_pack_u_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// transpose and pack the column-major matrix A into the matrix struct B +void blasfeo_ref_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sB, int bi, int bj); +// pack the vector x into the vector structure y +void blasfeo_ref_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sy, int yi); +// unpack the matrix structure A into the column-major matrix B +void blasfeo_ref_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// transpose and unpack the matrix structure A into the column-major matrix B +void blasfeo_ref_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *B, int ldb); +// pack the vector structure x into the vector y +void blasfeo_ref_unpack_dvec(int m, struct blasfeo_dvec *sx, int xi, double *y, int yi); + +// --- cast +// +void ref_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); // TODO +void ref_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); // TODO +void ref_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sx); // TODO + + +// ge +// --- insert/extract +// +// sA[ai, aj] <= a +void blasfeo_ref_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// <= sA[ai, aj] +double blasfeo_ref_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); + +// --- set +// A <= alpha +void blasfeo_ref_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- copy / scale +// B <= A +void blasfeo_ref_dgecp(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// A <= alpha*A +void blasfeo_ref_dgesc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// B <= alpha*A +void blasfeo_ref_dgecpsc(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A, A lower triangular +void blasfeo_ref_dtrcp_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_ref_dtrcpsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +void blasfeo_ref_dtrsc_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj); + +// --- sum +// B <= B + alpha*A +void blasfeo_ref_dgead(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int yi, int cj); +// y <= y + alpha*x +void blasfeo_ref_dvecad(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); + +// --- traspositions +// B <= A' +void blasfeo_ref_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A lower triangular +void blasfeo_ref_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); +// B <= A', A upper triangular +void blasfeo_ref_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj); + +// dia +// diag(A) += alpha +void blasfeo_ref_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A) <= alpha*x +void blasfeo_ref_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] <= alpha*x +void blasfeo_ref_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// x <= diag(A) +void blasfeo_ref_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// x <= diag(A)[idx] +void blasfeo_ref_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// diag(A) += alpha*x +void blasfeo_ref_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// diag(A)[idx] += alpha*x +void blasfeo_ref_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// diag(A)[idx] = y + alpha*x +void blasfeo_ref_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); + +// row +void blasfeo_ref_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_ref_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_ref_drowpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// col +void blasfeo_ref_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolsc(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +void blasfeo_ref_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +void blasfeo_ref_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +void blasfeo_ref_dcolpei(int kmax, int *ipiv, struct blasfeo_dmat *sA); + +// vec +// a <= alpha +void blasfeo_ref_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// sx[xi] <= a +void blasfeo_ref_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// <= sx[xi] +double blasfeo_ref_dvecex1(struct blasfeo_dvec *sx, int xi); +// y <= x +void blasfeo_ref_dveccp(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// x <= alpha*x +void blasfeo_ref_dvecsc(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// y <= alpha*x +void blasfeo_ref_dveccpsc(int m, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +void blasfeo_ref_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_ref_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +void blasfeo_ref_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi); +// z += alpha * x[idx] +void blasfeo_ref_dvecexad_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); + +void blasfeo_ref_dveccl(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); + +void blasfeo_ref_dveccl_mask(int m, + struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, + struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, + struct blasfeo_dvec *sm, int mi); + +void blasfeo_ref_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +void blasfeo_ref_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +void blasfeo_ref_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +void blasfeo_ref_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_REF_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h new file mode 100644 index 0000000..1c61635 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_aux_test.h @@ -0,0 +1,226 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +/* + * auxiliary algebra operations header + * + * include/blasfeo_aux_lib*.h + * + */ + +#ifndef BLASFEO_D_AUX_TEST_H_ +#define BLASFEO_D_AUX_TEST_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + +// --- memory calculations +int test_blasfeo_memsize_dmat(int m, int n); +int test_blasfeo_memsize_diag_dmat(int m, int n); +int test_blasfeo_memsize_dvec(int m); + +// --- creation +void test_blasfeo_create_dmat(int m, int n, struct blasfeo_dmat *sA, void *memory); +void test_blasfeo_create_dvec(int m, struct blasfeo_dvec *sA, void *memory); + +// --- conversion +void test_blasfeo_pack_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_pack_dvec(int m, double *x, int xi, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_pack_tran_dmat(int m, int n, double *A, int lda, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_unpack_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda); +void test_blasfeo_unpack_dvec(int m, struct blasfeo_dvec *sa, int ai, double *x, int xi); +void test_blasfeo_unpack_tran_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, double *A, int lda); + +// --- cast +void test_d_cast_mat2strmat(double *A, struct blasfeo_dmat *sA); +void test_d_cast_diag_mat2strmat(double *dA, struct blasfeo_dmat *sA); +void test_d_cast_vec2vecmat(double *a, struct blasfeo_dvec *sa); + +// ------ copy / scale + +// B <= A +void test_blasfeo_dgecp(int m, int n, + struct blasfeo_dmat *sA, int ai, int aj, + struct blasfeo_dmat *sB, int bi, int bj); + +// A <= alpha*A +void test_blasfeo_dgesc(int m, int n, + double alpha, + struct blasfeo_dmat *sA, int ai, int aj); + +// B <= alpha*A +void test_blasfeo_dgecpsc(int m, int n, + double alpha, + struct blasfeo_dmat *sA, int ai, int aj, + struct blasfeo_dmat *sB, int bi, int bj); + +// // --- insert/extract +// // +// // <= sA[ai, aj] +// void test_blasfeo_dgein1(double a, struct blasfeo_dmat *sA, int ai, int aj); +// // <= sA[ai, aj] +// double blasfeo_dgeex1(struct blasfeo_dmat *sA, int ai, int aj); +// // sx[xi] <= a +// void test_blasfeo_dvecin1(double a, struct blasfeo_dvec *sx, int xi); +// // <= sx[xi] +// double blasfeo_dvecex1(struct blasfeo_dvec *sx, int xi); +// // A <= alpha + +// // --- set +// void test_blasfeo_dgese(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// // a <= alpha +// void test_blasfeo_dvecse(int m, double alpha, struct blasfeo_dvec *sx, int xi); +// // B <= A + + +// // --- vector +// // y <= x +// void test_blasfeo_dveccp(int m, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci); +// // x <= alpha*x +// void test_blasfeo_dvecsc(int m, double alpha, struct blasfeo_dvec *sa, int ai); +// // TODO +// // x <= alpha*x +// void test_blasfeo_dveccpsc(int m, double alpha, struct blasfeo_dvec *sa, int ai, struct blasfeo_dvec *sc, int ci); + + +// // B <= A, A lower triangular +// void test_blasfeo_dtrcp_l(int m, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sB, int bi, int bj); + +// void test_blasfeo_dtrcpsc_l(int m, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sB, int bi, int bj); + +// void test_blasfeo_dtrsc_l(int m, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj); + + +// // B <= B + alpha*A +// void test_blasfeo_dgead(int m, int n, double alpha, +// struct blasfeo_dmat *sA, int ai, int aj, +// struct blasfeo_dmat *sC, int ci, int cj); + +// // y <= y + alpha*x +// void test_blasfeo_dvecad(int m, double alpha, +// struct blasfeo_dvec *sa, int ai, +// struct blasfeo_dvec *sc, int ci); + +// // --- traspositions +// void test_dgetr_lib(int m, int n, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dgetr(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_dtrtr_l_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dtrtr_l(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_dtrtr_u_lib(int m, double alpha, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dtrtr_u(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_ddiare(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj); +// void test_blasfeo_ddiain(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_ddiaex_lib(int kmax, double alpha, int offset, double *pD, int sdd, double *x); +// void test_ddiaad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +// void test_ddiain_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +// void test_blasfeo_ddiain_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_ddiaex_libsp(int kmax, int *idx, double alpha, double *pD, int sdd, double *x); +// void test_blasfeo_ddiaex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_ddiaex_sp(int kmax, double alpha, int *idx, struct blasfeo_dmat *sD, int di, int dj, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_ddiaad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_ddiaad_libsp(int kmax, int *idx, double alpha, double *x, double *pD, int sdd); +// void test_blasfeo_ddiaad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_ddiaadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD, int sdd); +// void test_blasfeo_ddiaadin_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_drowin_lib(int kmax, double alpha, double *x, double *pD); +// void test_blasfeo_drowin(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_drowex_lib(int kmax, double alpha, double *pD, double *x); +// void test_blasfeo_drowex(int kmax, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_drowad_lib(int kmax, double alpha, double *x, double *pD); +// void test_blasfeo_drowad(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_drowin_libsp(int kmax, double alpha, int *idx, double *x, double *pD); +// void test_drowad_libsp(int kmax, int *idx, double alpha, double *x, double *pD); +// void test_blasfeo_drowad_sp(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dmat *sD, int di, int dj); +// void test_drowadin_libsp(int kmax, int *idx, double alpha, double *x, double *y, double *pD); +// void test_drowsw_lib(int kmax, double *pA, double *pC); +// void test_blasfeo_drowsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_drowpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +// void test_blasfeo_dcolex(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi); +// void test_dcolin_lib(int kmax, double *x, int offset, double *pD, int sdd); +// void test_blasfeo_dcolin(int kmax, struct blasfeo_dvec *sx, int xi, struct blasfeo_dmat *sA, int ai, int aj); +// void test_dcolad_lib(int kmax, double alpha, double *x, int offset, double *pD, int sdd); +// void test_dcolin_libsp(int kmax, int *idx, double *x, double *pD, int sdd); +// void test_dcolad_libsp(int kmax, double alpha, int *idx, double *x, double *pD, int sdd); +// void test_dcolsw_lib(int kmax, int offsetA, double *pA, int sda, int offsetC, double *pC, int sdc); +// void test_blasfeo_dcolsw(int kmax, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sC, int ci, int cj); +// void test_blasfeo_dcolpe(int kmax, int *ipiv, struct blasfeo_dmat *sA); +// void test_dvecin_libsp(int kmax, int *idx, double *x, double *y); +// void test_dvecad_libsp(int kmax, int *idx, double alpha, double *x, double *y); +// void test_blasfeo_dvecad_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dvecin_sp(int m, double alpha, struct blasfeo_dvec *sx, int xi, int *idx, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dvecex_sp(int m, double alpha, int *idx, struct blasfeo_dvec *sx, int x, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dveccl(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi); +// void test_blasfeo_dveccl_mask(int m, struct blasfeo_dvec *sxm, int xim, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sxp, int xip, struct blasfeo_dvec *sz, int zi, struct blasfeo_dvec *sm, int mi); +// void test_blasfeo_dvecze(int m, struct blasfeo_dvec *sm, int mi, struct blasfeo_dvec *sv, int vi, struct blasfeo_dvec *se, int ei); +// void test_blasfeo_dvecnrm_inf(int m, struct blasfeo_dvec *sx, int xi, double *ptr_norm); +// void test_blasfeo_dvecpe(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); +// void test_blasfeo_dvecpei(int kmax, int *ipiv, struct blasfeo_dvec *sx, int xi); + +// ext_dep + +void test_blasfeo_allocate_dmat(int m, int n, struct blasfeo_dmat *sA); +void test_blasfeo_allocate_dvec(int m, struct blasfeo_dvec *sa); + +void test_blasfeo_free_dmat(struct blasfeo_dmat *sA); +void test_blasfeo_free_dvec(struct blasfeo_dvec *sa); + +void test_blasfeo_print_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_dvec(int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); + +void test_blasfeo_print_to_file_dmat(FILE *file, int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_to_file_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_to_file_tran_dvec(FILE *file, int m, struct blasfeo_dvec *sa, int ai); + +void test_blasfeo_print_exp_dmat(int m, int n, struct blasfeo_dmat *sA, int ai, int aj); +void test_blasfeo_print_exp_dvec(int m, struct blasfeo_dvec *sa, int ai); +void test_blasfeo_print_exp_tran_dvec(int m, struct blasfeo_dvec *sa, int ai); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_AUX_TEST_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h new file mode 100644 index 0000000..ea8d006 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas.h @@ -0,0 +1,46 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLAS_H_ +#define BLASFEO_D_BLAS_H_ + + + +#include "blasfeo_d_blasfeo_api.h" +#include "blasfeo_d_blas_api.h" + + + +#endif // BLASFEO_D_BLAS_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h new file mode 100644 index 0000000..2eab1e4 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blas_api.h @@ -0,0 +1,281 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef BLASFEO_D_BLAS_API_H_ +#define BLASFEO_D_BLAS_API_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef BLAS_API +#ifdef CBLAS_API +#ifndef BLASFEO_CBLAS_ENUM +#define BLASFEO_CBLAS_ENUM +#ifdef FORTRAN_BLAS_API +#ifndef CBLAS_H +enum CBLAS_LAYOUT {CblasRowMajor=101, CblasColMajor=102}; +enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; +enum CBLAS_UPLO {CblasUpper=121, CblasLower=122}; +enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; +enum CBLAS_SIDE {CblasLeft=141, CblasRight=142}; +#define CBLAS_ORDER CBLAS_LAYOUT /* this for backward compatibility with CBLAS_ORDER */ +#endif // CBLAS_H +#else // FORTRAN_BLAS_API +enum BLASFEO_CBLAS_LAYOUT {BlasfeoCblasRowMajor=101, BlasfeoCblasColMajor=102}; +enum BLASFEO_CBLAS_TRANSPOSE {BlasfeoCblasNoTrans=111, BlasfeoCblasTrans=112, BlasfeoCblasConjTrans=113}; +enum BLASFEO_CBLAS_UPLO {BlasfeoCblasUpper=121, BlasfeoCblasLower=122}; +enum BLASFEO_CBLAS_DIAG {BlasfeoCblasNonUnit=131, BlasfeoCblasUnit=132}; +enum BLASFEO_CBLAS_SIDE {BlasfeoCblasLeft=141, BlasfeoCblasRight=142}; +#define BLASFEO_CBLAS_ORDER BLASFEO_CBLAS_LAYOUT /* this for backward compatibility with BLASFEO_CBLAS_ORDER */ +#endif // FORTRAN_BLAS_API +#endif // BLASFEO_CBLAS_ENUM +#endif // CBLAS_API +#endif // BLAS_API + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef BLAS_API + + + +#ifdef FORTRAN_BLAS_API + + + +// BLAS 1 +// +void daxpy_(int *n, double *alpha, double *x, int *incx, double *y, int *incy); +// +void dcopy_(int *n, double *x, int *incx, double *y, int *incy); +// +double ddot_(int *n, double *x, int *incx, double *y, int *incy); + +// BLAS 2 +// +void dgemv_(char *tran, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +// +void dsymv_(char *uplo, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +// +void dger_(int *m, int *n, double *alpha, double *x, int *incx, double *y, int *incy, double *A, int *lda); + +// BLAS 3 +// +void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +// +void dsyrk_(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +// +void dtrmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void dtrsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void dsyr2k_(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); + + + +// LAPACK +// +void dgesv_(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +// +void dgetrf_np_(int *m, int *n, double *A, int *lda, int *info); +// +void dgetrs_(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void dlaswp_(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx); +// +void dposv_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); +// +void dpotrs_(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void dtrtrs_(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); + + + +// aux +void dgetr_(int *m, int *n, double *A, int *lda, double *B, int *ldb); + + + +#ifdef CBLAS_API + + + +// CBLAS 1 +// +void cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY); +// +void cblas_dswap(const int N, double *X, const int incX, double *Y, const int incY); +// +void cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); + +// CBLAS 2 +// +void cblas_dgemv(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); + +// CBLAS 3 +// +void cblas_dgemm(const enum CBLAS_LAYOUT layout, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +// +void cblas_dsyrk(const enum CBLAS_LAYOUT layout, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +// +void cblas_dtrmm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +// +void cblas_dtrsm(const enum CBLAS_LAYOUT layout, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); + + + +#endif // CBLAS_API + + + +#else // BLASFEO_API + + + +// BLAS 1 +// +void blasfeo_blas_daxpy(int *n, double *alpha, double *x, int *incx, double *y, int *incy); +// +double blasfeo_blas_ddot(int *n, double *x, int *incx, double *y, int *incy); +// +void blasfeo_blas_dcopy(int *n, double *x, int *incx, double *y, int *incy); + +// BLAS 2 +// +void blasfeo_blas_dgemv(char *trans, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +// +void blasfeo_blas_dsymv(char *uplo, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +// +void blasfeo_blas_dger(int *m, int *n, double *alpha, double *x, int *incx, double *y, int *incy, double *A, int *lda); + +// BLAS 3 +// +void blasfeo_blas_dgemm(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +// +void blasfeo_blas_dsyrk(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +// +void blasfeo_blas_dtrmm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void blasfeo_blas_dtrsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +// +void blasfeo_blas_dsyr2k(char *uplo, char *ta, int *m, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); + + + +// LAPACK +// +void blasfeo_lapack_dgesv(int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void blasfeo_lapack_dgetrf(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +// +void blasfeo_lapack_dgetrf_np(int *m, int *n, double *A, int *lda, int *info); +// +void blasfeo_lapack_dgetrs(char *trans, int *m, int *n, double *A, int *lda, int *ipiv, double *B, int *ldb, int *info); +// +void blasfeo_lapack_dlaswp(int *n, double *A, int *lda, int *k1, int *k2, int *ipiv, int *incx); +// +void blasfeo_lapack_dposv(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void blasfeo_lapack_dpotrf(char *uplo, int *m, double *A, int *lda, int *info); +// +void blasfeo_lapack_dpotrs(char *uplo, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); +// +void blasfeo_lapack_dtrtrs(char *uplo, char *trans, char *diag, int *m, int *n, double *A, int *lda, double *B, int *ldb, int *info); + + + +// aux +void blasfeo_blas_dgetr(int *m, int *n, double *A, int *lda, double *B, int *ldb); + + + +#ifdef CBLAS_API + + + +// CBLAS 1 +// +void blasfeo_cblas_daxpy(const int N, const double alpha, const double *X, const int incX, double *Y, const int incY); +// +void blasfeo_cblas_dswap(const int N, double *X, const int incX, double *Y, const int incY); +// +void blasfeo_cblas_dcopy(const int N, const double *X, const int incX, double *Y, const int incY); + +// CBLAS 2 +// +void blasfeo_cblas_dgemv(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const int M, const int N, const double alpha, const double *A, const int lda, const double *X, const int incX, const double beta, double *Y, const int incY); + +// CBLAS 3 +// +void blasfeo_cblas_dgemm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const double alpha, const double *A, const int lda, const double *B, const int ldb, const double beta, double *C, const int ldc); +// +void blasfeo_cblas_dsyrk(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE Trans, const int N, const int K, const double alpha, const double *A, const int lda, const double beta, double *C, const int ldc); +// +void blasfeo_cblas_dtrmm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); +// +void blasfeo_cblas_dtrsm(const enum BLASFEO_CBLAS_LAYOUT layout, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const double alpha, const double *A, const int lda, double *B, const int ldb); + + + +#endif // CBLAS_API + + + +#endif // BLASFEO_API + + + +#endif // BLAS_API + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLAS_API_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h new file mode 100644 index 0000000..88bb3fc --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h @@ -0,0 +1,364 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_API_H_ +#define BLASFEO_D_BLASFEO_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +// level 1 BLAS +// + +// z = y + alpha*x +// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n] +// NB: Different arguments semantic compare to equivalent standard BLAS routine +void blasfeo_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y +void blasfeo_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z += x .* y +void blasfeo_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +double blasfeo_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// return x^T * y +double blasfeo_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// construct givens plane rotation +void blasfeo_drotg(double a, double b, double *c, double *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A^T * x +void blasfeo_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(n) +void blasfeo_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A^T ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_dtrmv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_dtrmv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A lower triangular +void blasfeo_dtrmv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A lower triangular, unit diagonal +void blasfeo_dtrmv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A^T * x ; A upper triangular +void blasfeo_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A^T * x_t +void blasfeo_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_dsymv_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +void blasfeo_dsymv_l_mn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x, where A is symmetric and only the upper triangular patr of A is accessed +void blasfeo_dsymv_u(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// D = C + alpha * x * y^T +void blasfeo_dger(int m, int n, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B^T +void blasfeo_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#if ( defined(LA_HIGH_PERFORMANCE) & defined(MF_COLMAJ) ) +void blasfeo_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +#endif +// D <= alpha * A * B ; A lower triangular +void blasfeo_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +// D <= alpha * A^{-1} * B , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular with unit diagonal +void blasfeo_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular with unit diagonal +void blasfeo_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T; C, D lower triangular +void blasfeo_dsyr2k_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A; C, D lower triangular +void blasfeo_dsyr2k_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T; C, D upper triangular +void blasfeo_dsyr2k_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A; C, D upper triangular +void blasfeo_dsyr2k_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +// D <= chol( C + A * B^T ) ; C, D lower triangular +void blasfeo_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +int blasfeo_dgeqrf_worksize(int m, int n); // in bytes +void blasfeo_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_dorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +int blasfeo_dgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work); + + + +// +// BLAS API helper functions +// + +#if ( defined(BLAS_API) & defined(MF_PANELMAJ) ) +// BLAS 3 +void blasfeo_cm_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyrk3_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_runn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_runu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyr2k_ln(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyr2k_lt(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyr2k_un(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dsyr2k_ut(int m, int k, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dmat *sB, int bi, int bj, double beta, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +// BLAS 2 +void blasfeo_cm_dgemv_n(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dvec *sx, int xi, double beta, struct blasfeo_cm_dvec *sy, int yi, struct blasfeo_cm_dvec *sz, int zi); +void blasfeo_cm_dgemv_t(int m, int n, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dvec *sx, int xi, double beta, struct blasfeo_cm_dvec *sy, int yi, struct blasfeo_cm_dvec *sz, int zi); +void blasfeo_cm_dsymv_l(int m, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dvec *sx, int xi, double beta, struct blasfeo_cm_dvec *sy, int yi, struct blasfeo_cm_dvec *sz, int zi); +void blasfeo_cm_dsymv_u(int m, double alpha, struct blasfeo_cm_dmat *sA, int ai, int aj, struct blasfeo_cm_dvec *sx, int xi, double beta, struct blasfeo_cm_dvec *sy, int yi, struct blasfeo_cm_dvec *sz, int zi); +void blasfeo_cm_dger(int m, int n, double alpha, struct blasfeo_cm_dvec *sx, int xi, struct blasfeo_cm_dvec *sy, int yi, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +// LAPACK +void blasfeo_cm_dpotrf_l(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dpotrf_u(int m, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj); +void blasfeo_cm_dgetrf_rp(int m, int n, struct blasfeo_cm_dmat *sC, int ci, int cj, struct blasfeo_cm_dmat *sD, int di, int dj, int *ipiv); +#endif + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_API_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h new file mode 100644 index 0000000..141a1f0 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api_ref.h @@ -0,0 +1,147 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_API_REF_H_ +#define BLASFEO_D_BLASFEO_API_REF_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +// expose reference BLASFEO for testing + +// --- level 1 + +void blasfeo_daxpy_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_daxpby_ref(int kmax, double alpha, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dvecmul_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dvecmulacc_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +double blasfeo_dvecmuldot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +double blasfeo_ddot_ref(int m, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sy, int yi); +void blasfeo_drotg_ref(double a, double b, double *c, double *s); +void blasfeo_dcolrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj0, int aj1, double c, double s); +void blasfeo_drowrot_ref(int m, struct blasfeo_dmat_ref *sA, int ai0, int ai1, int aj, double c, double s); + + +// --- level 2 + +// dense +void blasfeo_dgemv_n_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dgemv_t_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltn_mn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_lnu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_ltu_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrsv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_unn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_utn_ref(int m, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_lnn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_ltn_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_lnu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dtrmv_ltu_ref(int m, int n, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, struct blasfeo_dvec_ref *sz, int zi); +void blasfeo_dgemv_nt_ref(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx_n, int xi_n, struct blasfeo_dvec_ref *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec_ref *sy_n, int yi_n, struct blasfeo_dvec_ref *sy_t, int yi_t, struct blasfeo_dvec_ref *sz_n, int zi_n, struct blasfeo_dvec_ref *sz_t, int zi_t); +void blasfeo_dsymv_l_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); + +// diagonal +void blasfeo_dgemv_d_ref(int m, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dvec_ref *sx, int xi, double beta, struct blasfeo_dvec_ref *sy, int yi, struct blasfeo_dvec_ref *sz, int zi); + + +// --- level 3 + +// dense +void blasfeo_dgemm_nn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_nt_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_tn_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_tt_ref(int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dsyrk_ln_mn_ref( int m, int n, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_ln_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_lt_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_un_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_ut_ref( int m, int k, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dtrmm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrmm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); + +void blasfeo_dtrsm_lunu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lunn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_llnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_llnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_lltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_runu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_runn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rutu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rutn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rlnu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rlnn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rltu_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dtrsm_rltn_ref( int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sD, int di, int dj); + +// diagonal +void dgemm_diag_left_lib_ref(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_dgemm_dn_ref(int m, int n, double alpha, struct blasfeo_dvec_ref *sA, int ai, struct blasfeo_dmat_ref *sB, int bi, int bj, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgemm_nd_ref(int m, int n, double alpha, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dvec_ref *sB, int bi, double beta, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); + +// --- lapack + +void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_dpotrf_l_ref(int m, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dpotrf_l_mn_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dsyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_dmat_ref *sA, int ai, int aj, struct blasfeo_dmat_ref *sB, int bi, int bj, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_nopivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj); +void blasfeo_dgetrf_rowpivot_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_dgeqrf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_pd_ref(int m, int n, struct blasfeo_dmat_ref *sC, int ci, int cj, struct blasfeo_dmat_ref *sD, int di, int dj, void *work); +void blasfeo_dgelqf_pd_la_ref(int m, int n1, struct blasfeo_dmat_ref *sL, int li, int lj, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work); +void blasfeo_dgelqf_pd_lla_ref(int m, int n1, struct blasfeo_dmat_ref *sL0, int l0i, int l0j, struct blasfeo_dmat_ref *sL1, int l1i, int l1j, struct blasfeo_dmat_ref *sA, int ai, int aj, void *work); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_API_REF_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_hp_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_hp_api.h new file mode 100644 index 0000000..405733f --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_hp_api.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_HP_API_H_ +#define BLASFEO_D_BLASFEO_HP_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +// level 3 BLAS +// + +// dense + + +// D <= beta * C + alpha * A^T * B +void blasfeo_hp_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T; C, D lower triangular +void blasfeo_hp_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * A^T ; C, D lower triangular +void blasfeo_hp_dsyrk3_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular +void blasfeo_hp_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_hp_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_HP_API_H_ + diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h new file mode 100644 index 0000000..1a8b22c --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h @@ -0,0 +1,283 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_BLASFEO_REF_API_H_ +#define BLASFEO_D_BLASFEO_REF_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +// level 1 BLAS +// + +// z = y + alpha*x +// z[zi:zi+n] = alpha*x[xi:xi+n] + y[yi:yi+n] +// NB: Different arguments semantic compare to equivalent standard BLAS routine +void blasfeo_ref_daxpy(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_ref_daxpby(int kmax, double alpha, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y +void blasfeo_ref_dvecmul(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z += x .* y +void blasfeo_ref_dvecmulacc(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +double blasfeo_ref_dvecmuldot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// return x^T * y +double blasfeo_ref_ddot(int m, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi); +// construct givens plane rotation +void blasfeo_ref_drotg(double a, double b, double *c, double *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_ref_dcolrot(int m, struct blasfeo_dmat *sA, int ai, int aj0, int aj1, double c, double s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_ref_drowrot(int m, struct blasfeo_dmat *sA, int ai0, int ai1, int aj, double c, double s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_ref_dgemv_n(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_ref_dgemv_t(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_ref_dtrsv_lnn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_ref_dtrsv_ltn_mn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_ref_dtrsv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_ref_dtrsv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_ref_dtrsv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_ref_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_ref_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_ref_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_ref_dtrmv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_ref_dtrmv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_ref_dtrmv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A lower triangular, unit diagonal +void blasfeo_ref_dtrmv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_ref_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_ref_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_ref_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ref_dsymv_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +void blasfeo_ref_dsymv_l_mn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// z <= beta * y + alpha * A * x, where A is symmetric and only the upper triangular patr of A is accessed +void blasfeo_ref_dsymv_u(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); +// D = C + alpha * x * y^T +void blasfeo_ref_dger(int m, int n, double alpha, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sy, int yi, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_ref_dgemv_d(int m, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_ref_dgemm_nn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_ref_dgemm_nt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_dgemm_tn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B^T +void blasfeo_ref_dgemm_tt(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ref_dsyrk_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dsyrk_ln_mn(int m, int n, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ref_dsyrk_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ref_dsyrk_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ref_dsyrk_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_ref_dtrmm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A lower triangular +void blasfeo_ref_dtrmm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_ref_dtrmm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A lower triangular +void blasfeo_ref_dtrmm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_ref_dtrmm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B ; A upper triangular +void blasfeo_ref_dtrmm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_ref_dtrmm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^T * B ; A upper triangular +void blasfeo_ref_dtrmm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_dtrmm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_dtrmm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_ref_dtrmm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A lower triangular +void blasfeo_ref_dtrmm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_ref_dtrmm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A ; A upper triangular +void blasfeo_ref_dtrmm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_ref_dtrmm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^T ; A upper triangular +void blasfeo_ref_dtrmm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_dtrsm_llnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_llnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_dtrsm_lltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_lltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_lunn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_ref_dtrsm_lunu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_lutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_ref_dtrsm_lutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rlnn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_rlnu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rltn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_ref_dtrsm_rltu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_runn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_ref_dtrsm_runu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_dtrsm_rutn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_ref_dtrsm_rutu(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T ; C, D lower triangular +void blasfeo_ref_dsyr2k_ln(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A ; C, D lower triangular +void blasfeo_ref_dsyr2k_lt(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T ; C, D upper triangular +void blasfeo_ref_dsyr2k_un(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A ; C, D upper triangular +void blasfeo_ref_dsyr2k_ut(int m, int k, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void dgemm_diag_left_lib(int m, int n, double alpha, double *dA, double *pB, int sdb, double beta, double *pC, int sdc, double *pD, int sdd); +void blasfeo_ref_dgemm_dn(int m, int n, double alpha, struct blasfeo_dvec *sA, int ai, struct blasfeo_dmat *sB, int bi, int bj, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_ref_dgemm_nd(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sB, int bi, double beta, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_ref_dpotrf_l(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dpotrf_l_mn(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_ref_dpotrf_u(int m, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ref_dsyrk_dpotrf_ln(int m, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +void blasfeo_ref_dsyrk_dpotrf_ln_mn(int m, int n, int k, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dmat *sB, int bi, int bj, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_ref_dgetrf_np(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_ref_dgetrf_rp(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +int blasfeo_ref_dgeqrf_worksize(int m, int n); // in bytes +void blasfeo_ref_dgeqrf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_ref_dorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_ref_dorglq(int m, int n, int k, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_ref_dgelqf(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +int blasfeo_ref_dgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_ref_dgelqf_pd(int m, int n, struct blasfeo_dmat *sC, int ci, int cj, struct blasfeo_dmat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_dgelqf_pd_la(int m, int n1, struct blasfeo_dmat *sL, int li, int lj, struct blasfeo_dmat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_dgelqf_pd_lla(int m, int n1, struct blasfeo_dmat *sL0, int l0i, int l0j, struct blasfeo_dmat *sL1, int l1i, int l1j, struct blasfeo_dmat *sA, int ai, int aj, void *work); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_BLASFEO_REF_API_H_ + diff --git a/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h b/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h new file mode 100644 index 0000000..d20ac38 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h @@ -0,0 +1,1321 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_D_KERNEL_H_ +#define BLASFEO_D_KERNEL_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// utils +void blasfeo_align_2MB(void *ptr, void **ptr_align); +void blasfeo_align_4096_byte(void *ptr, void **ptr_align); +void blasfeo_align_64_byte(void *ptr, void **ptr_align); + + +// +// lib8 +// + +// 24x8 +void kernel_dgemm_nt_24x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_24x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dtrsm_nt_rl_inv_24x8_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); // +void kernel_dpotrf_nt_l_24x8_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_24x8_vs_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_24x8_vs_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); // +void kernel_dgemm_dtrsm_nt_rl_inv_24x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_24x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); +void kernel_dsyrk_dpotrf_nt_l_24x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_24x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1); +void kernel_dlarfb8_rn_24_lib8(int kmax, double *pV, double *pT, double *pD, int sdd); +// 16x8 +void kernel_dgemm_nt_16x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_16x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nt_16x8_gen_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_nn_16x8_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_16x8_vs_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_16x8_gen_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dsyrk_nt_l_16x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nt_l_16x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_16x8_gen_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrmm_nn_rl_16x8_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd); +void kernel_dtrmm_nn_rl_16x8_vs_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int m1, int n1); +void kernel_dtrmm_nn_rl_16x8_gen_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_16x8_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); // +void kernel_dtrsm_nt_rl_inv_16x8_vs_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); // +void kernel_dpotrf_nt_l_16x8_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_16x8_vs_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1); +void kernel_dgemm_dtrsm_nt_rl_inv_16x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_16x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); +void kernel_dsyrk_dpotrf_nt_l_16x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_16x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1); +void kernel_dlarfb8_rn_16_lib8(int kmax, double *pV, double *pT, double *pD, int sdd); +void kernel_dlarfb8_rn_la_16_lib8(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda); +void kernel_dlarfb8_rn_lla_16_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda); +// 8x16 +void kernel_dgemm_tt_8x16_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_tt_8x16_vs_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_8x16_gen_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_nt_8x16_lib8(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nt_8x16_vs_lib8(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +// 8x8 +void kernel_dgemm_nt_8x8_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_nt_8x8_vs_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nt_8x8_gen_lib8(int k, double *alpha, double *A, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_nn_8x8_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_8x8_vs_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nn_8x8_gen_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_8x8_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_tt_8x8_vs_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_8x8_gen_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, int offc, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dsyrk_nt_l_8x8_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_8x8_vs_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dsyrk_nt_l_8x8_gen_lib8(int k, double *alpha, double *A, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrmm_nn_rl_8x8_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D); +void kernel_dtrmm_nn_rl_8x8_vs_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1); +void kernel_dtrmm_nn_rl_8x8_gen_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_8x8_lib8(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_8x8_vs_lib8(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int m1, int n1); +void kernel_dpotrf_nt_l_8x8_lib8(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_8x8_vs_lib8(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int m1, int n1); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8_vs_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int m1, int n1); +void kernel_dsyrk_dpotrf_nt_l_8x8_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int m1, int n1); +void kernel_dgelqf_vs_lib8(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dgelqf_pd_vs_lib8(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dgelqf_8_lib8(int kmax, double *pD, double *dD); +void kernel_dgelqf_pd_8_lib8(int kmax, double *pD, double *dD); +void kernel_dlarft_8_lib8(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarfb8_rn_8_lib8(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb8_rn_8_vs_lib8(int kmax, double *pV, double *pT, double *pD, int m1); +void kernel_dlarfb8_rn_1_lib8(int kmax, double *pV, double *pT, double *pD); +void kernel_dgelqf_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pT); +void kernel_dgelqf_pd_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pT); +void kernel_dgelqf_pd_la_vs_lib8(int m, int n1, int k, int offD, double *pD, int sdd, double *dD, int offA, double *pA, int sda); +void kernel_dgelqf_pd_la_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pA, double *pT); +void kernel_dlarft_la_8_lib8(int n1, double *dD, double *pA, double *pT); +void kernel_dlarfb8_rn_la_8_lib8(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dlarfb8_rn_la_8_vs_lib8(int n1, double *pVA, double *pT, double *pD, double *pA, int m1); +void kernel_dlarfb8_rn_la_1_lib8(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dgelqf_pd_lla_vs_lib8(int m, int n0, int n1, int k, int offD, double *pD, int sdd, double *dD, int offL, double *pL, int sdl, int offA, double *pA, int sda); +void kernel_dgelqf_pd_lla_dlarft8_8_lib8(int n0, int n1, double *pD, double *dD, double *pL, double *pA, double *pT); +void kernel_dlarft_lla_8_lib8(int n0, int n1, double *dD, double *pL, double *pA, double *pT); +void kernel_dlarfb8_rn_lla_8_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); +void kernel_dlarfb8_rn_lla_8_vs_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA, int m1); +void kernel_dlarfb8_rn_lla_1_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); + +// panel copy / pack +// 24 +void kernel_dpack_nn_24_lib8(int kmax, double *A, int lda, double *C, int sdc); +void kernel_dpack_nn_24_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1); +// 16 +void kernel_dpacp_nn_16_lib8(int kmax, int offsetA, double *A, int sda, double *B, int sdb); +void kernel_dpacp_nn_16_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1); +void kernel_dpack_nn_16_lib8(int kmax, double *A, int lda, double *C, int sdc); +void kernel_dpack_nn_16_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1); +// 8 +void kernel_dpacp_nn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_nn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1); +void kernel_dpacp_tn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_tn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1); +void kernel_dpacp_l_nn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_l_nn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1); +void kernel_dpacp_l_tn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_l_tn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1); +void kernel_dpaad_nn_8_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *B); +void kernel_dpaad_nn_8_vs_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *B, int m1); +void kernel_dpack_nn_8_lib8(int kmax, double *A, int lda, double *C); +void kernel_dpack_nn_8_vs_lib8(int kmax, double *A, int lda, double *C, int m1); +void kernel_dpack_tn_8_lib8(int kmax, double *A, int lda, double *C); +void kernel_dpack_tn_8_vs_lib8(int kmax, double *A, int lda, double *C, int m1); +// 4 +void kernel_dpack_tt_4_lib8(int kmax, double *A, int lda, double *C, int sdc); // TODO offsetC +void kernel_dpack_tt_4_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1); // TODO offsetC + +// level 2 BLAS +// 16 +void kernel_dgemv_n_16_lib8(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z); +// 8 +void kernel_dgemv_n_8_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z); +void kernel_dgemv_n_8_vs_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int m1); +//void kernel_dgemv_n_8_gen_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int m0, int m1); +void kernel_dgemv_n_8_gen_lib8(int k, double *alpha, int offsetA, double *A, double *x, double *beta, double *y, double *z, int m1); +void kernel_dgemv_t_8_lib8(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_8_vs_lib8(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z, int n1); +void kernel_dgemv_nt_8_lib8(int kmax, double *alpha_n, double *alpha_t, int offsetA, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t); +void kernel_dgemv_nt_8_vs_lib8(int kmax, double *alpha_n, double *alpha_t, int offsetA, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t, int n1); +void kernel_dsymv_l_8_lib8(int kmax, double *alpha, double *A, int sda, double *x, double *z); +void kernel_dsymv_l_8_vs_lib8(int kmax, double *alpha, double *A, int sda, double *x, double *z, int n1); +void kernel_dsymv_l_8_gen_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *x, double *z, int n1); +void kernel_dtrmv_n_ln_8_lib8(int k, double *A, double *x, double *z); +void kernel_dtrmv_n_ln_8_vs_lib8(int k, double *A, double *x, double *z, int m1); +void kernel_dtrmv_n_ln_8_gen_lib8(int k, int offsetA, double *A, double *x, double *z, int m1); +void kernel_dtrmv_t_ln_8_lib8(int k, double *A, int sda, double *x, double *z); +void kernel_dtrmv_t_ln_8_vs_lib8(int k, double *A, int sda, double *x, double *z, int n1); +void kernel_dtrmv_t_ln_8_gen_lib8(int k, int offsetA, double *A, int sda, double *x, double *z, int n1); +void kernel_dtrsv_n_l_inv_8_lib8(int k, double *A, double *inv_diag_A, double *x, double *z); +void kernel_dtrsv_n_l_inv_8_vs_lib8(int k, double *A, double *inv_diag_A, double *x, double *z, int m1, int n1); +void kernel_dtrsv_t_l_inv_8_lib8(int k, double *A, int sda, double *inv_diag_A, double *x, double *z); +void kernel_dtrsv_t_l_inv_8_vs_lib8(int k, double *A, int sda, double *inv_diag_A, double *x, double *z, int m1, int n1); + + + +// +// lib4 +// + +// level 2 BLAS +// 12 +void kernel_dgemv_n_12_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +// 8 +void kernel_dgemv_n_8_lib4(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dtrmv_un_8_lib4(int k, double *A, int sda, double *x, double *z); +// 4 +void kernel_dgemv_n_4_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z); +void kernel_dgemv_n_4_vs_lib4(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k1); +void kernel_dgemv_n_4_gen_lib4(int kmax, double *alpha, double *A, double *x, double *beta, double *y, double *z, int k0, int k1); +void kernel_dgemv_t_4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z, int k1); +void kernel_dtrsv_ln_inv_4_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ln_inv_4_vs_lib4(int k, double *A, double *inv_diag_A, double *x, double *y, double *z, int km, int kn); +void kernel_dtrsv_lt_inv_4_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_3_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_2_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_inv_1_lib4(int k, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_4_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_3_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_2_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_lt_one_1_lib4(int k, double *A, int sda, double *x, double *y, double *z); +void kernel_dtrsv_ln_one_4_vs_lib4(int kmax, double *A, double *x, double *y, double *z, int km, int kn); +void kernel_dtrsv_ln_one_4_lib4(int kmax, double *A, double *x, double *y, double *z); +void kernel_dtrsv_un_inv_4_lib4(int kmax, double *A, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ut_inv_4_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z); +void kernel_dtrsv_ut_inv_4_vs_lib4(int kmax, double *A, int sda, double *inv_diag_A, double *x, double *y, double *z, int m1, int n1); +void kernel_dtrmv_un_4_lib4(int k, double *A, double *x, double *z); +void kernel_dtrmv_ut_4_lib4(int k, double *A, int sda, double *x, double *z); +void kernel_dtrmv_ut_4_vs_lib4(int k, double *A, int sda, double *x, double *z, int km); +void kernel_dgemv_nt_6_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t); +void kernel_dgemv_nt_4_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t); +void kernel_dgemv_nt_4_vs_lib4(int kmax, double *alpha_n, double *alpha_t, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t, int km); +void kernel_dsymv_l_4_lib4(int kmax, double *alpha, double *A, int sda, double *x, double *z); +void kernel_dsymv_l_4_gen_lib4(int kmax, double *alpha, int offA, double *A, int sda, double *x, double *z, int km); + + + +// level 3 BLAS +// 12x4 +void kernel_dgemm_nt_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nt_12x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dgemm_nn_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nn_12x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_12x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_tt_12x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nn_u_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nt_l_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nt_ru_12x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); // +void kernel_dtrmm_nt_ru_12x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nn_rl_12x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd); +void kernel_dtrmm_nn_rl_12x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn); +void kernel_dtrsm_nt_rl_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_12x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde); +void kernel_dtrsm_nn_ll_one_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn); +void kernel_dtrsm_nn_lu_inv_12x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_12x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x12 +void kernel_dgemm_nt_4x12_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x12_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nn_4x12_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x12_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x12_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x12_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x12_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrsm_nt_rl_inv_4x12_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x12_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x8 +void kernel_dgemm_nt_8x8l_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8u_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8l_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11] +void kernel_dgemm_nt_8x8u_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [A00 *; A10 A11] +void kernel_dsyrk_nn_u_8x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_8x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_8x8_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // computes [L00 *; A10 L11] +void kernel_dsyrk_nt_l_8x8_vs_lib4(int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // computes [L00 *; A10 L11] +void kernel_dtrsm_nt_rl_inv_8x8l_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_8x8l_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_8x8u_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_8x8u_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x4 +void kernel_dgemm_nt_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nt_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dgemm_nt_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dgemm_nn_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_8x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_tt_8x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nn_u_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nn_u_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dsyrk_nt_l_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dsyrk_nt_l_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int km, int kn); // +void kernel_dsyrk_nt_l_8x4_gen_lib4(int k, double *alpha, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_dtrmm_nt_ru_8x4_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd); // +void kernel_dtrmm_nt_ru_8x4_vs_lib4(int k, double *alpha, double *A, int sda, double *B, double *D, int sdd, int km, int kn); // +void kernel_dtrmm_nn_rl_8x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd); +void kernel_dtrmm_nn_rl_8x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int km, int kn); +void kernel_dtrmm_nn_rl_8x4_gen_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_8x4_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde); +void kernel_dtrsm_nn_ll_one_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int sde, int km, int kn); +void kernel_dtrsm_nn_lu_inv_8x4_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_8x4_vs_lib4(int kmax, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x8 +void kernel_dgemm_nt_4x8_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x8_vs_lib4(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nn_4x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x8_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x8_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x8_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dtrsm_nt_rl_inv_4x8_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x8_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *E, int sed, double *inv_diag_E, int km, int kn); +// 8x2 +void kernel_dgemm_nn_8x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 2x8 +void kernel_dgemm_nn_2x8_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_2x8_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +// 10xX +void kernel_dgemm_nn_10x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_10x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_10x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_10x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 6xX +void kernel_dgemm_nn_8x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_8x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x8_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x8_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x6_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x6_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x4_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x4_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +void kernel_dgemm_nn_6x2_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); // +void kernel_dgemm_nn_6x2_vs_lib4(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); // +// 4x4 +void kernel_dgemm_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dgemm_nt_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dgemm_nn_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nn_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dgemm_tt_4x4_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D); // +void kernel_dgemm_tt_4x4_vs_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_tt_4x4_gen_lib4(int k, double *alpha, int offsetA, double *A, int sda, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_dsyrk_nn_u_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dsyrk_nn_u_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dsyrk_nt_l_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dsyrk_nt_l_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dsyrk_nt_u_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_u_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dsyrk_nt_u_4x4_gen_lib4(int k, double *alpha, double *A, double *B, double *beta, int offsetC, double *C, int sdc, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrmm_nt_ru_4x4_lib4(int k, double *alpha, double *A, double *B, double *D); // +void kernel_dtrmm_nt_ru_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *D, int km, int kn); // +void kernel_dtrmm_nn_rl_4x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D); +void kernel_dtrmm_nn_rl_4x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_gen_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, int offsetD, double *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nt_ru_inv_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_ru_one_4x4_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nn_ru_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_inv_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_4x4_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E); +void kernel_dtrsm_nn_ll_one_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *beta, double *C, double *D, double *E, int km, int kn); +void kernel_dtrsm_nn_lu_inv_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_lu_inv_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_lu_one_4x4_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E); +void kernel_dtrsm_nn_lu_one_4x4_vs_lib4(int kmax, double *A, double *B, int sdb, double *C, double *D, double *E, int km, int kn); +// 4x2 +void kernel_dgemm_nn_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +void kernel_dgemm_nt_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_nt_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); +void kernel_dsyrk_nt_l_4x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_4x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +void kernel_dtrmm_nn_rl_4x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D); +void kernel_dtrmm_nn_rl_4x2_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x2_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_rl_inv_4x2_vs_lib4(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +// 2x4 +void kernel_dgemm_nn_2x4_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dgemm_nn_2x4_vs_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); // +// 2x2 +void kernel_dgemm_nn_2x2_lib4(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_2x2_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dsyrk_nt_l_2x2_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +// diag +void kernel_dgemm_diag_right_4_a0_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *D, int sdd); +void kernel_dgemm_diag_right_4_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_3_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_2_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_right_1_lib4(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); +void kernel_dgemm_diag_left_4_a0_lib4(int kmax, double *alpha, double *A, double *B, double *D); +void kernel_dgemm_diag_left_4_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_3_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_2_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +void kernel_dgemm_diag_left_1_lib4(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D); +// low rank update +void kernel_dger4_sub_12r_lib4(int k, double *A, int sda, double *B, double *C, int sdc); +void kernel_dger4_sub_12r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km); +void kernel_dger4_sub_8r_lib4(int k, double *A, int sda, double *B, double *C, int sdc); +void kernel_dger4_sub_8r_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, int km); +void kernel_dger4_sub_4r_lib4(int n, double *A, double *B, double *C); +void kernel_dger4_sub_4r_vs_lib4(int n, double *A, double *B, double *C, int km); + + + +// LAPACK +// 12x4 +void kernel_dpotrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_l_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_l_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_m_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_m_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_r_12x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_r_12x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_l_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_l_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_m_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_m_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_r_12x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_r_12x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 8x8 +void kernel_dpotrf_nt_l_8x8_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_8x8_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 8x4 +void kernel_dpotrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dpotrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_l_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_l_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nn_r_8x4_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nn_r_8x4_vs_lib4(int k, double *A, int sda, double *B, int sdb, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_l_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_l_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_r_8x4_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dgetrf_nt_r_8x4_vs_lib4(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 4x4 +void kernel_dpotrf_nt_l_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +#if defined(TARGET_X64_INTEL_SANDY_BRIDGE) +void kernel_dlauum_nt_4x4_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); // +void kernel_dlauum_nt_4x4_vs_lib4(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int km, int kn); // +#endif +void kernel_dgetrf_nn_4x4_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D); +void kernel_dgetrf_nn_4x4_vs_lib4(int k, double *A, double *B, int sdb, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dgetrf_nt_4x4_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dgetrf_nt_4x4_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +void kernel_dgeqrf_4_lib4(int m, double *pD, int sdd, double *dD); +void kernel_dgeqrf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dlarf_4_lib4(int m, int n, double *pV, int sdv, double *tau, double *pC, int sdc); // rank-4 reflector +void kernel_dlarf_t_4_lib4(int m, int n, double *pD, int sdd, double *pVt, double *dD, double *pC0, int sdc, double *pW); +void kernel_dgelqf_4_lib4(int n, double *pD, double *dD); +void kernel_dgelqf_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dlarft_4_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_3_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_2_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dlarft_1_lib4(int kmax, double *pD, double *dD, double *pT); +void kernel_dgelqf_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT); +void kernel_dlarfb12_rn_12_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK); +void kernel_dlarfb12_rn_4_lib4(int kmax, double *pV, int sdd, double *pT, double *pD, double *pK, int km); +void kernel_dlarfb4_rn_12_lib4(int kmax, double *pV, double *pT, double *pD, int sdd); +void kernel_dlarfb4_rn_8_lib4(int kmax, double *pV, double *pT, double *pD, int sdd); +void kernel_dlarfb4_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rn_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rn_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rt_4_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb4_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb3_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb2_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dlarfb1_rt_1_lib4(int kmax, double *pV, double *pT, double *pD); +void kernel_dgelqf_pd_dlarft12_12_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_pd_dlarft4_8_lib4(int n, double *pD, int sdd, double *dD, double *pT); +void kernel_dgelqf_pd_dlarft4_4_lib4(int n, double *pD, double *dD, double *pT); +void kernel_dgelqf_pd_4_lib4(int n, double *pD, double *dD); +void kernel_dgelqf_pd_vs_lib4(int m, int n, int k, int offD, double *pD, int sdd, double *dD); +void kernel_dgelqf_pd_la_vs_lib4(int m, int n1, int k, int offD, double *pD, int sdd, double *dD, int offA, double *pA, int sda); +void kernel_dlarft_4_la_lib4(int n1, double *dD, double *pA, double *pT); +void kernel_dlarfb4_rn_12_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda); +void kernel_dlarfb4_rn_8_la_lib4(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda); +void kernel_dlarfb4_rn_4_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dlarfb4_rn_1_la_lib4(int n1, double *pVA, double *pT, double *pD, double *pA); +void kernel_dgelqf_pd_lla_vs_lib4(int m, int n0, int n1, int k, int offD, double *pD, int sdd, double *dD, int offL, double *pL, int sdl, int offA, double *pA, int sda); +void kernel_dlarft_4_lla_lib4(int n0, int n1, double *dD, double *pL, double *pA, double *pT); +void kernel_dlarfb4_rn_12_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda); +void kernel_dlarfb4_rn_8_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda); +void kernel_dlarfb4_rn_4_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); +void kernel_dlarfb4_rn_1_lla_lib4(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA); +// 4x2 +void kernel_dpotrf_nt_l_4x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_4x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +// 2x2 +void kernel_dpotrf_nt_l_2x2_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D); +void kernel_dpotrf_nt_l_2x2_vs_lib4(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int km, int kn); +// 12 +void kernel_dgetrf_pivot_12_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_12_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n); +// 8 +void kernel_dgetrf_pivot_8_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_8_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n); +// 4 +void kernel_dgetrf_pivot_4_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_4_vs_lib4(int m, double *pA, int sda, double *inv_diag_A, int* ipiv, int n1); +// vector +void kernel_drowsw_lib4(int kmax, double *pA, double *pC); + + + +// merged routines +// 12x4 +void kernel_dgemm_dtrsm_nt_rl_inv_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 4x12 +void kernel_dgemm_dtrsm_nt_rl_inv_4x12_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn); +// 8x8 +void kernel_dsyrk_dpotrf_nt_l_8x8_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdbp, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8l_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dgemm_dtrsm_nt_rl_inv_8x8u_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn); +// 8x4 +void kernel_dgemm_dtrsm_nt_rl_inv_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn); +// 4x8 +void kernel_dgemm_dtrsm_nt_rl_inv_4x8_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn); +// 4x4 +void kernel_dgemm_dtrsm_nt_rl_inv_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); +// 4x2 +void kernel_dgemm_dtrsm_nt_rl_inv_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E); +void kernel_dgemm_dtrsm_nt_rl_inv_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn); +void kernel_dsyrk_dpotrf_nt_l_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); +// 2x2 +void kernel_dsyrk_dpotrf_nt_l_2x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D); +void kernel_dsyrk_dpotrf_nt_l_2x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn); + +/* + * + * Auxiliary routines + * + * cpsc copy and scale, scale + * cp copy + * add + * set and scale + * transpose and scale + * set and scale + * + */ + +// copy and scale +void kernel_dgecpsc_8_0_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgecpsc_8_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B, int sdb); + +void kernel_dgecpsc_4_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_4_1_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_4_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_4_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_3_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_3_2_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgecpsc_3_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_2_0_lib4(int tri, int kmax, double alpha, double *A, double *B); +void kernel_dgecpsc_2_3_lib4(int tri, int kmax, double alpha, double *A0, int sda, double *B); + +void kernel_dgecpsc_1_0_lib4(int tri, int kmax, double alpha, double *A, double *B); + +// copy only +void kernel_dgecp_8_0_lib4(int tri, int kmax, double *A, int sda, double *B, int sdb); + +void kernel_dgecp_4_0_lib4(int tri, int kmax, double *A, double *B); + +// add +void kernel_dgead_8_0_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_1_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_2_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_8_3_lib4(int kmax, double alpha, double *A0, int sda, double *B, int sdb); +void kernel_dgead_4_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_4_1_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_4_2_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_4_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_3_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_3_2_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_3_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_2_0_lib4(int kmax, double alpha, double *A, double *B); +void kernel_dgead_2_3_lib4(int kmax, double alpha, double *A0, int sda, double *B); +void kernel_dgead_1_0_lib4(int kmax, double alpha, double *A, double *B); + +// set +void kernel_dgeset_4_lib4(int kmax, double alpha, double *A); +void kernel_dtrset_4_lib4(int kmax, double alpha, double *A); + +// traspose +void kernel_dgetr_8_lib4(int tri, int kmax, int kna, double alpha, double *A, int sda, double *C, int sdc); +void kernel_dgetr_4_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_3_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_2_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_1_lib4(int tri, int kmax, int kna, double alpha, double *A, double *C, int sdc); +void kernel_dgetr_4_0_lib4(int m, double *A, int sda, double *B); + + + +// pack +// 12 +void kernel_dpack_nn_12_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_nn_12_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +void kernel_dpack_tt_12_lib4(int kmax, double *A, int lda, double *B, int sdb); +// 8 +void kernel_dpack_nn_8_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_nn_8_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +void kernel_dpack_tt_8_lib4(int kmax, double *A, int lda, double *B, int sdb); +// 4 +void kernel_dpack_nn_4_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_nn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1); +void kernel_dpack_tn_4_p0_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_tn_4_lib4(int kmax, double *A, int lda, double *B); +void kernel_dpack_tn_4_vs_lib4(int kmax, double *A, int lda, double *B, int m1); +void kernel_dpack_tt_4_lib4(int kmax, double *A, int lda, double *B, int sdb); +void kernel_dpack_tt_4_vs_lib4(int kmax, double *A, int lda, double *B, int sdb, int m1); +// unpack +// 12 +void kernel_dunpack_nn_12_lib4(int kmax, double *A, int sda, double *B, int ldb); +void kernel_dunpack_nn_12_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1); +void kernel_dunpack_tt_12_lib4(int kmax, double *A, int sda, double *B, int ldb); +// 8 +void kernel_dunpack_nn_8_lib4(int kmax, double *A, int sda, double *B, int ldb); +void kernel_dunpack_nn_8_vs_lib4(int kmax, double *A, int sda, double *B, int ldb, int m1); +void kernel_dunpack_tt_8_lib4(int kmax, double *A, int sda, double *B, int ldb); +// 4 +void kernel_dunpack_nn_4_lib4(int kmax, double *A, double *B, int ldb); +void kernel_dunpack_nn_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1); +void kernel_dunpack_nt_4_lib4(int kmax, double *A, double *B, int ldb); +void kernel_dunpack_nt_4_vs_lib4(int kmax, double *A, double *B, int ldb, int m1); +void kernel_dunpack_tt_4_lib4(int kmax, double *A, int sda, double *B, int ldb); + +// panel copy +// 12 +void kernel_dpacp_nn_12_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb); +void kernel_dpacp_nn_12_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1); +// 8 +void kernel_dpacp_nn_8_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb); +void kernel_dpacp_nn_8_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1); +// 4 +void kernel_dpacp_nt_4_lib4(int kmax, double *A, int offsetB, double *B, int sdb); +void kernel_dpacp_tn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_nn_4_lib4(int kmax, int offsetA, double *A, int sda, double *B); +void kernel_dpacp_nn_4_vs_lib4(int kmax, int offsetA, double *A, int sda, double *B, int m1); + + + +/************************************************ +* BLAS API kernels +************************************************/ + +//#if defined(BLAS_API) + +// A, B panel-major bs=4; C, D column-major +// 12x4 +void kernel_dgemm_nt_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dsyrk_nt_l_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_12x4_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_12x4_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyr2k_nt_l_12x4_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyr2k_nt_l_12x4_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_12x4_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_12x4_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_12x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_12x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_ll_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nt_ll_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_12x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_12x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x12 +void kernel_dgemm_nt_4x12_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x12_p0_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1, double *A_p, double *B_p); +void kernel_dtrmm_nt_rl_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x12_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 8x8 +void kernel_dsyrk_nt_l_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_8x8_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_8x8_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_8x8_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, int sdb0, double *A1, int sda1, double *B1, int sdb1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_8x8_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, int sdb0, double *A1, int sda1, double *B1, int sdb1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyr2k_nt_l_8x8_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, int sdb0, double *A1, int sda1, double *B1, int sdb1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyr2k_nt_l_8x8_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, int sdb0, double *A1, int sda1, double *B1, int sdb1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dpotrf_nt_l_8x8_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_8x8_vs_lib44cc(int kmax, double *A, int sda, double *B, int sdb, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 8x4 +void kernel_dgemm_nt_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_p0_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dgemm_nt_8x4_p_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *A_p); +void kernel_dsyrk_nt_l_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_8x4_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_8x4_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyr2k_nt_l_8x4_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyr2k_nt_l_8x4_vs_lib44cc(int kmax, double *alpha, double *A0, int sda0, double *B0, double *A1, int sda1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_8x4_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_8x4_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_8x4_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_8x4_vs_lib44cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_ll_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nt_ll_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib44cc4(int kmax, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_8x4_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_8x4_vs_lib44cc(int kmax, double *A, int sda, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x8 +void kernel_dgemm_nt_4x8_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_lib44cc(int kmax, double *alpha, double *A, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x8_tran_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib444c(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nt_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_u_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_u_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_4x4_lib44cc(int kmax, double *alpha, double *A0, double *B0, double *A1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_4x4_vs_lib44cc(int kmax, double *alpha, double *A0, double *B0, double *A1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyr2k_nt_l_4x4_lib44cc(int kmax, double *alpha, double *A0, double *B0, double *A1, double *B1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyr2k_nt_l_4x4_vs_lib44cc(int kmax, double *alpha, double *A0, double *B0, double *A1, double *B1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_vs_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_tran_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib444c(int kmax, double *alpha, double *A, double *B, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nt_ll_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *inv_diag_E); +void kernel_dtrsm_nt_ll_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nt_rl_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, double *E, int m1, int n1); +void kernel_dpotrf_nt_l_4x4_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD); +void kernel_dpotrf_nt_l_4x4_vs_lib44cc(int kmax, double *A, double *B, double *C, int ldc, double *D, int ldd, double *dD, int m1, int n1); +// 4x2 +void kernel_dgemm_nt_4x2_lib44cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); + +// A panel-major bs=4; B, C, D column-major +// 12x4 +void kernel_dgemm_nn_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_12x4_lib4ccc(int kmax, double *alpha, double *A0, int sda0, double *B0, int ldb0, double *A1, int sda1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_12x4_vs_lib4ccc(int kmax, double *alpha, double *A0, int sda0, double *B0, int ldb0, double *A1, int sda1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_12x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_12x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_inv_12x4_lib4ccc4(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_12x4_vs_lib4ccc4(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_l_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_l_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_m_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_m_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_r_12x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_r_12x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +// 4x12 +void kernel_dgemm_nn_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x12_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x12_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x12_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nn_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_8x4_lib4ccc(int kmax, double *alpha, double *A0, int sda0, double *B0, int ldb0, double *A1, int sda1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_8x4_vs_lib4ccc(int kmax, double *alpha, double *A0, int sda0, double *B0, int ldb0, double *A1, int sda1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_8x4_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_8x4_vs_lib4ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_inv_8x4_lib4ccc4(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_8x4_vs_lib4ccc4(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int sde, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4cccc(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_l_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_l_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +void kernel_dgetrf_nn_r_8x4_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_r_8x4_vs_lib4ccc(int kmax, double *A, int sda, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); +// 4x8 +void kernel_dgemm_nn_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x8_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x8_tran_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x8_tran_vs_lib4c4c(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nn_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dsyrk_nt_l_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dsyrk_nt_l_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dger2k_nt_4x4_lib4ccc(int kmax, double *alpha, double *A0, double *B0, int ldb0, double *A1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dger2k_nt_4x4_vs_lib4ccc(int kmax, double *alpha, double *A0, double *B0, int ldb0, double *A1, double *B1, int ldb1, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nn_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nn_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_rl_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_rl_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_vs_lib4ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dtrmm_nt_ru_one_4x4_tran_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd); +void kernel_dtrmm_nt_ru_one_4x4_tran_vs_lib4c4c(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, double *D, int ldd, int m1, int n1); +void kernel_dtrsm_nn_ll_inv_4x4_lib4ccc4(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *inv_diag_E); +void kernel_dtrsm_nn_ll_inv_4x4_vs_lib4ccc4(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, double *inv_diag_E, int km, int kn); +void kernel_dtrsm_nn_ll_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ll_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, double *E, int lde, int m1, int n1); +void kernel_dgetrf_nn_4x4_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag); +void kernel_dgetrf_nn_4x4_vs_lib4ccc(int kmax, double *A, double *B, int ldb, double *C, int ldc, double *D, int ldd, double *diag, int m1, int n1); + +// B panel-major bs=4; A, C, D column-major +// 12x4 +void kernel_dgemm_nt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_12x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_12x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x12 +void kernel_dgemm_nt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x12_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x12_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x8 +void kernel_dgemm_nt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x8_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x8_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x4_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x4_vs_libc4cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// A, C, D panel-major; B, E column-major +// TODO merge with above +// 12x4 +void kernel_dtrsm_nn_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_12x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_12x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +// 8x4 +void kernel_dtrsm_nn_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nn_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_rl_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_8x4_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde); +void kernel_dtrsm_nt_ru_one_8x4_vs_lib4c44c(int kmax, double *A, int sda, double *B, int ldb, double *beta, double *C, int sdc, double *D, int sdd, double *E, int lde, int m1, int n1); +// 4x4 +void kernel_dtrsm_nn_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nn_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nn_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_rl_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_rl_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); +void kernel_dtrsm_nt_ru_inv_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE); +void kernel_dtrsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, double *dE, int m1, int n1); +void kernel_dtrsm_nt_ru_one_4x4_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde); +void kernel_dtrsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, double *A, double *B, int ldb, double *beta, double *C, double *D, double *E, int lde, int m1, int n1); + +// A, B, C, D column-major +// 12x4 +void kernel_dgemm_nn_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_12x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_12x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x12 +void kernel_dgemm_nn_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x12_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x12_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x4 +void kernel_dgemm_nn_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x8 +void kernel_dgemm_nn_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x8_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x8_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 4x4 +void kernel_dgemm_nn_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_4x4_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_4x4_vs_libcccc(int kmax, double *alpha, double *A, int lda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// A column-major +// 12 +void kernel_dgetrf_pivot_12_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_12_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); +// 8 +void kernel_dgetrf_pivot_8_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_8_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); +// 4 +void kernel_dgetrf_pivot_4_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv); +void kernel_dgetrf_pivot_4_vs_lib(int m, double *pA, int lda, double *inv_diag_A, int* ipiv, int n); + +// vector +void kernel_ddot_11_lib(int n, double *x, double *y, double *res); +void kernel_daxpy_11_lib(int n, double *alpha, double *x, double *y); +void kernel_drowsw_lib(int kmax, double *pA, int lda, double *pC, int ldc); + +//#endif // BLAS_API + + + +// larger kernels +// 12 +void kernel_dgemm_nt_12xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dgemm_nt_12xn_pl_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +void kernel_dgemm_nt_mx12_p0_lib44cc(int m, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); +// 8 +void kernel_dgemm_nt_8xn_p0_lib44cc(int n, int k, double *alpha, double *A, int sda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, double *A_p, double *B_p); + + + +// A, B panel-major bs=8; C, D column-major +// 24x8 +void kernel_dgemm_nt_24x8_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_24x8_vs_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 16x8 +void kernel_dgemm_nt_16x8_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_16x8_vs_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x8 +void kernel_dgemm_nt_8x8_lib88cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x8_vs_lib88cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// A, panel-major bs=8; B, C, D column-major +// 24x8 +void kernel_dgemm_nt_24x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_24x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nn_24x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_24x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 16x8 +void kernel_dgemm_nt_16x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_16x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nn_16x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_16x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x8 +void kernel_dgemm_nt_8x8_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x8_vs_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_nn_8x8_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nn_8x8_vs_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + +// B, panel-major bs=8; A, C, D column-major +// 8x24 +void kernel_dgemm_nt_8x24_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x24_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x24_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x24_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x16 +void kernel_dgemm_nt_8x16_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x16_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x16_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x16_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +// 8x8 +void kernel_dgemm_nt_8x8_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_nt_8x8_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); +void kernel_dgemm_tt_8x8_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd); +void kernel_dgemm_tt_8x8_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1); + + +// level 2 BLAS +void kernel_dgemv_n_4_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z); +void kernel_dgemv_n_4_vs_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z, int km); +void kernel_dgemv_t_4_libc(int kmax, double *alpha, double *A, int lda, double *x, double *beta, double *y, double *z); +void kernel_dgemv_t_4_vs_libc(int kmax, double *alpha, double *A, int lda, double *x, double *beta, double *y, double *z, int km); +void kernel_dsymv_l_4_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z); +void kernel_dsymv_l_4_vs_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z, int km); +void kernel_dsymv_u_4_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z); +void kernel_dsymv_u_4_vs_libc(int kmax, double *alpha, double *A, int lda, double *x, double *z, int km); +void kernel_dger_4_libc(int kmax, double *alpha, double *x, double *y, double *C, int ldc, double *D, int ldd); +void kernel_dger_4_vs_libc(int kmax, double *alpha, double *x, double *y, double *C, int ldc, double *D, int ldd, int km); + + + +// aux +void kernel_dvecld_inc1(int kmax, double *x); +void kernel_dveccp_inc1(int kmax, double *x, double *y); + +//void kernel_dgetr_nt_8_p0_lib(int kmax, double *A, int lda, double *C, int ldc, double *Ap, double *Bp); +//void kernel_dgetr_nt_8_lib(int kmax, double *A, int lda, double *C, int ldc); +//void kernel_dgetr_nt_4_lib(int kmax, double *A, int lda, double *C, int ldc); +void kernel_dgetr_tn_8_p0_lib(int kmax, double *A, int lda, double *C, int ldc, double *Ap, double *Bp); +void kernel_dgetr_tn_8_lib(int kmax, double *A, int lda, double *C, int ldc); +void kernel_dgetr_tn_4_lib(int kmax, double *A, int lda, double *C, int ldc); +void kernel_dgetr_tn_4_vs_lib(int kmax, double *A, int lda, double *C, int ldc, int m1); + + +// building blocks for blocked algorithms +// +void blasfeo_hp_dgemm_nt_m2(int m, int n, int k, double alpha, double *pA, int sda, double *pB, int sdb, double beta, double *C, int ldc, double *D, int ldd); +void blasfeo_hp_dgemm_nt_n2(int m, int n, int k, double alpha, double *pA, int sda, double *pB, int sdb, double beta, double *C, int ldc, double *D, int ldd); +// +void kernel_dpack_buffer_fn(int m, int n, double *A, int lda, double *pA, int sda); +void kernel_dpack_buffer_ft(int m, int n, double *A, int lda, double *pA, int sda); +void kernel_dpack_buffer_ln(int m, double *A, int lda, double *pA, int sda); +void kernel_dpack_buffer_lt(int m, double *A, int lda, double *pA, int sda); +void kernel_dpack_buffer_ut(int m, double *A, int lda, double *pA, int sda); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_D_KERNEL_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h new file mode 100644 index 0000000..74c3fb5 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_i_aux_ext_dep.h @@ -0,0 +1,69 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_I_AUX_EXT_DEP_H_ +#define BLASFEO_I_AUX_EXT_DEP_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +// i_aux_extern_depend_lib +void int_zeros(int **pA, int row, int col); +void int_zeros_align(int **pA, int row, int col); +void int_free(int *pA); +void int_free_align(int *pA); +void int_print_mat(int row, int col, int *A, int lda); +int int_print_to_string_mat(char **buf_out, int row, int col, int *A, int lda); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_I_AUX_EXT_DEP_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h new file mode 100644 index 0000000..6248853 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_m_aux.h @@ -0,0 +1,57 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_M_AUX_H_ +#define BLASFEO_M_AUX_H_ + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +void blasfeo_cvt_d2s_vec(int m, struct blasfeo_dvec *vd, int vdi, struct blasfeo_svec *vs, int vsi); +void blasfeo_cvt_s2d_vec(int m, struct blasfeo_svec *vs, int vsi, struct blasfeo_dvec *vd, int vdi); +void blasfeo_cvt_d2s_mat(int m, int n, struct blasfeo_dmat *Md, int mid, int nid, struct blasfeo_smat *Ms, int mis, int nis); +void blasfeo_cvt_s2d_mat(int m, int n, struct blasfeo_smat *Ms, int mis, int nis, struct blasfeo_dmat *Md, int mid, int nid); + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_M_AUX_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_memory.h b/third_party/acados/include/blasfeo/include/blasfeo_memory.h new file mode 100644 index 0000000..da4e7fa --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_memory.h @@ -0,0 +1,62 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2020 by Gianluca Frison. * +* All rights reserved. * +* * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef BLASFEO_MEMORY_H_ +#define BLASFEO_MEMORY_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +int blasfeo_is_init(); +// +void blasfeo_init(); +// +void blasfeo_quit(); +// +void *blasfeo_get_buffer(); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_MEMORY_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_naming.h b/third_party/acados/include/blasfeo/include/blasfeo_naming.h new file mode 100644 index 0000000..c289443 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_naming.h @@ -0,0 +1,77 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +/* + * ----------- Naming conventions + * + * (precision)(data) + * + * 1) d(double) + * s(single) + * + * 2) ge(general) + * tr(triangular) + * vec(vector) + * row(row) + * col(column) + * dia(diagonal) + * + * 3) se(set) + * cp(copy) + * sc(scale) + * ad(add) + * tr(transpose) + * in(insert) + * ex(extract) + * pe(premute) + * sw(swap) + * + * f(factorization) + * + * lqf(LQ factorization) + * qrf (factorization) + * trf (LU factorization using partial pivoting with row interchanges.) + * + * 4) _l(lower) / _u(upper) + * _lib8 (hp implementation, 8 rows kernel) + * _lib4 (hp implementation, 4 rows kernel) + * _lib0 (hp interface with reference implentation) + * _lib (reference implementation) + * _libref (reference implementation with dedicated namespace) + * + * 5) _sp(sparse) + * _exp(exponential format) + */ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h b/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h new file mode 100644 index 0000000..67a0a4d --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_processor_features.h @@ -0,0 +1,88 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Ian McInerney * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_PROCESSOR_FEATURES_H_ +#define BLASFEO_PROCESSOR_FEATURES_H_ + +/** + * Flags to indicate the different processor features + */ +//enum +//{ +// // x86-64 CPU features +// BLASFEO_PROCESSOR_FEATURE_AVX = 0x0001, /// AVX instruction set +// BLASFEO_PROCESSOR_FEATURE_AVX2 = 0x0002, /// AVX2 instruction set +// BLASFEO_PROCESSOR_FEATURE_FMA = 0x0004, /// FMA instruction set +// BLASFEO_PROCESSOR_FEATURE_SSE3 = 0x0008, /// SSE3 instruction set +// +// // ARM CPU features +// BLASFEO_PROCESSOR_FEATURE_VFPv3 = 0x0100, /// VFPv3 instruction set +// BLASFEO_PROCESSOR_FEATURE_NEON = 0x0100, /// NEON instruction set +// BLASFEO_PROCESSOR_FEATURE_VFPv4 = 0x0100, /// VFPv4 instruction set +// BLASFEO_PROCESSOR_FEATURE_NEONv2 = 0x0100, /// NEONv2 instruction set +//} BLASFEO_PROCESSOR_FEATURES; + +/** + * Test the features that this processor provides against what the library was compiled with. + * + * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum) + * @return 0 if current processor doesn't support all features required for this library, 1 otherwise + */ +int blasfeo_processor_cpu_features( int* features ); + +/** + * Test the features that this processor provides against what the library was compiled with. + * + * @param features - Pointer to an integer to store the supported feature set (using the flags in the BLASFEO_PROCESSOR_FEATURES enum) + * @return 0 if current processor doesn't support all features required for this library, 1 otherwise + */ +void blasfeo_processor_library_features( int* features ); + +/** + * Create a string listing the features the current processor supports. + * + * @param features - Flags from the BLASFEO_PROCESSOR_FEATURES enum indicating the features supported + * @param featureString - Character array to store the feature string in + */ +void blasfeo_processor_feature_string( int features, char* featureString ); + +/** + * Get a string listing the processor features that this library version needs to run. + * + * @param featureString - Character array to store the feature string in + */ +void blasfeo_processor_library_string( char* featureString ); + +#endif // BLASFEO_PROCESSOR_FEATURES_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h new file mode 100644 index 0000000..f43f5e8 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux.h @@ -0,0 +1,168 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_H_ +#define BLASFEO_S_AUX_H_ + + + +#include + +#include "blasfeo_s_aux_old.h" +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +/************************************************ +* d_aux_lib.c +************************************************/ + +// returns the memory size (in bytes) needed for a smat +size_t blasfeo_memsize_smat(int m, int n); +size_t blasfeo_memsize_smat_ps(int ps, int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a smat +size_t blasfeo_memsize_diag_smat(int m, int n); +// returns the memory size (in bytes) needed for a svec +size_t blasfeo_memsize_svec(int m); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void blasfeo_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory); +void blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_u_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +//void s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +//void s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +//void s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); + +// ge +void blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +float blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// tr +void blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// dia +void blasfeo_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +void blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// row +void blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// col +void blasfeo_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// vec +void blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); +void blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); +void blasfeo_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi); +float blasfeo_svecex1(struct blasfeo_svec *sx, int xi); +void blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +// z += alpha * x[idx] +void blasfeo_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +void blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +void blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +void blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +void blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_svecnrm_2(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); +void blasfeo_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + + +/* +* Explicitly panel-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_pm_memsize_smat(int ps, int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_pm_create_smat(int ps, int m, int n, struct blasfeo_pm_smat *sA, void *memory); + + + +/* +* Explicitly column-major matrix format +*/ + +// returns the memory size (in bytes) needed for a dmat +size_t blasfeo_cm_memsize_smat(int m, int n); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_cm_create_smat(int m, int n, struct blasfeo_pm_smat *sA, void *memory); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h new file mode 100644 index 0000000..5209d20 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep.h @@ -0,0 +1,141 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_EXT_DEP_H_ +#define BLASFEO_S_AUX_EXT_DEP_H_ + + + +#include + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef EXT_DEP + +/************************************************ +* s_aux_extern_depend_lib.c +************************************************/ + +/* column-major matrices */ + +// dynamically allocate row*col floats of memory and set accordingly a pointer to float; set allocated memory to zero +void s_zeros(float **pA, int row, int col); +// dynamically allocate row*col floats of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero +void s_zeros_align(float **pA, int row, int col); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to float; set allocated memory to zero +void s_zeros_align_bytes(float **pA, int size); +// free the memory allocated by d_zeros +void s_free(float *pA); +// free the memory allocated by d_zeros_align or d_zeros_align_bytes +void s_free_align(float *pA); +// print a column-major matrix +void s_print_mat(int m, int n, float *A, int lda); +// print the transposed of a column-major matrix +void s_print_tran_mat(int row, int col, float *A, int lda); +// print to file a column-major matrix +void s_print_to_file_mat(FILE *file, int row, int col, float *A, int lda); +// print to file a column-major matrix in exponential format +void s_print_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda); +// print to string a column-major matrix +void s_print_to_string_mat(char **buf_out, int row, int col, float *A, int lda); +// print to file the transposed of a column-major matrix +void s_print_tran_to_file_mat(FILE *file, int row, int col, float *A, int lda); +// print to file the transposed of a column-major matrix in exponential format +void s_print_tran_to_file_exp_mat(FILE *file, int row, int col, float *A, int lda); +// print in exponential notation a column-major matrix +void s_print_exp_mat(int m, int n, float *A, int lda); +// print in exponential notation the transposed of a column-major matrix +void s_print_exp_tran_mat(int row, int col, float *A, int lda); + +/* strmat and strvec */ + +// create a strmat for a matrix of size m*n by dynamically allocating memory +void blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA); +// create a strvec for a vector of size m by dynamically allocating memory +void blasfeo_allocate_svec(int m, struct blasfeo_svec *sa); +// free the memory allocated by blasfeo_allocate_dmat +void blasfeo_free_smat(struct blasfeo_smat *sA); +// free the memory allocated by blasfeo_allocate_dvec +void blasfeo_free_svec(struct blasfeo_svec *sa); +// print a strmat +void blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print in exponential notation a strmat +void blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to file a strmat +void blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to file a strmat in exponential format +void blasfeo_print_to_file_exp_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print to string a strmat +void blasfeo_print_to_string_smat(char **buf_out, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print the transpose of a strmat +void blasfeo_print_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +// print a strvec +void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +// print in exponential notation a strvec +void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +// print to file a strvec +void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +// print to string a strvec +void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); +// print the transposed of a strvec +void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); +// print in exponential notation the transposed of a strvec +void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); +// print to file the transposed of a strvec +void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +// print to string the transposed of a strvec +void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + + + +#endif // BLASFEO_S_AUX_EXT_DEP_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h new file mode 100644 index 0000000..6640e20 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ext_dep_ref.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_EXT_DEP_REF_H_ +#define BLASFEO_S_AUX_EXT_DEP_REF_H_ + +#if defined(EXT_DEP) + + + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// expose reference BLASFEO for testing +// see blasfeo_s_aux_exp_dep.h for help + +void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_allocate_smat_ref(int m, int n, struct blasfeo_smat_ref *sA); +void blasfeo_allocate_svec_ref(int m, struct blasfeo_svec_ref *sa); +void blasfeo_free_smat_ref(struct blasfeo_smat_ref *sA); +void blasfeo_free_svec_ref(struct blasfeo_svec_ref *sa); +void blasfeo_print_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_exp_smat_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_file_exp_smat_ref(FILE *file, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_to_string_smat_ref(char **buf_out, int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj); +void blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_string_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void blasfeo_print_to_string_tran_svec(char **buf_out, int m, struct blasfeo_svec *sa, int ai); + + +#ifdef __cplusplus +} +#endif + + + +#endif // EXT_DEP + +#endif // BLASFEO_S_AUX_EXT_DEP_REF_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h new file mode 100644 index 0000000..5c6db37 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_old.h @@ -0,0 +1,64 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +// TODO remove +// +void strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +void sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +void sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd); +void sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd); +void sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x); +void sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +void sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +void sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x); +void sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +void sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd); +void srowin_lib(int kmax, float alpha, float *x, float *pD); +void srowex_lib(int kmax, float alpha, float *pD, float *x); +void srowad_lib(int kmax, float alpha, float *x, float *pD); +void srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD); +void srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD); +void srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD); +void srowsw_lib(int kmax, float *pA, float *pC); +void scolin_lib(int kmax, float *x, int offset, float *pD, int sdd); +void scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +void scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd); +void scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd); +void scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +void svecin_libsp(int kmax, int *idx, float *x, float *y); +void svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y); diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h new file mode 100644 index 0000000..998e1d8 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_ref.h @@ -0,0 +1,147 @@ + +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_REF_H_ +#define BLASFEO_S_AUX_REF_H_ + + + +#include + +#include "blasfeo_s_aux_old.h" +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + +/************************************************ +* d_aux_lib.c +************************************************/ + +// returns the memory size (in bytes) needed for a smat +size_t blasfeo_ref_memsize_smat(int m, int n); +size_t blasfeo_ref_memsize_smat_ps(int ps, int m, int n); +// returns the memory size (in bytes) needed for the diagonal of a smat +size_t blasfeo_ref_memsize_diag_smat(int m, int n); +// returns the memory size (in bytes) needed for a svec +size_t blasfeo_ref_memsize_svec(int m); +// create a strmat for a matrix of size m*n by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void blasfeo_ref_create_smat_ps(int ps, int m, int n, struct blasfeo_smat *sA, void *memory); +// create a strvec for a vector of size m by using memory passed by a pointer (pointer is not updated) +void blasfeo_ref_create_svec(int m, struct blasfeo_svec *sA, void *memory); +void blasfeo_ref_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj); +void blasfeo_ref_pack_l_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sB, int bi, int bj); +void blasfeo_ref_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void blasfeo_ref_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_ref_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void blasfeo_ref_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +void ref_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +void ref_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +void ref_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); + +// ge +void blasfeo_ref_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +float blasfeo_ref_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// tr +void blasfeo_ref_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// dia +void blasfeo_ref_sdiare(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// row +void blasfeo_ref_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_ref_srowpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// col +void blasfeo_ref_scolex(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolsc(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj); +void blasfeo_ref_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void blasfeo_ref_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +void blasfeo_ref_scolpei(int kmax, int *ipiv, struct blasfeo_smat *sA); +// vec +void blasfeo_ref_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); +void blasfeo_ref_sveccpsc(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +void blasfeo_ref_svecin1(float a, struct blasfeo_svec *sx, int xi); +float blasfeo_ref_svecex1(struct blasfeo_svec *sx, int xi); +void blasfeo_ref_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +// z += alpha * x[idx] +void blasfeo_ref_svecexad_sp(int m, double alpha, int *idx, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +void blasfeo_ref_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +void blasfeo_ref_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +void blasfeo_ref_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); +void blasfeo_ref_svecpei(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_REF_H_ + diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h new file mode 100644 index 0000000..08d9a14 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_aux_test.h @@ -0,0 +1,177 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_AUX_TEST_H_ +#define BLASFEO_S_AUX_TEST_H_ + +#include + +#include "blasfeo_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +/************************************************ +* d_aux_lib.c +************************************************/ + +int test_blasfeo_memsize_smat(int m, int n); +int test_blasfeo_memsize_diag_smat(int m, int n); +int test_blasfeo_memsize_svec(int m); + +void test_blasfeo_create_smat(int m, int n, struct blasfeo_smat *sA, void *memory); +void test_blasfeo_create_svec(int m, struct blasfeo_svec *sA, void *memory); + +void test_blasfeo_pack_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_pack_svec(int m, float *x, int xi, struct blasfeo_svec *sa, int ai); +void test_blasfeo_pack_tran_smat(int m, int n, float *A, int lda, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_unpack_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); +void test_blasfeo_unpack_svec(int m, struct blasfeo_svec *sa, int ai, float *x, int xi); +void test_blasfeo_unpack_tran_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj, float *A, int lda); + +void test_s_cast_mat2strmat(float *A, struct blasfeo_smat *sA); +void test_s_cast_diag_mat2strmat(float *dA, struct blasfeo_smat *sA); +void test_s_cast_vec2vecmat(float *a, struct blasfeo_svec *sa); +// copy and scale +void test_blasfeo_sgecpsc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void test_blasfeo_sgecp(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +void test_blasfeo_sgesc(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); + +// void test_blasfeo_sgein1(float a, struct blasfeo_smat *sA, int ai, int aj); +// float test_blasfeo_sgeex1(struct blasfeo_smat *sA, int ai, int aj); +// void test_blasfeo_svecin1(float a, struct blasfeo_svec *sx, int xi); +// float test_blasfeo_svecex1(struct blasfeo_svec *sx, int xi); + +// // A <= alpha +// void test_blasfeo_sgese(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj); +// // a <= alpha +// void test_blasfeo_svecse(int m, float alpha, struct blasfeo_svec *sx, int xi); + + +// void test_blasfeo_sveccp(int m, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); +// void test_blasfeo_svecsc(int m, float alpha, struct blasfeo_svec *sa, int ai); + +// void test_strcp_l_lib(int m, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +// void test_blasfeo_strcp_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_sgead_lib(int m, int n, float alpha, int offsetA, float *A, int sda, int offsetB, float *B, int sdb); +// void test_blasfeo_sgead(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_svecad(int m, float alpha, struct blasfeo_svec *sa, int ai, struct blasfeo_svec *sc, int ci); + +// void test_sgetr_lib(int m, int n, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_sgetr(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_strtr_l_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_strtr_l(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_strtr_u_lib(int m, float alpha, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_strtr_u(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); + +// void test_sdiareg_lib(int kmax, float reg, int offset, float *pD, int sdd); +// void test_blasfeo_sdiaex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +// void test_blasfeo_sdiain(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_sdiain_sqrt_lib(int kmax, float *x, int offset, float *pD, int sdd); +// void test_sdiaex_lib(int kmax, float alpha, int offset, float *pD, int sdd, float *x); +// void test_sdiaad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +// void test_sdiain_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +// void test_blasfeo_sdiain_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_sdiaex_libsp(int kmax, int *idx, float alpha, float *pD, int sdd, float *x); +// void test_blasfeo_sdiaex_sp(int kmax, float alpha, int *idx, struct blasfeo_smat *sD, int di, int dj, struct blasfeo_svec *sx, int xi); +// void test_blasfeo_sdiaad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_sdiaad_libsp(int kmax, int *idx, float alpha, float *x, float *pD, int sdd); +// void test_blasfeo_sdiaad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_sdiaadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD, int sdd); +// void test_blasfeo_sdiaadin_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_srowin_lib(int kmax, float alpha, float *x, float *pD); +// void test_blasfeo_srowin(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_srowex_lib(int kmax, float alpha, float *pD, float *x); +// void test_blasfeo_srowex(int kmax, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi); +// void test_srowad_lib(int kmax, float alpha, float *x, float *pD); +// void test_blasfeo_srowad(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_srowin_libsp(int kmax, float alpha, int *idx, float *x, float *pD); +// void test_srowad_libsp(int kmax, int *idx, float alpha, float *x, float *pD); +// void test_blasfeo_srowad_sp(int kmax, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_smat *sD, int di, int dj); +// void test_srowadin_libsp(int kmax, int *idx, float alpha, float *x, float *y, float *pD); +// void test_srowsw_lib(int kmax, float *pA, float *pC); +// void test_blasfeo_srowsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_srowpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +// void test_scolin_lib(int kmax, float *x, int offset, float *pD, int sdd); +// void test_blasfeo_scolin(int kmax, struct blasfeo_svec *sx, int xi, struct blasfeo_smat *sA, int ai, int aj); +// void test_scolad_lib(int kmax, float alpha, float *x, int offset, float *pD, int sdd); +// void test_scolin_libsp(int kmax, int *idx, float *x, float *pD, int sdd); +// void test_scolad_libsp(int kmax, float alpha, int *idx, float *x, float *pD, int sdd); +// void test_scolsw_lib(int kmax, int offsetA, float *pA, int sda, int offsetC, float *pC, int sdc); +// void test_blasfeo_scolsw(int kmax, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sC, int ci, int cj); +// void test_blasfeo_scolpe(int kmax, int *ipiv, struct blasfeo_smat *sA); +// void test_svecin_libsp(int kmax, int *idx, float *x, float *y); +// void test_svecad_libsp(int kmax, int *idx, float alpha, float *x, float *y); +// void test_blasfeo_svecad_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_svecin_sp(int m, float alpha, struct blasfeo_svec *sx, int xi, int *idx, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_svecex_sp(int m, float alpha, int *idx, struct blasfeo_svec *sx, int x, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_sveccl(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi); +// void test_blasfeo_sveccl_mask(int m, struct blasfeo_svec *sxm, int xim, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sxp, int xip, struct blasfeo_svec *sz, int zi, struct blasfeo_svec *sm, int mi); +// void test_blasfeo_svecze(int m, struct blasfeo_svec *sm, int mi, struct blasfeo_svec *sv, int vi, struct blasfeo_svec *se, int ei); +// void test_blasfeo_svecnrm_inf(int m, struct blasfeo_svec *sx, int xi, float *ptr_norm); +// void test_blasfeo_svecpe(int kmax, int *ipiv, struct blasfeo_svec *sx, int xi); + + +// ext_dep + +void test_blasfeo_allocate_smat(int m, int n, struct blasfeo_smat *sA); +void test_blasfeo_allocate_svec(int m, struct blasfeo_svec *sa); + +void test_blasfeo_free_smat(struct blasfeo_smat *sA); +void test_blasfeo_free_svec(struct blasfeo_svec *sa); + +void test_blasfeo_print_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_svec(int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_tran_svec(int m, struct blasfeo_svec *sa, int ai); + +void test_blasfeo_print_to_file_smat(FILE *file, int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_to_file_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_to_file_tran_svec(FILE *file, int m, struct blasfeo_svec *sa, int ai); + +void test_blasfeo_print_exp_smat(int m, int n, struct blasfeo_smat *sA, int ai, int aj); +void test_blasfeo_print_exp_svec(int m, struct blasfeo_svec *sa, int ai); +void test_blasfeo_print_exp_tran_svec(int m, struct blasfeo_svec *sa, int ai); + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_AUX_TEST_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h new file mode 100644 index 0000000..200f1f5 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blas.h @@ -0,0 +1,46 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLAS_H_ +#define BLASFEO_S_BLAS_H_ + + + +#include "blasfeo_s_blasfeo_api.h" +#include "blasfeo_s_blas_api.h" + + + +#endif // BLASFEO_S_BLAS_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h new file mode 100644 index 0000000..cf2b3e0 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blas_api.h @@ -0,0 +1,182 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef BLASFEO_S_BLAS_API_H_ +#define BLASFEO_S_BLAS_API_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef BLAS_API +#ifdef CBLAS_API +#ifndef BLASFEO_CBLAS_ENUM +#define BLASFEO_CBLAS_ENUM +#ifdef FORTRAN_BLAS_API +#ifndef CBLAS_H +enum CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102}; +enum CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; +enum CBLAS_UPLO {CblasUpper=121, CblasLower=122}; +enum CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; +enum CBLAS_SIDE {CblasLeft=141, CblasRight=142}; +#endif // CBLAS_H +#else // FORTRAN_BLAS_API +enum BLASFEO_CBLAS_ORDER {CblasRowMajor=101, CblasColMajor=102}; +enum BLASFEO_CBLAS_TRANSPOSE {CblasNoTrans=111, CblasTrans=112, CblasConjTrans=113}; +enum BLASFEO_CBLAS_UPLO {CblasUpper=121, CblasLower=122}; +enum BLASFEO_CBLAS_DIAG {CblasNonUnit=131, CblasUnit=132}; +enum BLASFEO_CBLAS_SIDE {CblasLeft=141, CblasRight=142}; +#endif // FORTRAN_BLAS_API +#endif // BLASFEO_CBLAS_ENUM +#endif // CBLAS_API +#endif // BLAS_API + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +#ifdef BLAS_API + + + +#ifdef FORTRAN_BLAS_API + + + +// BLAS 1 +// +void saxpy_(int *n, float *alpha, float *x, int *incx, float *y, int *incy); +// +float sdot_(int *n, float *x, int *incx, float *y, int *incy); + +// BLAS 3 +// +void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +// +void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); + + + +// LAPACK +// +void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); + + + +#ifdef CBLAS_API + + + +// CBLAS 1 +// +void cblas_saxpy(const int N, const float alpha, const float *X, const int incX, float *Y, const int incY); + +// CBLAS 3 +// +void cblas_sgemm(const enum CBLAS_ORDER Order, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const float alpha, const float *A, const int lda, const float *B, const int ldb, const float beta, float *C, const int ldc); +// +void cblas_strsm(const enum CBLAS_ORDER Order, const enum CBLAS_SIDE Side, const enum CBLAS_UPLO Uplo, const enum CBLAS_TRANSPOSE TransA, const enum CBLAS_DIAG Diag, const int M, const int N, const float alpha, const float *A, const int lda, float *B, const int ldb); + + + +#endif // CBLAS_API + + + +#else // BLASFEO_API + + + +// BLAS 1 +// +void blasfeo_blas_saxpy(int *n, float *alpha, float *x, int *incx, float *y, int *incy); +// +float blasfeo_blas_sdot(int *n, float *x, int *incx, float *y, int *incy); + +// BLAS 3 +// +void blasfeo_blas_sgemm(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +// +void blasfeo_blas_strsm(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); + + + +// LAPACK +// +void blasfeo_lapack_spotrf(char *uplo, int *m, float *A, int *lda, int *info); + + + +#ifdef CBLAS_API + + + +// CBLAS 1 +// +void blasfeo_cblas_saxpy(const int N, const float alpha, const float *X, const int incX, float *Y, const int incY); + +// CBLAS 3 +// +void blasfeo_cblas_sgemm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_TRANSPOSE TransB, const int M, const int N, const int K, const float alpha, const float *A, const int lda, const float *B, const int ldb, const float beta, float *C, const int ldc); +// +void blasfeo_cblas_strsm(const enum BLASFEO_CBLAS_ORDER Order, const enum BLASFEO_CBLAS_SIDE Side, const enum BLASFEO_CBLAS_UPLO Uplo, const enum BLASFEO_CBLAS_TRANSPOSE TransA, const enum BLASFEO_CBLAS_DIAG Diag, const int M, const int N, const float alpha, const float *A, const int lda, float *B, const int ldb); + + + +#endif // CBLAS_API + + + +#endif // BLASFEO_API + + + +#endif // BLAS_API + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLAS_API_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h new file mode 100644 index 0000000..8b98104 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h @@ -0,0 +1,284 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_API_H_ +#define BLASFEO_S_BLASFEO_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +// level 1 BLAS +// + +// z = y + alpha*x +void blasfeo_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y +void blasfeo_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z += x .* y +void blasfeo_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +float blasfeo_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// return x^T * y +float blasfeo_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi); +// construct givens plane rotation +void blasfeo_srotg(float a, float b, float *c, float *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_strmv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_strmv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_strmv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular, unit diagonal +void blasfeo_strmv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ssymv_l(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +void blasfeo_ssymv_l_mn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x, where A is symmetric and only the upper triangular patr of A is accessed +void blasfeo_ssymv_u(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// D = C + alpha * x * y^T +void blasfeo_sger(int m, int n, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^T ; B upper triangular +void blasfeo_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T; C, D lower triangular +void blasfeo_ssyr2k_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A; C, D lower triangular +void blasfeo_ssyr2k_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T; C, D upper triangular +void blasfeo_ssyr2k_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A; C, D upper triangular +void blasfeo_ssyr2k_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +void blasfeo_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_sgeqrf_worksize(int m, int n); // in bytes +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_sorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_sgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work); + + + + +// +// BLAS API helper functions +// + +#if ( defined(BLAS_API) & defined(MF_PANELMAJ) ) +// BLAS 3 +void blasfeo_cm_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, float beta, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_llnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_llnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lunn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lunu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_lutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rlnn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rlnu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rltn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rltu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_runn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_runu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rutn(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_strsm_rutu(int m, int n, float alpha, struct blasfeo_cm_smat *sA, int ai, int aj, struct blasfeo_cm_smat *sB, int bi, int bj, struct blasfeo_cm_smat *sD, int di, int dj); +// LAPACK +void blasfeo_cm_spotrf_l(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +void blasfeo_cm_spotrf_u(int m, struct blasfeo_cm_smat *sC, int ci, int cj, struct blasfeo_cm_smat *sD, int di, int dj); +#endif + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_API_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h new file mode 100644 index 0000000..f429a79 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api_ref.h @@ -0,0 +1,135 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_API_REF_H_ +#define BLASFEO_S_BLASFEO_API_REF_H_ + +#include "blasfeo_common.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + +// expose reference BLASFEO for testing + +// --- level 1 + +void blasfeo_saxpy_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_saxpby_ref(int kmax, float alpha, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_svecmul_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_svecmulacc_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +float blasfeo_svecmuldot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +float blasfeo_sdot_ref(int m, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sy, int yi); +void blasfeo_srotg_ref(float a, float b, float *c, float *s); +void blasfeo_scolrot_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj0, int aj1, float c, float s); +void blasfeo_srowrot_ref(int m, struct blasfeo_smat_ref *sA, int ai0, int ai1, int aj, float c, float s); + + +// --- level 2 + +// dense +void blasfeo_sgemv_n_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_sgemv_t_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltn_mn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_lnu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_ltu_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strsv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_unn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_utn_ref(int m, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_lnn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_ltn_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_lnu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_strmv_ltu_ref(int m, int n, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, struct blasfeo_svec_ref *sz, int zi); +void blasfeo_sgemv_nt_ref(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx_n, int xi_n, struct blasfeo_svec_ref *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec_ref *sy_n, int yi_n, struct blasfeo_svec_ref *sy_t, int yi_t, struct blasfeo_svec_ref *sz_n, int zi_n, struct blasfeo_svec_ref *sz_t, int zi_t); +void blasfeo_ssymv_l_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); + +// diagonal +void blasfeo_sgemv_d_ref(int m, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_svec_ref *sx, int xi, float beta, struct blasfeo_svec_ref *sy, int yi, struct blasfeo_svec_ref *sz, int zi); + + +// --- level 3 + +// dense +void blasfeo_sgemm_nn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_nt_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_tn_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_tt_ref(int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +void blasfeo_ssyrk_ln_mn_ref( int m, int n, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_ln_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_lt_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_un_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_ut_ref( int m, int k, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +void blasfeo_strmm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strmm_rlnn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rltn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rltu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_rutn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_llnu_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_strsm_lunn_ref( int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sD, int di, int dj); + +// diagonal +void dgemm_diag_left_lib_ref(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_sgemm_dn_ref(int m, int n, float alpha, struct blasfeo_svec_ref *sA, int ai, struct blasfeo_smat_ref *sB, int bi, int bj, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgemm_nd_ref(int m, int n, float alpha, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_svec_ref *sB, int bi, float beta, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); + +// --- lapack + +void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_spotrf_l_ref(int m, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_spotrf_l_mn_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_dpotrf_ln_ref(int m, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_ssyrk_dpotrf_ln_mn_ref(int m, int n, int k, struct blasfeo_smat_ref *sA, int ai, int aj, struct blasfeo_smat_ref *sB, int bi, int bj, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_nopivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj); +void blasfeo_sgetrf_rowpivot_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, int *ipiv); +void blasfeo_sgeqrf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_pd_ref(int m, int n, struct blasfeo_smat_ref *sC, int ci, int cj, struct blasfeo_smat_ref *sD, int di, int dj, void *work); +void blasfeo_sgelqf_pd_la_ref(int m, int n1, struct blasfeo_smat_ref *sL, int li, int lj, struct blasfeo_smat_ref *sA, int ai, int aj, void *work); +void blasfeo_sgelqf_pd_lla_ref(int m, int n1, struct blasfeo_smat_ref *sL0, int l0i, int l0j, struct blasfeo_smat_ref *sL1, int l1i, int l1j, struct blasfeo_smat_ref *sA, int ai, int aj, void *work); + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_API_REF_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h new file mode 100644 index 0000000..17cb179 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h @@ -0,0 +1,252 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_BLASFEO_REF_API_H_ +#define BLASFEO_S_BLASFEO_REF_API_H_ + + + +#include "blasfeo_common.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +// level 1 BLAS +// + +// z = y + alpha*x +void blasfeo_ref_saxpy(int kmax, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = beta*y + alpha*x +void blasfeo_ref_saxpby(int kmax, float alpha, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y +void blasfeo_ref_svecmul(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z += x .* y +void blasfeo_ref_svecmulacc(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z = x .* y, return sum(z) = x^T * y +float blasfeo_ref_svecmuldot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// return x^T * y +float blasfeo_ref_sdot(int m, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi); +// construct givens plane rotation +void blasfeo_ref_srotg(float a, float b, float *c, float *s); +// apply plane rotation [a b] [c -s; s; c] to the aj0 and aj1 columns of A at row index ai +void blasfeo_ref_scolrot(int m, struct blasfeo_smat *sA, int ai, int aj0, int aj1, float c, float s); +// apply plane rotation [c s; -s c] [a; b] to the ai0 and ai1 rows of A at column index aj +void blasfeo_ref_srowrot(int m, struct blasfeo_smat *sA, int ai0, int ai1, int aj, float c, float s); + + + +// +// level 2 BLAS +// + +// dense + +// z <= beta * y + alpha * A * x +void blasfeo_ref_sgemv_n(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A' * x +void blasfeo_ref_sgemv_t(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(n) +void blasfeo_ref_strsv_lnn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(n) +void blasfeo_ref_strsv_ltn_mn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, not_unit +void blasfeo_ref_strsv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A ) * x, A (m)x(m) lower, not_transposed, unit +void blasfeo_ref_strsv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, not_unit +void blasfeo_ref_strsv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) lower, transposed, unit +void blasfeo_ref_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, not_transposed, not_unit +void blasfeo_ref_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit +void blasfeo_ref_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular +void blasfeo_ref_strmv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A * x ; A lower triangular, unit diagonal +void blasfeo_ref_strmv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular +void blasfeo_ref_strmv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A lower triangular, unit diagonal +void blasfeo_ref_strmv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x ; A upper triangular +void blasfeo_ref_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z <= A' * x ; A upper triangular +void blasfeo_ref_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi); +// z_n <= beta_n * y_n + alpha_n * A * x_n +// z_t <= beta_t * y_t + alpha_t * A' * x_t +void blasfeo_ref_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t); +// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed +void blasfeo_ref_ssymv_l(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +void blasfeo_ref_ssymv_l_mn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// z <= beta * y + alpha * A * x, where A is symmetric and only the upper triangular patr of A is accessed +void blasfeo_ref_ssymv_u(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); +// D = C + alpha * x * y^T +void blasfeo_ref_sger(int m, int n, float alpha, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sy, int yi, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// z <= beta * y + alpha * A * x, A diagonal +void blasfeo_ref_sgemv_d(int m, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi); + + + +// +// level 3 BLAS +// + +// dense + +// D <= beta * C + alpha * A * B +void blasfeo_ref_sgemm_nn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T +void blasfeo_ref_sgemm_nt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_sgemm_tn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B +void blasfeo_ref_sgemm_tt(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D lower triangular +void blasfeo_ref_ssyrk_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_ssyrk_ln_mn(int m, int n, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D lower triangular +void blasfeo_ref_ssyrk_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T ; C, D upper triangular +void blasfeo_ref_ssyrk_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B ; C, D upper triangular +void blasfeo_ref_ssyrk_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^T ; B upper triangular +void blasfeo_ref_strmm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A ; A lower triangular +void blasfeo_ref_strmm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_strsm_llnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_llnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular employint explicit inverse of diagonal +void blasfeo_ref_strsm_lltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_lltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_lunn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-1} * B , with A upper triangular withunit diagonal +void blasfeo_ref_strsm_lunu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_lutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A^{-T} * B , with A upper triangular withunit diagonal +void blasfeo_ref_strsm_lutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rlnn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_rlnu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rltn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A lower triangular with unit diagonal +void blasfeo_ref_strsm_rltu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_runn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-1} , with A upper triangular with unit diagonal +void blasfeo_ref_strsm_runu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular employing explicit inverse of diagonal +void blasfeo_ref_strsm_rutn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * B * A^{-T} , with A upper triangular with unit diagonal +void blasfeo_ref_strsm_rutu(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T ; C, D lower triangular +void blasfeo_ref_ssyr2k_ln(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A ; C, D lower triangular +void blasfeo_ref_ssyr2k_lt(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A * B^T + alpha * B * A^T ; C, D upper triangular +void blasfeo_ref_ssyr2k_un(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= beta * C + alpha * A^T * B + alpha * B^T * A ; C, D upper triangular +void blasfeo_ref_ssyr2k_ut(int m, int k, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + +// diagonal + +// D <= alpha * A * B + beta * C, with A diagonal (stored as strvec) +void sgemm_diag_left_ib(int m, int n, float alpha, float *dA, float *pB, int sdb, float beta, float *pC, int sdc, float *pD, int sdd); +void blasfeo_ref_sgemm_dn(int m, int n, float alpha, struct blasfeo_svec *sA, int ai, struct blasfeo_smat *sB, int bi, int bj, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= alpha * A * B + beta * C, with B diagonal (stored as strvec) +void blasfeo_ref_sgemm_nd(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sB, int bi, float beta, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); + + + +// +// LAPACK +// + +// D <= chol( C ) ; C, D lower triangular +void blasfeo_ref_spotrf_l(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_spotrf_l_mn(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C ) ; C, D upper triangular +void blasfeo_ref_spotrf_u(int m, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= chol( C + A * B' ) ; C, D lower triangular +void blasfeo_ref_ssyrk_spotrf_ln(int m, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +void blasfeo_ref_ssyrk_spotrf_ln_mn(int m, int n, int k, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_smat *sB, int bi, int bj, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; no pivoting +void blasfeo_ref_sgetrf_np(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj); +// D <= lu( C ) ; row pivoting +void blasfeo_ref_sgetrf_rp(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, int *ipiv); +// D <= qr( C ) +void blasfeo_ref_sgeqrf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_ref_sgeqrf_worksize(int m, int n); // in bytes +// D <= Q factor, where C is the output of the LQ factorization +int blasfeo_ref_sorglq_worksize(int m, int n, int k); // in bytes +void blasfeo_ref_sorglq(int m, int n, int k, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// D <= lq( C ) +void blasfeo_ref_sgelqf(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +int blasfeo_ref_sgelqf_worksize(int m, int n); // in bytes +// D <= lq( C ), positive diagonal elements +void blasfeo_ref_sgelqf_pd(int m, int n, struct blasfeo_smat *sC, int ci, int cj, struct blasfeo_smat *sD, int di, int dj, void *work); +// [L, A] <= lq( [L, A] ), positive diagonal elements, array of matrices, with +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_sgelqf_pd_la(int m, int n1, struct blasfeo_smat *sL, int li, int lj, struct blasfeo_smat *sA, int ai, int aj, void *work); +// [L, L, A] <= lq( [L, L, A] ), positive diagonal elements, array of matrices, with: +// L lower triangular, of size (m)x(m) +// A full, of size (m)x(n1) +void blasfeo_ref_sgelqf_pd_lla(int m, int n1, struct blasfeo_smat *sL0, int l0i, int l0j, struct blasfeo_smat *sL1, int l1i, int l1j, struct blasfeo_smat *sA, int ai, int aj, void *work); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_BLASFEO_REF_API_H_ + diff --git a/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h b/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h new file mode 100644 index 0000000..99d2b28 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h @@ -0,0 +1,692 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_S_KERNEL_H_ +#define BLASFEO_S_KERNEL_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// utils +void blasfeo_align_2MB(void *ptr, void **ptr_align); +void blasfeo_align_4096_byte(void *ptr, void **ptr_align); +void blasfeo_align_64_byte(void *ptr, void **ptr_align); + + + +// +// lib8 +// + +// 24x4 +void kernel_sgemm_nt_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nt_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nt_24x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nn_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nn_24x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_24x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_24x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_ssyrk_nt_l_20x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_20x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_spotrf_nt_l_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_spotrf_nt_l_20x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_20x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_24x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_24x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_20x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_20x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_ssyrk_spotrf_nt_l_24x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_24x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_strmm_nn_rl_24x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd); +void kernel_strmm_nn_rl_24x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn); + +// 16x8 +void kernel_sgemm_nt_16x8_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *spil); + +// 16x4 +void kernel_sgemm_nt_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nt_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nt_16x4_gen_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_nn_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_sgemm_nn_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_16x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_16x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_ssyrk_nt_l_12x4_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_ssyrk_nt_l_12x4_vs_lib8(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); +void kernel_spotrf_nt_l_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_spotrf_nt_l_12x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_12x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_16x4_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_16x4_vs_lib8(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_12x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_12x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_ssyrk_spotrf_nt_l_16x4_vs_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_16x4_lib8(int kp, float *Ap, int sdap, float *Bp, int km_, float *Am, int sdam, float *Bm, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_strmm_nn_rl_16x4_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd); +void kernel_strmm_nn_rl_16x4_vs_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *D, int sdd, int km, int kn); +void kernel_strmm_nn_rl_16x4_gen_lib8(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); + +// 8x8 +void kernel_sgemm_nt_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_8x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_8x8_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); +void kernel_sgemm_nn_8x8_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nn_8x8_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_ssyrk_nt_l_8x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_ssyrk_nt_l_8x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_spotrf_nt_l_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_8x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x8_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x8_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); + +// 8x4 +void kernel_sgemm_nt_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_8x4_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_sgemm_nn_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); +void kernel_sgemm_nn_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nn_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +//void kernel_ssyrk_nt_l_8x4_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_ssyrk_nt_l_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_spotrf_nt_l_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_strsm_nt_rl_inv_8x4_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x4_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_sgemm_strsm_nt_rl_inv_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x4_vs_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_8x4_lib8(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); +void kernel_strmm_nn_rl_8x4_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D); +void kernel_strmm_nn_rl_8x4_vs_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D, int km, int kn); +void kernel_strmm_nn_rl_8x4_gen_lib8(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strmm_nt_ru_8x4_lib8(int k, float *alpha, float *A, float *B, float *D); +void kernel_strmm_nt_ru_8x4_vs_lib8(int k, float *alpha, float *A, float *B, float *D, int km, int kn); + +// 4x8 +void kernel_sgemm_nt_4x8_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_nt_4x8_vs_lib8(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); +void kernel_sgemm_nt_4x8_gen_lib8(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strsm_nt_rl_inv_4x8_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_4x8_vs_lib8(int k, float *A, float *B, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); + +// 8 +void kernel_sgemv_n_8_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z); +void kernel_sgemv_n_8_vs_lib8(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_n_8_gen_lib8(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1); +void kernel_sgemv_t_8_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_8_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_t_4_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_4_vs_lib8(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_strsv_ln_inv_8_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_ln_inv_8_vs_lib8(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_strsv_lt_inv_8_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_8_vs_lib8(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_sgemv_nt_4_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_vs_lib8(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km); +void kernel_ssymv_l_4l_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z); +void kernel_ssymv_l_4r_lib8(int kmax, float *alpha, float *A, int sda, float *x, float *z); +void kernel_ssymv_l_4l_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km); +void kernel_ssymv_l_4r_gen_lib8(int kmax, float *alpha, int offA, float *A, int sda, float *x, float *z, int km); + +// -------- aux + +// ---- copy + +// lib4 +// +void kernel_sgecpsc_4_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_4_0_lib4(int kmax, float *A, float *B); + +void kernel_sgecpsc_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_1_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_4_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_3_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_3_0_lib4(int kmax, float *A, float *B); +void kernel_sgecpsc_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_3_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_sgecpsc_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_3_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_2_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_2_0_lib4(int kmax, float *A, float *B); +void kernel_sgecpsc_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgecp_2_3_lib4(int kmax, float *A0, int sda, float *B); + +void kernel_sgecpsc_1_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgecp_1_0_lib4(int kmax, float *A, float *B); + +// lib8 +// +void kernel_sgecp_8_0_lib8(int m, float *A, float *B); +void kernel_sgecp_8_0_gen_lib8(int m, float *A, float *B, int m1); +void kernel_sgecp_8_0_gen_u_lib8(int m, float *A, float *B, int m1); + +void kernel_sgesc_8_0_lib8(int m, float *alpha, float *A); +void kernel_sgesc_8_0_gen_lib8(int m, float *alpha, float *A, int m1); +void kernel_sgesc_8_0_gen_u_lib8(int m, float *alpha, float *A, int m1); + +void kernel_sgecpsc_8_0_lib8(int m, float *alpha, float *A, float *B); +void kernel_sgecpsc_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1); +void kernel_sgecpsc_8_0_gen_u_lib8(int m, float *alpha, float *A, float *B, int m1); + +void kernel_sgecp_8_1_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_1_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_2_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_2_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_3_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_3_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_4_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_4_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_5_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_5_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_6_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_6_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +void kernel_sgecp_8_7_lib8(int m, float *A, int sda, float *B); +void kernel_sgecp_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgecpsc_8_7_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgecpsc_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + +// transpose +// +void kernel_sgetr_8_0_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_0_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_1_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_1_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_2_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_2_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_3_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_3_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_4_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_4_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_5_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_5_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_6_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_6_gen_lib8(int m, float *A, int sda, float *B, int m1); +void kernel_sgetr_8_7_lib8(int m, float *A, int sda, float *B); +void kernel_sgetr_8_7_gen_lib8(int m, float *A, int sda, float *B, int m1); + +// add +// +void kernel_sgead_8_0_lib8(int m, float *alpha, float *A, float *B); +void kernel_sgead_8_0_gen_lib8(int m, float *alpha, float *A, float *B, int m1); +void kernel_sgead_8_1_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_1_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_2_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_2_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_3_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_3_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_4_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_4_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_5_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_5_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_6_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_6_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); +void kernel_sgead_8_7_lib8(int m, float *alpha, float *A, int sda, float *B); +void kernel_sgead_8_7_gen_lib8(int m, float *alpha, float *A, int sda, float *B, int m1); + + +// +// lib4 +// + + + +// level 2 BLAS +// 4 +void kernel_sgemv_n_4_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z); +void kernel_sgemv_n_4_vs_lib4(int k, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k1); +void kernel_sgemv_n_4_gen_lib4(int kmax, float *alpha, float *A, float *x, float *beta, float *y, float *z, int k0, int k1); +void kernel_sgemv_t_4_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z); +void kernel_sgemv_t_4_vs_lib4(int k, float *alpha, int offsetA, float *A, int sda, float *x, float *beta, float *y, float *z, int k1); +void kernel_strsv_ln_inv_4_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_ln_inv_4_vs_lib4(int k, float *A, float *inv_diag_A, float *x, float *y, float *z, int km, int kn); +void kernel_strsv_lt_inv_4_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_3_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_2_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strsv_lt_inv_1_lib4(int k, float *A, int sda, float *inv_diag_A, float *x, float *y, float *z); +void kernel_strmv_un_4_lib4(int k, float *A, float *x, float *z); +void kernel_strmv_ut_4_lib4(int k, float *A, int sda, float *x, float *z); +void kernel_strmv_ut_4_vs_lib4(int k, float *A, int sda, float *x, float *z, int km); +void kernel_sgemv_nt_6_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t); +void kernel_sgemv_nt_4_vs_lib4(int kmax, float *alpha_n, float *alpha_t, float *A, int sda, float *x_n, float *x_t, float *beta_t, float *y_t, float *z_n, float *z_t, int km); +void kernel_ssymv_l_4_lib4(int kmax, float *alpha, float *A, int sda, float *x_n, float *z_n); +void kernel_ssymv_l_4_gen_lib4(int kmax, float *alpha, int offA, float *A, int sda, float *x_n, float *z_n, int km); + + + +// level 3 BLAS +// 12x4 +void kernel_sgemm_nt_16x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_16x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_strsm_nt_rl_inv_16x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_16x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 12x4 +void kernel_sgemm_nt_12x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_12x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_strsm_nt_rl_inv_12x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_12x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 8x8 +void kernel_sgemm_nt_8x8_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_8x8_vs_lib4(int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_sgemm_nn_8x8_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nn_8x8_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +// 8x4 +void kernel_sgemm_nt_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nt_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_sgemm_nn_8x4_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_sgemm_nn_8x4_vs_lib4(int k, float *alpha, float *A, int sda, int offsetB, float *B, int sdb, float *beta, float *C, int sdc, float *D, int sdd, int m1, int n1); // +void kernel_ssyrk_nt_l_8x4_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); // +void kernel_ssyrk_nt_l_8x4_vs_lib4(int k, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, int km, int kn); // +void kernel_strsm_nt_rl_inv_8x4_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_8x4_vs_lib4(int k, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd, float *E, float *inv_diag_E, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); // +void kernel_sgemm_nt_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); // +void kernel_sgemm_nt_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int k0, int k1); +void kernel_sgemm_nn_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D); // +void kernel_sgemm_nn_4x4_vs_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, float *C, float *D, int km, int kn); // +void kernel_sgemm_nn_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); // +void kernel_ssyrk_nt_l_4x4_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D); // +void kernel_ssyrk_nt_l_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *beta, float *C, float *D, int km, int kn); // +void kernel_ssyrk_nt_l_4x4_gen_lib4(int k, float *alpha, float *A, float *B, float *beta, int offsetC, float *C, int sdc, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strmm_nt_ru_4x4_lib4(int k, float *alpha, float *A, float *B, float *D); // +void kernel_strmm_nt_ru_4x4_vs_lib4(int k, float *alpha, float *A, float *B, float *D, int km, int kn); // +void kernel_strmm_nn_rl_4x4_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, float *D); +void kernel_strmm_nn_rl_4x4_gen_lib4(int k, float *alpha, float *A, int offsetB, float *B, int sdb, int offsetD, float *D, int sdd, int m0, int m1, int n0, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nt_rl_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E); +void kernel_strsm_nt_rl_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nt_ru_inv_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nt_ru_one_4x4_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E); +void kernel_strsm_nt_ru_one_4x4_vs_lib4(int k, float *A, float *B, float *beta, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nn_ru_inv_4x4_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *beta, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_strsm_nn_ll_one_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E); +void kernel_strsm_nn_ll_one_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *E, int km, int kn); +void kernel_strsm_nn_lu_inv_4x4_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E); +void kernel_strsm_nn_lu_inv_4x4_vs_lib4(int kmax, float *A, float *B, int sdb, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +// diag +void kernel_sgemm_diag_right_4_a0_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *D, int sdd); +void kernel_sgemm_diag_right_4_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_3_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_2_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_right_1_lib4(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int sdc, float *D, int sdd); +void kernel_sgemm_diag_left_4_a0_lib4(int kmax, float *alpha, float *A, float *B, float *D); +void kernel_sgemm_diag_left_4_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_3_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_2_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); +void kernel_sgemm_diag_left_1_lib4(int kmax, float *alpha, float *A, float *B, float *beta, float *C, float *D); + + + +// LAPACK +// 16x4 +void kernel_spotrf_nt_l_16x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_16x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 12x4 +void kernel_spotrf_nt_l_12x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_12x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 8x4 +void kernel_spotrf_nt_l_8x4_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D); +void kernel_spotrf_nt_l_8x4_vs_lib4(int k, float *A, int sda, float *B, float *C, int sdc, float *D, int sdd, float *inv_diag_D, int m1, int n1); +// 4x4 +void kernel_spotrf_nt_l_4x4_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D); +void kernel_spotrf_nt_l_4x4_vs_lib4(int k, float *A, float *B, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_sgetrf_nn_4x4_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D); +void kernel_sgetrf_nn_4x4_vs_lib4(int k, float *A, float *B, int sdb, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_sgetrf_pivot_4_lib4(int m, float *pA, int sda, float *inv_diag_A, int* ipiv); +void kernel_sgetrf_pivot_4_vs_lib4(int m, int n, float *pA, int sda, float *inv_diag_A, int* ipiv); + + + +// merged routines +// 4x4 +void kernel_sgemm_strsm_nt_rl_inv_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E); +void kernel_sgemm_strsm_nt_rl_inv_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *E, float *inv_diag_E, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_4x4_vs_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D, int km, int kn); +void kernel_ssyrk_spotrf_nt_l_4x4_lib4(int kp, float *Ap, float *Bp, int km_, float *Am, float *Bm, float *C, float *D, float *inv_diag_D); + + + +// auxiliary routines +void kernel_strcp_l_4_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_4_1_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_4_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_4_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_3_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_3_2_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_3_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_2_0_lib4(int kmax, float *A, float *B); +void kernel_strcp_l_2_3_lib4(int kmax, float *A0, int sda, float *B); +void kernel_strcp_l_1_0_lib4(int kmax, float *A, float *B); +void kernel_sgead_4_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_4_1_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_4_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_4_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_3_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_3_2_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_3_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_2_0_lib4(int kmax, float *alpha, float *A, float *B); +void kernel_sgead_2_3_lib4(int kmax, float *alpha, float *A0, int sda, float *B); +void kernel_sgead_1_0_lib4(int kmax, float *alpha, float *A, float *B); +// TODO +void kernel_sgeset_4_lib4(int kmax, float alpha, float *A); +void kernel_strset_4_lib4(int kmax, float alpha, float *A); +void kernel_sgetr_4_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_3_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_2_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); +void kernel_sgetr_1_lib4(int tri, int kmax, int kna, float alpha, float *A, float *C, int sdc); + + + +// pack +// 24 lib 8 +void kernel_spack_nn_24_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_24_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 16 lib 8 +void kernel_spack_nn_16_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_16_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 8 lib 8 +void kernel_spack_nn_8_lib8(int kmax, float *A, int lda, float *B); +void kernel_spack_nn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tn_8_lib8(int kmax, float *A, int lda, float *B); +void kernel_spack_tn_8_vs_lib8(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tt_8_lib8(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_tt_8_vs_lib8(int kmax, float *A, int lda, float *B, int sdb, int m1); +// 8 lib 4 +void kernel_spack_nn_8_lib4(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_nn_8_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1); +//void kernel_spack_tt_8_lib4(int kmax, float *A, int lda, float *B, int sdb); +// 4 +void kernel_spack_nn_4_lib4(int kmax, float *A, int lda, float *B); +void kernel_spack_nn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tn_4_lib4(int kmax, float *A, int lda, float *B); +void kernel_spack_tn_4_vs_lib4(int kmax, float *A, int lda, float *B, int m1); +void kernel_spack_tt_4_lib4(int kmax, float *A, int lda, float *B, int sdb); +void kernel_spack_tt_4_vs_lib4(int kmax, float *A, int lda, float *B, int sdb, int m1); +// unpack +// 8 +void kernel_sunpack_nn_8_lib4(int kmax, float *A, int sda, float *B, int ldb); +void kernel_sunpack_nn_8_vs_lib4(int kmax, float *A, int sda, float *B, int ldb, int m1); +//void kernel_sunpack_tt_8_lib4(int kmax, float *A, int sda, float *B, int ldb); +// 4 +void kernel_sunpack_nn_4_lib4(int kmax, float *A, float *B, int ldb); +void kernel_sunpack_nn_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1); +void kernel_sunpack_nt_4_lib4(int kmax, float *A, float *B, int ldb); +void kernel_sunpack_nt_4_vs_lib4(int kmax, float *A, float *B, int ldb, int m1); +void kernel_sunpack_tt_4_lib4(int kmax, float *A, int sda, float *B, int ldb); + +// panel copy +// 4 +void kernel_spacp_nt_4_lib4(int kmax, float *A, int offsetB, float *B, int sdb); +void kernel_spacp_tn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B); +void kernel_spacp_nn_4_lib4(int kmax, int offsetA, float *A, int sda, float *B); +void kernel_spacp_nn_4_vs_lib4(int kmax, int offsetA, float *A, int sda, float *B, int m1); + + + +/************************************************ +* BLAS API kernels +************************************************/ + +//#if defined(BLAS_API) + +// A, B panel-major bs=8; C, D column-major +// 24x4 +void kernel_sgemm_nt_24x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_24x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 16x4 +void kernel_sgemm_nt_16x4_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_16x4_vs_lib88cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x8 +void kernel_sgemm_nt_8x8_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_spotrf_nt_l_8x8_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_spotrf_nt_l_8x8_vs_lib88cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1); +void kernel_strsm_nt_rl_inv_8x8_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_8x8_vs_lib88ccc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +// 8x4 +void kernel_sgemm_nt_8x4_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib88cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A, B panel-major bs=4; C, D column-major +// 8x8 +void kernel_sgemm_nt_8x8_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +// 8x4 +void kernel_sgemm_nt_8x4_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib44cc(int kmax, float *alpha, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_spotrf_nt_l_8x4_lib44cc(int kmax, float *A, int sda, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_strsm_nt_rl_inv_8x4_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_8x4_vs_lib44ccc(int kmax, float *A, int sda, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_lib44cc(int kmax, float *alpha, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib44ccc(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E); +void kernel_strsm_nt_rl_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E); +void kernel_strsm_nt_ru_one_4x4_vs_lib44cc4(int kmax, float *A, float *B, float *beta, float *C, int ldc, float *D, int ldd, float *E, int m1, int n1); +void kernel_spotrf_nt_l_4x4_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD); +void kernel_spotrf_nt_l_4x4_vs_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1); + +// B panel-major bs=8; A, C, D column-major +// 4x24 +void kernel_sgemm_nt_4x24_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x24_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x24_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x24_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x16 +void kernel_sgemm_nt_4x16_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x16_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x16_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x16_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x8 +void kernel_sgemm_nt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x8 +void kernel_sgemm_nt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// B panel-major bs=4; A, C, D column-major +// 8x8 +void kernel_sgemm_nt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_8x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_8x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x8 +void kernel_sgemm_nt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x8_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x8_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x4 +void kernel_sgemm_nt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x4_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x4_vs_libc4cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A panel-major bs=8; B, C, D column-major +// 24x4 +void kernel_sgemm_nn_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_24x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_24x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 16x4 +void kernel_sgemm_nn_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_16x4_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_16x4_vs_lib8ccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x8 +void kernel_sgemm_nn_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x8_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 8x4 +void kernel_sgemm_nn_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x4_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib8ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// A panel-major bs=4; B, C, D column-major +// 8x8 +void kernel_sgemm_nn_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x8_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +// 8x4 +void kernel_sgemm_nn_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_8x4_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_8x4_vs_lib4ccc(int kmax, float *alpha, float *A, int sda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +// 4x4 +void kernel_sgemm_nn_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_4x4_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_lib4ccc(int kmax, float *alpha, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_strsm_nn_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nn_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nn_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nt_rl_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nn_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nn_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde); +void kernel_strsm_nt_ru_one_4x4_vs_lib4cccc(int kmax, float *A, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, float *E, int lde, int m1, int n1); + +// A, C, D panel-major; B, E column-major +// TODO merge with above +// 4x4 +void kernel_strsm_nn_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nn_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nn_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nn_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nn_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nn_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nn_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_rl_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nt_rl_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_rl_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nt_rl_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); +void kernel_strsm_nt_ru_inv_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE); +void kernel_strsm_nt_ru_inv_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, float *dE, int m1, int n1); +void kernel_strsm_nt_ru_one_4x4_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde); +void kernel_strsm_nt_ru_one_4x4_vs_lib4c44c(int kmax, float *A, float *B, int ldb, float *beta, float *C, float *D, float *E, int lde, int m1, int n1); + +// A, B, C, D column-major +void kernel_sgemm_nn_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nn_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_nt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_nt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); +void kernel_sgemm_tt_4x4_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd); +void kernel_sgemm_tt_4x4_vs_libcccc(int kmax, float *alpha, float *A, int lda, float *B, int ldb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1); + +// vector +void kernel_sdot_11_lib(int n, float *x, float *y, float *res); +void kernel_saxpy_11_lib(int n, float *alpha, float *x, float *y); + + +//#endif // BLAS_API + + + +// larger kernels +// 24 +void kernel_sgemm_nt_24xn_p0_lib88cc(int n, int k, float *alpha, float *A, int sda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, float *A_p, float *B_p); + + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_S_KERNEL_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h b/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h new file mode 100644 index 0000000..9bd248b --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_stdlib.h @@ -0,0 +1,62 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_STDLIB_H_ +#define BLASFEO_STDLIB_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +#include + +// +void blasfeo_malloc(void **ptr, size_t size); +// +void blasfeo_malloc_align(void **ptr, size_t size); +// +void blasfeo_free(void *ptr); +// +void blasfeo_free_align(void *ptr); + + + +#ifdef __cplusplus +} +#endif + +#endif // BLASFEO_STDLIB_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_target.h b/third_party/acados/include/blasfeo/include/blasfeo_target.h new file mode 100644 index 0000000..51f617a --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_target.h @@ -0,0 +1,73 @@ +#ifndef TARGET_X64_INTEL_HASWELL +#define TARGET_X64_INTEL_HASWELL +#endif + +#ifndef TARGET_NEED_FEATURE_AVX2 +#define TARGET_NEED_FEATURE_AVX2 1 +#endif + +#ifndef TARGET_NEED_FEATURE_FMA +#define TARGET_NEED_FEATURE_FMA 1 +#endif + +#ifndef TARGET_NEED_FEATURE_SSE3 +/* #undef TARGET_NEED_FEATURE_SSE3 */ +#endif + +#ifndef TARGET_NEED_FEATURE_AVX +/* #undef TARGET_NEED_FEATURE_AVX */ +#endif + +#ifndef TARGET_NEED_FEATURE_VFPv3 +/* #undef TARGET_NEED_FEATURE_VFPv3 */ +#endif + +#ifndef TARGET_NEED_FEATURE_NEON +/* #undef TARGET_NEED_FEATURE_NEON */ +#endif + +#ifndef TARGET_NEED_FEATURE_VFPv4 +/* #undef TARGET_NEED_FEATURE_VFPv4 */ +#endif + +#ifndef TARGET_NEED_FEATURE_NEONv2 +/* #undef TARGET_NEED_FEATURE_NEONv2 */ +#endif + +#ifndef LA_HIGH_PERFORMANCE +#define LA_HIGH_PERFORMANCE +#endif + +#ifndef MF_PANELMAJ +#define MF_PANELMAJ +#endif + +#ifndef EXT_DEP +#define ON 1 +#define OFF 0 +#if ON==ON +#define EXT_DEP +#endif +#undef ON +#undef OFF +#endif + +#ifndef BLAS_API +#define ON 1 +#define OFF 0 +#if OFF==ON +#define BLAS_API +#endif +#undef ON +#undef OFF +#endif + +#ifndef FORTRAN_BLAS_API +#define ON 1 +#define OFF 0 +#if OFF==ON +#define FORTRAN_BLAS_API +#endif +#undef ON +#undef OFF +#endif diff --git a/third_party/acados/include/blasfeo/include/blasfeo_timing.h b/third_party/acados/include/blasfeo/include/blasfeo_timing.h new file mode 100644 index 0000000..5671b88 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_timing.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_TIMING_H_ +#define BLASFEO_TIMING_H_ + +//#include + +#if (defined _WIN32 || defined _WIN64) && !(defined __MINGW32__ || defined __MINGW64__) + + /* Use Windows QueryPerformanceCounter for timing. */ + #include + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + LARGE_INTEGER tic; + LARGE_INTEGER toc; + LARGE_INTEGER freq; + } blasfeo_timer; + +#elif(defined __APPLE__) + + #include + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + uint64_t tic; + uint64_t toc; + mach_timebase_info_data_t tinfo; + } blasfeo_timer; + +#elif(defined __DSPACE__) + + #include + + typedef struct blasfeo_timer_ { + double time; + } blasfeo_timer; + +#elif(defined __XILINX_NONE_ELF__ || defined __XILINX_ULTRASCALE_NONE_ELF_JAILHOUSE__) + + #include "xtime_l.h" + + typedef struct blasfeo_timer_ { + uint64_t tic; + uint64_t toc; + } blasfeo_timer; + +#else + + /* Use POSIX clock_gettime() for timing on non-Windows machines. */ + #include + + #if __STDC_VERSION__ >= 199901L // C99 Mode + + #include + #include + + typedef struct blasfeo_timer_ { + struct timeval tic; + struct timeval toc; + } blasfeo_timer; + + #else // ANSI C Mode + + /** A structure for keeping internal timer data. */ + typedef struct blasfeo_timer_ { + struct timespec tic; + struct timespec toc; + } blasfeo_timer; + + #endif // __STDC_VERSION__ >= 199901L + +#endif // (defined _WIN32 || defined _WIN64) + +/** A function for measurement of the current time. */ +void blasfeo_tic(blasfeo_timer* t); + +/** A function which returns the elapsed time. */ +double blasfeo_toc(blasfeo_timer* t); + +#endif // BLASFEO_TIMING_H_ diff --git a/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h b/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h new file mode 100644 index 0000000..1598551 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/blasfeo_v_aux_ext_dep.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef BLASFEO_V_AUX_EXT_DEP_H_ +#define BLASFEO_V_AUX_EXT_DEP_H_ + + + +#include "blasfeo_target.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +/************************************************ +* d_aux_extern_depend_lib.c +************************************************/ + +#ifdef EXT_DEP + +void v_zeros(void **ptrA, int size); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to void; set allocated memory to zero +void v_zeros_align(void **ptrA, int size); +// free the memory allocated by v_zeros +void v_free(void *ptrA); +// free the memory allocated by v_zeros_aligned +void v_free_align(void *ptrA); +// dynamically allocate size bytes of memory and set accordingly a pointer to char; set allocated memory to zero +void c_zeros(char **ptrA, int size); +// dynamically allocate size bytes of memory aligned to 64-byte boundaries and set accordingly a pointer to char; set allocated memory to zero +void c_zeros_align(char **ptrA, int size); +// free the memory allocated by c_zeros +void c_free(char *ptrA); +// free the memory allocated by c_zeros_aligned +void c_free_align(char *ptrA); + +#endif // EXT_DEP + + + +#ifdef __cplusplus +} +#endif + + + +#endif // BLASFEO_V_AUX_EXT_DEP_H_ diff --git a/third_party/acados/include/blasfeo/include/d_blas.h b/third_party/acados/include/blasfeo/include/d_blas.h new file mode 100644 index 0000000..ffd2578 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/d_blas.h @@ -0,0 +1,78 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +double ddot_(int *m, double *x, int *incx, double *y, int *incy); +void dcopy_(int *m, double *x, int *incx, double *y, int *incy); +void daxpy_(int *m, double *alpha, double *x, int *incx, double *y, int *incy); +void dscal_(int *m, double *alpha, double *x, int *incx); +void drot_(int *m, double *x, int *incx, double *y, int *incy, double *c, double *s); +void drotg_(double *a, double *b, double *c, double *s); + +// level 2 +void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +void dsymv_(char *uplo, int *m, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); +void dtrmv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx); +void dtrsv_(char *uplo, char *trans, char *diag, int *n, double *A, int *lda, double *x, int *incx); +void dger_(int *m, int *n, double *alpha, double *x, int *incx, double *y, int *incy, double *A, int *lda); + +// level 3 +void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); +void dsyrk_(char *uplo, char *trans, int *n, int *k, double *alpha, double *A, int *lda, double *beta, double *C, int *ldc); +void dtrmm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +void dtrsm_(char *side, char *uplo, char *trans, char *diag, int *m, int *n, double *alpha, double *A, int *lda, double *B, int *ldb); +void dsyr2k_(char *uplo, char *trans, int *n, int *k, double *alpha, double *A, int *lda, double *B, int *ldb, double *beta, double *C, int *ldc); + +// lapack +void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); +void dgetrf_(int *m, int *n, double *A, int *lda, int *ipiv, int *info); +void dgeqrf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info); +void dgeqr2_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *info); +void dgelqf_(int *m, int *n, double *A, int *lda, double *tau, double *work, int *lwork, int *info); +void dorglq_(int *m, int *n, int *k, double *A, int *lda, double *tau, double *work, int *lwork, int *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/third_party/acados/include/blasfeo/include/d_blas_64.h b/third_party/acados/include/blasfeo/include/d_blas_64.h new file mode 100644 index 0000000..4f40d00 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/d_blas_64.h @@ -0,0 +1,73 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +double ddot_(long long *m, double *x, long long *incx, double *y, long long *incy); +void dcopy_(long long *m, double *x, long long *incx, double *y, long long *incy); +void daxpy_(long long *m, double *alpha, double *x, long long *incx, double *y, long long *incy); +void dscal_(long long *m, double *alpha, double *x, long long *incx); + +// level 2 +void dgemv_(char *ta, long long *m, long long *n, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy); +void dsymv_(char *uplo, long long *m, double *alpha, double *A, long long *lda, double *x, long long *incx, double *beta, double *y, long long *incy); +void dtrmv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx); +void dtrsv_(char *uplo, char *trans, char *diag, long long *n, double *A, long long *lda, double *x, long long *incx); +void dger_(long long *m, long long *n, double *alpha, double *x, long long *incx, double *y, long long *incy, double *A, long long *lda); + +// level 3 +void dgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, double *alpha, double *A, long long *lda, double *B, long long *ldb, double *beta, double *C, long long *ldc); +void dsyrk_(char *uplo, char *trans, long long *n, long long *k, double *alpha, double *A, long long *lda, double *beta, double *C, long long *ldc); +void dtrmm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb); +void dtrsm_(char *side, char *uplo, char *trans, char *diag, long long *m, long long *n, double *alpha, double *A, long long *lda, double *B, long long *ldb); + +// lapack +void dpotrf_(char *uplo, long long *m, double *A, long long *lda, long long *info); +void dgetrf_(long long *m, long long *n, double *A, long long *lda, long long *ipiv, long long *info); +void dgeqrf_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *lwork, long long *info); +void dgeqr2_(long long *m, long long *n, double *A, long long *lda, double *tau, double *work, long long *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/third_party/acados/include/blasfeo/include/s_blas.h b/third_party/acados/include/blasfeo/include/s_blas.h new file mode 100644 index 0000000..3b299ce --- /dev/null +++ b/third_party/acados/include/blasfeo/include/s_blas.h @@ -0,0 +1,78 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +float sdot_(int *m, float *x, int *incx, float *y, int *incy); +void scopy_(int *m, float *x, int *incx, float *y, int *incy); +void saxpy_(int *m, float *alpha, float *x, int *incx, float *y, int *incy); +void sscal_(int *m, float *alpha, float *x, int *incx); +void srot_(int *m, float *x, int *incx, float *y, int *incy, float *c, float *s); +void srotg_(float *a, float *b, float *c, float *s); + +// level 2 +void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); +void ssymv_(char *uplo, int *m, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); +void strmv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx); +void strsv_(char *uplo, char *trans, char *diag, int *n, float *A, int *lda, float *x, int *incx); +void sger_(int *m, int *n, float *alpha, float *x, int *incx, float *y, int *incy, float *A, int *lda); + +// level 3 +void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); +void ssyrk_(char *uplo, char *trans, int *n, int *k, float *alpha, float *A, int *lda, float *beta, float *C, int *ldc); +void strmm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); +void strsm_(char *side, char *uplo, char *transa, char *diag, int *m, int *n, float *alpha, float *A, int *lda, float *B, int *ldb); +void ssyr2k_(char *uplo, char *trans, int *n, int *k, float *alpha, float *A, int *lda, float *B, int *ldb, float *beta, float *C, int *ldc); + +// lapack +void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); +void sgetrf_(int *m, int *n, float *A, int *lda, int *ipiv, int *info); +void sgeqrf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info); +void sgeqr2_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *info); +void sgelqf_(int *m, int *n, float *A, int *lda, float *tau, float *work, int *lwork, int *info); +void sorglq_(int *m, int *n, int *k, float *A, int *lda, float *tau, float *work, int *lwork, int *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/third_party/acados/include/blasfeo/include/s_blas_64.h b/third_party/acados/include/blasfeo/include/s_blas_64.h new file mode 100644 index 0000000..b9efab6 --- /dev/null +++ b/third_party/acados/include/blasfeo/include/s_blas_64.h @@ -0,0 +1,73 @@ +/************************************************************************************************** +* * +* This file is part of BLASFEO. * +* * +* BLASFEO -- BLAS For Embedded Optimization. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + + +// headers to reference BLAS and LAPACK routines employed in BLASFEO WR + +// level 1 +float sdot_(long long *m, float *x, long long *incx, float *y, long long *incy); +void scopy_(long long *m, float *x, long long *incx, float *y, long long *incy); +void saxpy_(long long *m, float *alpha, float *x, long long *incx, float *y, long long *incy); +void sscal_(long long *m, float *alpha, float *x, long long *incx); + +// level 2 +void sgemv_(char *ta, long long *m, long long *n, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy); +void ssymv_(char *uplo, long long *m, float *alpha, float *A, long long *lda, float *x, long long *incx, float *beta, float *y, long long *incy); +void strmv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx); +void strsv_(char *uplo, char *trans, char *diag, long long *n, float *A, long long *lda, float *x, long long *incx); +void sger_(long long *m, long long *n, float *alpha, float *x, long long *incx, float *y, long long *incy, float *A, long long *lda); + +// level 3 +void sgemm_(char *ta, char *tb, long long *m, long long *n, long long *k, float *alpha, float *A, long long *lda, float *B, long long *ldb, float *beta, float *C, long long *ldc); +void ssyrk_(char *uplo, char *trans, long long *n, long long *k, float *alpha, float *A, long long *lda, float *beta, float *C, long long *ldc); +void strmm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb); +void strsm_(char *side, char *uplo, char *transa, char *diag, long long *m, long long *n, float *alpha, float *A, long long *lda, float *B, long long *ldb); + +// lapack +void spotrf_(char *uplo, long long *m, float *A, long long *lda, long long *info); +void sgetrf_(long long *m, long long *n, float *A, long long *lda, long long *ipiv, long long *info); +void sgeqrf_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *lwork, long long *info); +void sgeqr2_(long long *m, long long *n, float *A, long long *lda, float *tau, float *work, long long *info); + + + +#ifdef __cplusplus +} +#endif diff --git a/third_party/acados/include/hpipm/include/hpipm_aux_mem.h b/third_party/acados/include/hpipm/include/hpipm_aux_mem.h new file mode 100644 index 0000000..7bd3d7e --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_aux_mem.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_AUX_MEM_H_ +#define HPIPM_AUX_MEM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +void hpipm_zero_memset(hpipm_size_t memsize, void *mem); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_AUX_MEM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_aux_string.h b/third_party/acados/include/hpipm/include/hpipm_aux_string.h new file mode 100644 index 0000000..804cba5 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_aux_string.h @@ -0,0 +1,50 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_AUX_STRING_H_ +#define HPIPM_AUX_STRING_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +#define MAX_STR_LEN 5 +int hpipm_strcmp(char *str1, char *str2); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_AUX_STRING_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_common.h b/third_party/acados/include/hpipm/include/hpipm_common.h new file mode 100644 index 0000000..0cc96a7 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_common.h @@ -0,0 +1,76 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_COMMON_H_ +#define HPIPM_COMMON_H_ + +#include + +#ifdef __cplusplus +extern "C" { +#endif + +typedef size_t hpipm_size_t; + +enum hpipm_mode + { + SPEED_ABS, // focus on speed, absolute IPM formulation + SPEED, // focus on speed, relative IPM formulation + BALANCE, // balanced mode, relative IPM formulation + ROBUST, // focus on robustness, relative IPM formulation + }; + +enum hpipm_status + { + SUCCESS, // found solution satisfying accuracy tolerance + MAX_ITER, // maximum iteration number reached + MIN_STEP, // minimum step length reached + NAN_SOL, // NaN in solution detected + INCONS_EQ, // unconsistent equality constraints + }; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_COMMON_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h new file mode 100644 index 0000000..0e4c41f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_cast_qcqp.h @@ -0,0 +1,71 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_CAST_QCQP_H_ +#define HPIPM_D_CAST_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp.h" +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_cast_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim); +// +void d_cast_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_CAST_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond.h b/third_party/acados/include/hpipm/include/hpipm_d_cond.h new file mode 100644 index 0000000..5900a2a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_cond.h @@ -0,0 +1,135 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_H_ +#define HPIPM_D_COND_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp.h" +#include "hpipm_d_dense_qp_sol.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qp_dim.h" +#include "hpipm_d_ocp_qp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_cond_qp_arg + { + int cond_last_stage; // condense last stage + int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2 + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct d_cond_qp_ws + { + struct blasfeo_dmat *Gamma; + struct blasfeo_dmat *GammaQ; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lx; + struct blasfeo_dmat *AL; + struct blasfeo_dvec *Gammab; + struct blasfeo_dvec *l; + struct blasfeo_dvec *tmp_nbgM; + struct blasfeo_dvec *tmp_nuxM; + int bs; // block size + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_cond_qp_arg_memsize(); +// +void d_cond_qp_arg_create(struct d_cond_qp_arg *cond_arg, void *mem); +// +void d_cond_qp_arg_set_default(struct d_cond_qp_arg *cond_arg); +// condensing algorithm: 0 N2-nx3, 1 N3-nx2 +void d_cond_qp_arg_set_cond_alg(int cond_alg, struct d_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void d_cond_qp_arg_set_ric_alg(int ric_alg, struct d_cond_qp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void d_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_prim_sol(int value, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_cond_qp_arg *cond_arg); + +// +void d_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, struct d_dense_qp_dim *dense_dim); +// +hpipm_size_t d_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg); +// +void d_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws, void *mem); +// +void d_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// TODO remove +void d_cond_qp_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + +// +void d_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h b/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h new file mode 100644 index 0000000..73afba3 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_cond_aux.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_AUX_H_ +#define HPIPM_D_COND_AUX_H_ + + + +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_cond_BAbt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_BAt(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_b(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_RSQrq(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_RSQ(struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_rq(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_DCt(struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, int *idxs_rev2, struct blasfeo_dvec *Z2, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_cond_d(struct d_ocp_qp *ocp_qp, struct blasfeo_dvec *d2, struct blasfeo_dvec *d_mask2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_expand_primal_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_so, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + +// +void d_update_cond_BAbt(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *BAbt2, struct blasfeo_dvec *b, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_update_cond_RSQrq_N2nx3(int *idxc, struct d_ocp_qp *ocp_qp, struct blasfeo_dmat *RSQrq2, struct blasfeo_dvec *rq, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); +// +void d_update_cond_DCtd(int *idxc, struct d_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_dmat *DCt2, struct blasfeo_dvec *d2, int *idxs2, struct blasfeo_dvec *Z2, struct blasfeo_dvec *z, struct d_cond_qp_arg *cond_arg, struct d_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_AUX_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h new file mode 100644 index 0000000..266567b --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_cond_qcqp.h @@ -0,0 +1,129 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_COND_QCQP_H_ +#define HPIPM_D_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp.h" +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_cond_qcqp_arg + { + struct d_cond_qp_arg *qp_arg; + int cond_last_stage; // condense last stage +// int cond_variant; // TODO + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution equality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct d_cond_qcqp_ws + { + struct d_cond_qp_ws *qp_ws; + struct blasfeo_dmat *hess_array; // TODO remove + struct blasfeo_dmat *zero_hess; // TODO remove + struct blasfeo_dvec *grad_array; // TODO remove + struct blasfeo_dvec *zero_grad; // TODO remove + struct blasfeo_dvec *tmp_nvc; + struct blasfeo_dvec *tmp_nuxM; + struct blasfeo_dmat *GammaQ; + struct blasfeo_dmat *tmp_DCt; + struct blasfeo_dmat *tmp_nuM_nxM; +// struct blasfeo_dvec *d_qp; +// struct blasfeo_dvec *d_mask_qp; + hpipm_size_t memsize; + }; + + +// +hpipm_size_t d_cond_qcqp_arg_memsize(); +// +void d_cond_qcqp_arg_create(struct d_cond_qcqp_arg *cond_arg, void *mem); +// +void d_cond_qcqp_arg_set_default(struct d_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void d_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_cond_qcqp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void d_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct d_cond_qcqp_arg *cond_arg); + +// +void d_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, struct d_dense_qcqp_dim *dense_dim); +// +hpipm_size_t d_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg); +// +void d_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws, void *mem); +// +void d_cond_qcqp_qc(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_qc_lhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dmat *Hq2, int *Hq_nzero2, struct blasfeo_dmat *Ct2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_qc_rhs(struct d_ocp_qcqp *ocp_qp, struct blasfeo_dvec *d2, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp *dense_qp, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); +// +void d_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_dense_qcqp_sol *dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_cond_qcqp_arg *cond_arg, struct d_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_COND_QCQP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h new file mode 100644 index 0000000..f39d9a9 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm.h @@ -0,0 +1,101 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_CORE_QP_IPM_ +#define HPIPM_D_CORE_QP_IPM_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_core_qp_ipm_workspace + { + double *v; // primal variables + double *pi; // equality constraints multipliers + double *lam; // inequality constraints multipliers + double *t; // inequality constraints slacks + double *t_inv; // inverse of t + double *v_bkp; // backup of primal variables + double *pi_bkp; // backup of equality constraints multipliers + double *lam_bkp; // backup of inequality constraints multipliers + double *t_bkp; // backup of inequality constraints slacks + double *dv; // step in v + double *dpi; // step in pi + double *dlam; // step in lam + double *dt; // step in t + double *res_g; // q-residuals + double *res_b; // b-residuals + double *res_d; // d-residuals + double *res_m; // m-residuals + double *res_m_bkp; // m-residuals + double *Gamma; // Hessian update + double *gamma; // gradient update + double alpha; // step length + double alpha_prim; // step length + double alpha_dual; // step length + double sigma; // centering XXX + double mu; // duality measuere + double mu_aff; // affine duality measuere + double nc_inv; // 1.0/nc, where nc is the total number of inequality constraints + double nc_mask_inv; // 1.0/nc_mask + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double t_min_inv; // inverse of min value in t vector + double tau_min; // min value of barrier parameter + int nv; // number of primal variables + int ne; // number of equality constraints + int nc; // (twice the) number of (two-sided) inequality constraints + int nc_mask; // total number of ineq constr after masking + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam also in solution, or only in Gamma computation + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_memsize_core_qp_ipm(int nv, int ne, int nc); +// +void d_create_core_qp_ipm(int nv, int ne, int nc, struct d_core_qp_ipm_workspace *workspace, void *mem); +// +void d_core_qp_ipm(struct d_core_qp_ipm_workspace *workspace); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_CORE_QP_IPM_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h new file mode 100644 index 0000000..30cc824 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_core_qp_ipm_aux.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_AUX_ +#define HPIPM_S_CORE_QP_IPM_AUX_ + +#ifdef __cplusplus +extern "C" { +#endif + +// +void d_compute_Gamma_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_gamma_qp(double *res_d, double *res_m, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_lam_t_qp(double *res_d, double *res_m, double *dlam, double *dt, struct d_core_qp_ipm_workspace *rws); +// +void d_compute_alpha_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_update_var_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_mu_aff_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_backup_res_m(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_centering_correction_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_centering_qp(struct d_core_qp_ipm_workspace *rws); +// +void d_compute_tau_min_qp(struct d_core_qp_ipm_workspace *rws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_AUX_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h new file mode 100644 index 0000000..3da5716 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp.h @@ -0,0 +1,199 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_H_ +#define HPIPM_D_DENSE_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dmat *Hv; // hessian of cost & vector work space + struct blasfeo_dmat *A; // equality constraint matrix + struct blasfeo_dmat *Ct; // inequality constraints matrix + struct blasfeo_dmat *Hq; // hessians of quadratic constraints + struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *b; // equality constraint vector + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_dense_qcqp_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp *qp, void *memory); + +// +void d_dense_qcqp_set(char *field, void *value, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_H(double *H, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_g(double *g, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_A(double *A, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_b(double *b, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxb(int *idxb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lb(double *lb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lb_mask(double *lb, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ub(double *ub, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ub_mask(double *ub, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_C(double *C, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lg(double *lg, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_lg_mask(double *lg, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ug(double *ug, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ug_mask(double *ug, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Hq(double *Hq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_gq(double *gq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_uq(double *uq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_uq_mask(double *uq, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxs(int *idxs, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_idxs_rev(int *idxs_rev, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Zl(double *Zl, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_Zu(double *Zu, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_zl(double *zl, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_zu(double *zu, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ls(double *ls, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_ls_mask(double *ls, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_us(double *us, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_set_us_mask(double *us, struct d_dense_qcqp *qp); + +// getters (COLMAJ) + +void d_dense_qcqp_get_H(struct d_dense_qcqp *qp, double *H); +// +void d_dense_qcqp_get_g(struct d_dense_qcqp *qp, double *g); +// +void d_dense_qcqp_get_A(struct d_dense_qcqp *qp, double *A); +// +void d_dense_qcqp_get_b(struct d_dense_qcqp *qp, double *b); +// +void d_dense_qcqp_get_idxb(struct d_dense_qcqp *qp, int *idxb); +// +void d_dense_qcqp_get_lb(struct d_dense_qcqp *qp, double *lb); +// +void d_dense_qcqp_get_lb_mask(struct d_dense_qcqp *qp, double *lb); +// +void d_dense_qcqp_get_ub(struct d_dense_qcqp *qp, double *ub); +// +void d_dense_qcqp_get_ub_mask(struct d_dense_qcqp *qp, double *ub); +// +void d_dense_qcqp_get_C(struct d_dense_qcqp *qp, double *C); +// +void d_dense_qcqp_get_lg(struct d_dense_qcqp *qp, double *lg); +// +void d_dense_qcqp_get_lg_mask(struct d_dense_qcqp *qp, double *lg); +// +void d_dense_qcqp_get_ug(struct d_dense_qcqp *qp, double *ug); +// +void d_dense_qcqp_get_ug_mask(struct d_dense_qcqp *qp, double *ug); +// +void d_dense_qcqp_get_idxs(struct d_dense_qcqp *qp, int *idxs); +// +void d_dense_qcqp_get_idxs_rev(struct d_dense_qcqp *qp, int *idxs_rev); +// +void d_dense_qcqp_get_Zl(struct d_dense_qcqp *qp, double *Zl); +// +void d_dense_qcqp_get_Zu(struct d_dense_qcqp *qp, double *Zu); +// +void d_dense_qcqp_get_zl(struct d_dense_qcqp *qp, double *zl); +// +void d_dense_qcqp_get_zu(struct d_dense_qcqp *qp, double *zu); +// +void d_dense_qcqp_get_ls(struct d_dense_qcqp *qp, double *ls); +// +void d_dense_qcqp_get_ls_mask(struct d_dense_qcqp *qp, double *ls); +// +void d_dense_qcqp_get_us(struct d_dense_qcqp *qp, double *us); +// +void d_dense_qcqp_get_us_mask(struct d_dense_qcqp *qp, double *us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h new file mode 100644 index 0000000..fa8c574 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_dim.h @@ -0,0 +1,98 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_DIM_H_ +#define HPIPM_D_DENSE_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_dim + { + struct d_dense_qp_dim *qp_dim; // dim of qp approximation + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nq; // number of quadratic constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int nsq; // number of softened quadratic constraints + int ns; // number of softened constraints (nsb+nsg+nsq) TODO number of slacks + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_dim_memsize(); +// +void d_dense_qcqp_dim_create(struct d_dense_qcqp_dim *dim, void *memory); +// +void d_dense_qcqp_dim_set(char *field_name, int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nv(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ne(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nb(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ng(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nq(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsb(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsg(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_nsq(int value, struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_dim_set_ns(int value, struct d_dense_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_DIM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h new file mode 100644 index 0000000..fa3f98f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_ipm.h @@ -0,0 +1,193 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_IPM_H_ +#define HPIPM_D_DENSE_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_ipm_arg + { + struct d_dense_qp_ipm_arg *qp_arg; + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_dense_qcqp_ipm_ws + { + struct d_dense_qp_ipm_ws *qp_ws; + struct d_dense_qp *qp; + struct d_dense_qp_sol *qp_sol; + struct d_dense_qcqp_res_ws *qcqp_res_ws; + struct d_dense_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nv; + int iter; // iteration number + int status; + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_dense_qcqp_ipm_arg_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_ipm_arg_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_ipm_arg *arg, void *mem); +// +void d_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set(char *field, void *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_mu0(double *mu0, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_lam_min(double *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_t_min(double *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_split_step(int *value, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_dense_qcqp_ipm_ws_memsize(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_ipm_ws_create(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws, void *mem); +// +void d_dense_qcqp_ipm_get(char *field, struct d_dense_qcqp_ipm_ws *ws, void *value); +// +void d_dense_qcqp_ipm_get_status(struct d_dense_qcqp_ipm_ws *ws, int *status); +// +void d_dense_qcqp_ipm_get_iter(struct d_dense_qcqp_ipm_ws *ws, int *iter); +// +void d_dense_qcqp_ipm_get_max_res_stat(struct d_dense_qcqp_ipm_ws *ws, double *res_stat); +// +void d_dense_qcqp_ipm_get_max_res_eq(struct d_dense_qcqp_ipm_ws *ws, double *res_eq); +// +void d_dense_qcqp_ipm_get_max_res_ineq(struct d_dense_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_dense_qcqp_ipm_get_max_res_comp(struct d_dense_qcqp_ipm_ws *ws, double *res_comp); +// +void d_dense_qcqp_ipm_get_stat(struct d_dense_qcqp_ipm_ws *ws, double **stat); +// +void d_dense_qcqp_ipm_get_stat_m(struct d_dense_qcqp_ipm_ws *ws, int *stat_m); +// +void d_dense_qcqp_init_var(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +// +void d_dense_qcqp_ipm_solve(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +#if 0 +// +void d_dense_qcqp_ipm_predict(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +// +void d_dense_qcqp_ipm_sens(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_ipm_arg *arg, struct d_dense_qcqp_ipm_ws *ws); +#endif + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_IPM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h new file mode 100644 index 0000000..a76f162 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_res.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_RES_H_ +#define HPIPM_D_DENSE_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_res + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // infinity norm of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_dense_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nv; // work space of size nv + struct blasfeo_dvec *tmp_nbgq; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_ns; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_res_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_res_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res *res, void *mem); +// +hpipm_size_t d_dense_qcqp_res_ws_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_res_ws_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_res_ws *workspace, void *mem); +// +void d_dense_qcqp_res_compute(struct d_dense_qcqp *qp, struct d_dense_qcqp_sol *qp_sol, struct d_dense_qcqp_res *res, struct d_dense_qcqp_res_ws *ws); +// +void d_dense_qcqp_res_compute_inf_norm(struct d_dense_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QCQP_RES_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h new file mode 100644 index 0000000..6c697a8 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_sol.h @@ -0,0 +1,85 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QCQP_SOL_H_ +#define HPIPM_D_DENSE_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qcqp_sol + { + struct d_dense_qcqp_dim *dim; + struct blasfeo_dvec *v; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qcqp_sol_memsize(struct d_dense_qcqp_dim *dim); +// +void d_dense_qcqp_sol_create(struct d_dense_qcqp_dim *dim, struct d_dense_qcqp_sol *qp_sol, void *memory); +// +void d_dense_qcqp_sol_get_v(struct d_dense_qcqp_sol *qp_sol, double *v); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_SOL_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h new file mode 100644 index 0000000..a34218b --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qcqp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QCQP_UTILS_H_ +#define HPIPM_D_DENSE_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_dense_qcqp_dim.h" +#include "hpipm_d_dense_qcqp.h" +#include "hpipm_d_dense_qcqp_sol.h" +//#include "hpipm_d_dense_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_dense_qcqp_dim_print(struct d_dense_qcqp_dim *qp_dim); +// +//void d_dense_qcqp_dim_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim); +// +void d_dense_qcqp_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp); +// +//void d_dense_qcqp_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp *qp); +// +void d_dense_qcqp_sol_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_sol *dense_qcqp_sol); +// +//void d_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_ipm_arg *arg); +// +void d_dense_qcqp_res_print(struct d_dense_qcqp_dim *qp_dim, struct d_dense_qcqp_res *dense_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QCQP_UTILS_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h new file mode 100644 index 0000000..02fba5a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp.h @@ -0,0 +1,207 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_H_ +#define HPIPM_D_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp + { + struct d_dense_qp_dim *dim; + struct blasfeo_dmat *Hv; // hessian of cost & vector work space + struct blasfeo_dmat *A; // equality constraint matrix + struct blasfeo_dmat *Ct; // inequality constraints matrix + struct blasfeo_dvec *gz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *b; // equality constraint vector + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_dense_qp_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_create(struct d_dense_qp_dim *dim, struct d_dense_qp *qp, void *memory); + +// setters - colmaj +// +void d_dense_qp_set_all(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp); +// +void d_dense_qp_get_all(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us); +// +void d_dense_qp_set(char *field, void *value, struct d_dense_qp *qp); +// +void d_dense_qp_set_H(double *H, struct d_dense_qp *qp); +// +void d_dense_qp_set_g(double *g, struct d_dense_qp *qp); +// +void d_dense_qp_set_A(double *A, struct d_dense_qp *qp); +// +void d_dense_qp_set_b(double *b, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxb(int *idxb, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jb(double *Jb, struct d_dense_qp *qp); +// +void d_dense_qp_set_lb(double *lb, struct d_dense_qp *qp); +// +void d_dense_qp_set_lb_mask(double *lb, struct d_dense_qp *qp); +// +void d_dense_qp_set_ub(double *ub, struct d_dense_qp *qp); +// +void d_dense_qp_set_ub_mask(double *ub, struct d_dense_qp *qp); +// +void d_dense_qp_set_C(double *C, struct d_dense_qp *qp); +// +void d_dense_qp_set_lg(double *lg, struct d_dense_qp *qp); +// +void d_dense_qp_set_lg_mask(double *lg, struct d_dense_qp *qp); +// +void d_dense_qp_set_ug(double *ug, struct d_dense_qp *qp); +// +void d_dense_qp_set_ug_mask(double *ug, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxs(int *idxs, struct d_dense_qp *qp); +// +void d_dense_qp_set_idxs_rev(int *idxs_rev, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jsb(double *Jsb, struct d_dense_qp *qp); +// +void d_dense_qp_set_Jsg(double *Jsg, struct d_dense_qp *qp); +// +void d_dense_qp_set_Zl(double *Zl, struct d_dense_qp *qp); +// +void d_dense_qp_set_Zu(double *Zu, struct d_dense_qp *qp); +// +void d_dense_qp_set_zl(double *zl, struct d_dense_qp *qp); +// +void d_dense_qp_set_zu(double *zu, struct d_dense_qp *qp); +// +void d_dense_qp_set_ls(double *ls, struct d_dense_qp *qp); +// +void d_dense_qp_set_ls_mask(double *ls, struct d_dense_qp *qp); +// +void d_dense_qp_set_us(double *us, struct d_dense_qp *qp); +// +void d_dense_qp_set_us_mask(double *us, struct d_dense_qp *qp); + +// getters - colmaj +// +void d_dense_qp_get_H(struct d_dense_qp *qp, double *H); +// +void d_dense_qp_get_g(struct d_dense_qp *qp, double *g); +// +void d_dense_qp_get_A(struct d_dense_qp *qp, double *A); +// +void d_dense_qp_get_b(struct d_dense_qp *qp, double *b); +// +void d_dense_qp_get_idxb(struct d_dense_qp *qp, int *idxb); +// +void d_dense_qp_get_lb(struct d_dense_qp *qp, double *lb); +// +void d_dense_qp_get_lb_mask(struct d_dense_qp *qp, double *lb); +// +void d_dense_qp_get_ub(struct d_dense_qp *qp, double *ub); +// +void d_dense_qp_get_ub_mask(struct d_dense_qp *qp, double *ub); +// +void d_dense_qp_get_C(struct d_dense_qp *qp, double *C); +// +void d_dense_qp_get_lg(struct d_dense_qp *qp, double *lg); +// +void d_dense_qp_get_lg_mask(struct d_dense_qp *qp, double *lg); +// +void d_dense_qp_get_ug(struct d_dense_qp *qp, double *ug); +// +void d_dense_qp_get_ug_mask(struct d_dense_qp *qp, double *ug); +// +void d_dense_qp_get_idxs(struct d_dense_qp *qp, int *idxs); +// +void d_dense_qp_get_idxs_rev(struct d_dense_qp *qp, int *idxs_rev); +// +void d_dense_qp_get_Zl(struct d_dense_qp *qp, double *Zl); +// +void d_dense_qp_get_Zu(struct d_dense_qp *qp, double *Zu); +// +void d_dense_qp_get_zl(struct d_dense_qp *qp, double *zl); +// +void d_dense_qp_get_zu(struct d_dense_qp *qp, double *zu); +// +void d_dense_qp_get_ls(struct d_dense_qp *qp, double *ls); +// +void d_dense_qp_get_ls_mask(struct d_dense_qp *qp, double *ls); +// +void d_dense_qp_get_us(struct d_dense_qp *qp, double *us); +// +void d_dense_qp_get_us_mask(struct d_dense_qp *qp, double *us); + +// setters - rowmaj +// +void d_dense_qp_set_all_rowmaj(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us, struct d_dense_qp *qp); + +// getters - rowmaj +// +void d_dense_qp_get_all_rowmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, double *Zl, double *Zu, double *zl, double *zu, int *idxs, double *d_ls, double *d_us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h new file mode 100644 index 0000000..98a551f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_dim.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_DIM_H_ +#define HPIPM_D_DENSE_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_dim + { + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int ns; // number of softened constraints (nsb+nsg) + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_dim_memsize(); +// +void d_dense_qp_dim_create(struct d_dense_qp_dim *qp_dim, void *memory); +// +void d_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set(char *field_name, int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nv(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ne(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nb(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ng(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nsb(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_nsg(int value, struct d_dense_qp_dim *dim); +// +void d_dense_qp_dim_set_ns(int value, struct d_dense_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h new file mode 100644 index 0000000..e6e5d5d --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_ipm.h @@ -0,0 +1,260 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_IPM_H_ +#define HPIPM_D_DENSE_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_ipm_arg + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int kkt_fact_alg; // 0 null-space, 1 schur-complement + int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints + int compute_obj; // compute obj on exit + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_dense_qp_ipm_ws + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_dense_qp_res_ws *res_ws; + struct d_dense_qp_sol *sol_step; + struct d_dense_qp_sol *sol_itref; + struct d_dense_qp *qp_step; + struct d_dense_qp *qp_itref; + struct d_dense_qp_res *res; + struct d_dense_qp_res *res_itref; + struct d_dense_qp_res *res_step; + struct blasfeo_dvec *Gamma; // + struct blasfeo_dvec *gamma; // + struct blasfeo_dvec *Zs_inv; // + struct blasfeo_dmat *Lv; // + struct blasfeo_dmat *AL; // + struct blasfeo_dmat *Le; // + struct blasfeo_dmat *Ctx; // + struct blasfeo_dvec *lv; // + struct blasfeo_dvec *sv; // scale for Lv + struct blasfeo_dvec *se; // scale for Le + struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng + struct blasfeo_dvec *tmp_ns; // work space of size ns + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *lq1; + struct blasfeo_dvec *tmp_m; + struct blasfeo_dmat *A_LQ; + struct blasfeo_dmat *A_Q; + struct blasfeo_dmat *Zt; + struct blasfeo_dmat *ZtH; + struct blasfeo_dmat *ZtHZ; + struct blasfeo_dvec *xy; + struct blasfeo_dvec *Yxy; + struct blasfeo_dvec *xz; + struct blasfeo_dvec *tmp_nv; + struct blasfeo_dvec *tmp_2ns; + struct blasfeo_dvec *tmp_nv2ns; + struct blasfeo_dmat *A_li; // A of linearly independent equality constraints + struct blasfeo_dvec *b_li; // b of linearly independent equality constraints + struct blasfeo_dmat *A_bkp; // pointer to backup A + struct blasfeo_dvec *b_bkp; // pointer to backup b + struct blasfeo_dmat *Ab_LU; + double *stat; // convergence statistics + int *ipiv_v; + int *ipiv_e; + int *ipiv_e1; + void *lq_work0; + void *lq_work1; + void *lq_work_null; + void *orglq_work_null; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // numer of recorded stat per ipm iter + int scale; + int use_hess_fact; + int use_A_fact; + int status; + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int ne_li; // number of linearly independent equality constraints + int ne_bkp; // ne backup + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t d_dense_qp_ipm_arg_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_ipm_arg_create(struct d_dense_qp_dim *dim, struct d_dense_qp_ipm_arg *arg, void *mem); +// +void d_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set(char *field, void *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_mu0(double *mu0, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_reg_prim(double *reg, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_reg_dual(double *reg, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_lam_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_t_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_tau_min(double *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_compute_obj(int *value, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_arg_set_t_lam_min(int *value, struct d_dense_qp_ipm_arg *arg); + +// +void d_dense_qp_ipm_arg_set_split_step(int *value, struct d_dense_qp_ipm_arg *arg); + +// +hpipm_size_t d_dense_qp_ipm_ws_memsize(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_ipm_ws_create(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws, void *mem); +// +void d_dense_qp_ipm_get(char *field, struct d_dense_qp_ipm_ws *ws, void *value); +// +void d_dense_qp_ipm_get_status(struct d_dense_qp_ipm_ws *ws, int *status); +// +void d_dense_qp_ipm_get_iter(struct d_dense_qp_ipm_ws *ws, int *iter); +// +void d_dense_qp_ipm_get_max_res_stat(struct d_dense_qp_ipm_ws *ws, double *res_stat); +// +void d_dense_qp_ipm_get_max_res_eq(struct d_dense_qp_ipm_ws *ws, double *res_eq); +// +void d_dense_qp_ipm_get_max_res_ineq(struct d_dense_qp_ipm_ws *ws, double *res_ineq); +// +void d_dense_qp_ipm_get_max_res_comp(struct d_dense_qp_ipm_ws *ws, double *res_comp); +// +void d_dense_qp_ipm_get_stat(struct d_dense_qp_ipm_ws *ws, double **stat); +// +void d_dense_qp_ipm_get_stat_m(struct d_dense_qp_ipm_ws *ws, int *stat_m); +// +void d_dense_qp_init_var(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_abs_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_delta_step(int kk, struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_solve(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_predict(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_ipm_sens(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_compute_step_length(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h new file mode 100644 index 0000000..6d05779 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_kkt.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_KKT_H_ +#define HPIPM_D_DENSE_QP_KKT_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_fact_solve_kkt_unconstr_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_fact_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_fact_lq_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_solve_kkt_step_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_remove_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_restore_lin_dep_eq(struct d_dense_qp *qp, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); +// +void d_dense_qp_compute_obj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_ipm_arg *arg, struct d_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_KKT_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h new file mode 100644 index 0000000..7c20232 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_res.h @@ -0,0 +1,105 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_RES_H_ +#define HPIPM_D_DENSE_QP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_res + { + struct d_dense_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_dense_qp_res_ws + { + struct blasfeo_dvec *tmp_nbg; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_ns; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_res_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_res_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res *res, void *mem); +// +hpipm_size_t d_dense_qp_res_ws_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_res_ws_create(struct d_dense_qp_dim *dim, struct d_dense_qp_res_ws *workspace, void *mem); +// +void d_dense_qp_res_compute(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws); +// +void d_dense_qp_res_compute_lin(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_dense_qp_sol *qp_step, struct d_dense_qp_res *res, struct d_dense_qp_res_ws *ws); +// +void d_dense_qp_res_compute_inf_norm(struct d_dense_qp_res *res); +// +void d_dense_qp_res_get_all(struct d_dense_qp_res *res, double *res_g, double *res_ls, double *res_us, double *res_b, double *res_d_lb, double *res_d_ub, double *res_d_lg, double *res_d_ug, double *res_d_ls, double *res_d_us, double *res_m_lb, double *res_m_ub, double *res_m_lg, double *res_m_ug, double *res_m_ls, double *res_m_us); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QP_RES_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h new file mode 100644 index 0000000..aaa3fdb --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_sol.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_DENSE_QP_SOL_H_ +#define HPIPM_D_DENSE_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_dense_qp_sol + { + struct d_dense_qp_dim *dim; + struct blasfeo_dvec *v; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + double obj; + int valid_obj; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_dense_qp_sol_memsize(struct d_dense_qp_dim *dim); +// +void d_dense_qp_sol_create(struct d_dense_qp_dim *dim, struct d_dense_qp_sol *qp_sol, void *memory); +// +void d_dense_qp_sol_get_all(struct d_dense_qp_sol *qp_sol, double *v, double *ls, double *us, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug, double *lam_ls, double *lam_us); +// +void d_dense_qp_sol_get(char *field, struct d_dense_qp_sol *sol, void *value); +// +void d_dense_qp_sol_get_v(struct d_dense_qp_sol *sol, double *v); +// +void d_dense_qp_sol_get_valid_obj(struct d_dense_qp_sol *sol, int *valid_obj); +// +void d_dense_qp_sol_get_obj(struct d_dense_qp_sol *sol, double *obj); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_DENSE_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h new file mode 100644 index 0000000..ccb77aa --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_dense_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_DENSE_QP_UTILS_H_ +#define HPIPM_D_DENSE_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" +#include "hpipm_d_dense_qp.h" +#include "hpipm_d_dense_qp_sol.h" +#include "hpipm_d_dense_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_dense_qp_dim_print(struct d_dense_qp_dim *qp_dim); +// +//void d_dense_qp_dim_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim); +// +void d_dense_qp_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp); +// +//void d_dense_qp_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp *qp); +// +void d_dense_qp_sol_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_sol *dense_qp_sol); +// +//void d_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *arg); +// +void d_dense_qp_res_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_res *dense_qp_res); +// +void d_dense_qp_arg_print(struct d_dense_qp_dim *qp_dim, struct d_dense_qp_ipm_arg *qp_ipm_arg); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_DENSE_QP_UTILS_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h new file mode 100644 index 0000000..240c9b0 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp.h @@ -0,0 +1,303 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_H_ +#define HPIPM_D_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space + struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space + struct blasfeo_dmat *DCt; // inequality constraints matrix + struct blasfeo_dmat **Hq; // hessians of quadratic constraints + struct blasfeo_dvec *b; // dynamics vector + struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qcqp_strsize(); +// +hpipm_size_t d_ocp_qcqp_memsize(struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp *qp, void *memory); +// +void d_ocp_qcqp_copy_all(struct d_ocp_qcqp *qp_orig, struct d_ocp_qcqp *qp_dest); + +// setters +// +void d_ocp_qcqp_set_all_zero(struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_rhs_zero(struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set(char *field_name, int stage, void *value, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_A(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_B(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_b(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Q(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_S(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_R(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_q(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_r(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lb(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lb_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ub(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ub_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbx_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubx_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lbu_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ubu_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxb(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxbx(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxbu(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_C(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_D(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lg(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lg_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ug(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_ug_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Qq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Sq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Rq(int stage, double *mat, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_qq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_rq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_uq(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_uq_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Zl(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Zu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_zl(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_zu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lls(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lls_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lus(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_lus_mask(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxs(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsbu(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsbx(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsg(int stage, double *vec, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_set_Jsq(int stage, double *vec, struct d_ocp_qcqp *qp); + +// getters +// +void d_ocp_qcqp_get(char *field, int stage, struct d_ocp_qcqp *qp, void *value); +// +void d_ocp_qcqp_get_A(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_B(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_b(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Q(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_S(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_R(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_q(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_r(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ub(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ub_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lb(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lb_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbx_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubx_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lbu_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ubu_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_idxb(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_idxbx(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jbx(int stage, struct d_ocp_qcqp *qp, double *vec); +// +//void d_ocp_qcqp_get_idxbu(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jbu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_C(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_D(int stage, struct d_ocp_qcqp *qp, double *mat); +// +void d_ocp_qcqp_get_lg(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lg_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ug(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_ug_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Zl(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_Zu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_zl(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_zu(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lls(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lls_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lus(int stage, struct d_ocp_qcqp *qp, double *vec); +// +void d_ocp_qcqp_get_lus_mask(int stage, struct d_ocp_qcqp *qp, double *vec); +// XXX only valid if there is one slack per softed constraint !!! +void d_ocp_qcqp_get_idxs(int stage, struct d_ocp_qcqp *qp, int *vec); +// +void d_ocp_qcqp_get_idxs_rev(int stage, struct d_ocp_qcqp *qp, int *vec); +// +//void d_ocp_qcqp_get_Jsbu(int stage, struct d_ocp_qcqp *qp, float *vec); +// +//void d_ocp_qcqp_get_Jsbx(int stage, struct d_ocp_qcqp *qp, float *vec); +// +//void d_ocp_qcqp_get_Jsg(int stage, struct d_ocp_qcqp *qp, float *vec); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h new file mode 100644 index 0000000..268628a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_dim.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_DIM_H_ +#define HPIPM_D_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_dim + { + struct d_ocp_qp_dim *qp_dim; // dim of qp approximation + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of (two-sided) state box constraints + int *nbu; // number of (two-sided) input box constraints + int *ng; // number of (two-sided) general constraints + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints + int *nsbx; // number of (two-sided) soft state box constraints + int *nsbu; // number of (two-sided) soft input box constraints + int *nsg; // number of (two-sided) soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_dim_strsize(); +// +hpipm_size_t d_ocp_qcqp_dim_memsize(int N); +// +void d_ocp_qcqp_dim_create(int N, struct d_ocp_qcqp_dim *qp_dim, void *memory); +// +void d_ocp_qcqp_dim_copy_all(struct d_ocp_qcqp_dim *dim_orig, struct d_ocp_qcqp_dim *dim_dest); +// +void d_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_ng(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nq(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_ns(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_dim_get(struct d_ocp_qcqp_dim *dim, char *field, int stage, int *value); +// +void d_ocp_qcqp_dim_get_N(struct d_ocp_qcqp_dim *dim, int *value); +// +void d_ocp_qcqp_dim_get_nx(struct d_ocp_qcqp_dim *dim, int stage, int *value); +// +void d_ocp_qcqp_dim_get_nu(struct d_ocp_qcqp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_DIM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h new file mode 100644 index 0000000..99f2329 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_ipm.h @@ -0,0 +1,190 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_IPM_H_ +#define HPIPM_D_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_ipm_arg + { + struct d_ocp_qp_ipm_arg *qp_arg; + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_ocp_qcqp_ipm_ws + { + struct d_ocp_qp_ipm_ws *qp_ws; + struct d_ocp_qp *qp; + struct d_ocp_qp_sol *qp_sol; + struct d_ocp_qcqp_res_ws *qcqp_res_ws; + struct d_ocp_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t d_ocp_qcqp_ipm_arg_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_ipm_arg_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, void *mem); +// +void d_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void d_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void d_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void d_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void d_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void d_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void d_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +void d_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void d_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void d_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void d_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t d_ocp_qcqp_ipm_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_ipm_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws, void *mem); +// +void d_ocp_qcqp_ipm_get(char *field, struct d_ocp_qcqp_ipm_ws *ws, void *value); +// +void d_ocp_qcqp_ipm_get_status(struct d_ocp_qcqp_ipm_ws *ws, int *status); +// +void d_ocp_qcqp_ipm_get_iter(struct d_ocp_qcqp_ipm_ws *ws, int *iter); +// +void d_ocp_qcqp_ipm_get_max_res_stat(struct d_ocp_qcqp_ipm_ws *ws, double *res_stat); +// +void d_ocp_qcqp_ipm_get_max_res_eq(struct d_ocp_qcqp_ipm_ws *ws, double *res_eq); +// +void d_ocp_qcqp_ipm_get_max_res_ineq(struct d_ocp_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_ocp_qcqp_ipm_get_max_res_comp(struct d_ocp_qcqp_ipm_ws *ws, double *res_comp); +// +void d_ocp_qcqp_ipm_get_stat(struct d_ocp_qcqp_ipm_ws *ws, double **stat); +// +void d_ocp_qcqp_ipm_get_stat_m(struct d_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void d_ocp_qcqp_init_var(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws); +// +void d_ocp_qcqp_ipm_solve(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_ipm_arg *arg, struct d_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QCQP_IPM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h new file mode 100644 index 0000000..077f134 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_res.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_RES_H_ +#define HPIPM_D_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_res + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_ocp_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qcqp_res_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_res_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t d_ocp_qcqp_res_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim); +// +void d_ocp_qcqp_res_ws_create(struct d_ocp_qcqp_dim *ocp_dim, struct d_ocp_qcqp_res_ws *workspace, void *mem); +// +void d_ocp_qcqp_res_compute(struct d_ocp_qcqp *qp, struct d_ocp_qcqp_sol *qp_sol, struct d_ocp_qcqp_res *res, struct d_ocp_qcqp_res_ws *ws); +// +void d_ocp_qcqp_res_compute_inf_norm(struct d_ocp_qcqp_res *res); +// +void d_ocp_qcqp_res_get_max_res_stat(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_eq(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_ineq(struct d_ocp_qcqp_res *res, double *value); +// +void d_ocp_qcqp_res_get_max_res_comp(struct d_ocp_qcqp_res *res, double *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QCQP_RES_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h new file mode 100644 index 0000000..68adbfc --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_sol.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_SOL_H_ +#define HPIPM_D_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qcqp_sol + { + struct d_ocp_qcqp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qcqp_sol_strsize(); +// +hpipm_size_t d_ocp_qcqp_sol_memsize(struct d_ocp_qcqp_dim *dim); +// +void d_ocp_qcqp_sol_create(struct d_ocp_qcqp_dim *dim, struct d_ocp_qcqp_sol *qp_sol, void *memory); +// +void d_ocp_qcqp_sol_copy_all(struct d_ocp_qcqp_sol *qp_sol_orig, struct d_ocp_qcqp_sol *qp_sol_dest); +// +void d_ocp_qcqp_sol_get(char *field, int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_u(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_x(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_sl(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_su(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_pi(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_lb(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_ub(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_lg(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_get_lam_ug(int stage, struct d_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_ocp_qcqp_sol_set(char *field, int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_u(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_x(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_sl(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); +// +void d_ocp_qcqp_sol_set_su(int stage, double *vec, struct d_ocp_qcqp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_SOL_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h new file mode 100644 index 0000000..00248f1 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qcqp_utils.h @@ -0,0 +1,81 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QCQP_UTILS_H_ +#define HPIPM_D_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qcqp_dim.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qcqp_sol.h" +#include "hpipm_d_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_ocp_qcqp_dim_print(struct d_ocp_qcqp_dim *qcqp_dim); +// +void d_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim); +// +void d_ocp_qcqp_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp *qp); +// +void d_ocp_qcqp_sol_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_sol *ocp_qcqp_sol); +// +void d_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_ipm_arg *arg); +// +void d_ocp_qcqp_res_print(struct d_ocp_qcqp_dim *qcqp_dim, struct d_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QCQP_UTILS_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h new file mode 100644 index 0000000..ca26cdb --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp.h @@ -0,0 +1,306 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_H_ +#define HPIPM_D_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dmat *BAbt; // dynamics matrix & vector work space + struct blasfeo_dmat *RSQrq; // hessian of cost & vector work space + struct blasfeo_dmat *DCt; // inequality constraints matrix + struct blasfeo_dvec *b; // dynamics vector + struct blasfeo_dvec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_dvec *d; // inequality constraints vector + struct blasfeo_dvec *d_mask; // inequality constraints mask vector + struct blasfeo_dvec *m; // rhs of complementarity condition + struct blasfeo_dvec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1] + int *diag_H_flag; // flag the fact that Hessian is diagonal + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qp_strsize(); +// +hpipm_size_t d_ocp_qp_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp *qp, void *memory); +// +void d_ocp_qp_copy_all(struct d_ocp_qp *qp_orig, struct d_ocp_qp *qp_dest); + +// setters +// +void d_ocp_qp_set_all_zero(struct d_ocp_qp *qp); +// +void d_ocp_qp_set_rhs_zero(struct d_ocp_qp *qp); +// +void d_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_all_rowmaj(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxbx, double **lbx, double **ubx, int **idxbu, double **lbu, double **ubu, double **C, double **D, double **lg, double **ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **ls, double **us, struct d_ocp_qp *qp); +// +void d_ocp_qp_set(char *field_name, int stage, void *value, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el(char *field_name, int stage, int index, void *value, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_A(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_B(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_b(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Q(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_S(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_R(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_q(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_r(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lb(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lb_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ub(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ub_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbx_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el_lbx(int stage, int index, double *elem, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubx_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_el_ubx(int stage, int index, double *elem, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lbu_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ubu_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxb(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbx(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbu(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_C(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_D(int stage, double *mat, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lg(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lg_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ug(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_ug_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Zl(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Zu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_zl(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_zu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lls(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lls_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lus(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_lus_mask(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxs(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxs_rev(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsbu(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsbx(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jsg(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxe(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbxe(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxbue(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_idxge(int stage, int *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbxe(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jbue(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_Jge(int stage, double *vec, struct d_ocp_qp *qp); +// +void d_ocp_qp_set_diag_H_flag(int stage, int *value, struct d_ocp_qp *qp); + +// getters +// +void d_ocp_qp_get(char *field, int stage, struct d_ocp_qp *qp, void *value); +// +void d_ocp_qp_get_A(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_B(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_b(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Q(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_S(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_R(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_q(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_r(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ub(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ub_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lb(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lb_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbx(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbx_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubx(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubx_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lbu_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ubu_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_idxb(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_idxbx(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jbx(int stage, struct d_ocp_qp *qp, double *vec); +// +//void d_ocp_qp_get_idxbu(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jbu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_C(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_D(int stage, struct d_ocp_qp *qp, double *mat); +// +void d_ocp_qp_get_lg(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lg_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ug(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_ug_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Zl(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_Zu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_zl(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_zu(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lls(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lls_mask(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lus(int stage, struct d_ocp_qp *qp, double *vec); +// +void d_ocp_qp_get_lus_mask(int stage, struct d_ocp_qp *qp, double *vec); +// XXX only valid if there is one slack per softed constraint !!! +void d_ocp_qp_get_idxs(int stage, struct d_ocp_qp *qp, int *vec); +// +void d_ocp_qp_get_idxs_rev(int stage, struct d_ocp_qp *qp, int *vec); +// +//void d_ocp_qp_get_Jsbu(int stage, struct d_ocp_qp *qp, float *vec); +// +//void d_ocp_qp_get_Jsbx(int stage, struct d_ocp_qp *qp, float *vec); +// +//void d_ocp_qp_get_Jsg(int stage, struct d_ocp_qp *qp, float *vec); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h new file mode 100644 index 0000000..dcfb801 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_dim.h @@ -0,0 +1,142 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_DIM_H_ +#define HPIPM_D_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_dim + { + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of state box constraints + int *nbu; // number of input box constraints + int *ng; // number of general constraints + int *ns; // number of soft constraints + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nbxe; // number of state box constraints which are equality + int *nbue; // number of input box constraints which are equality + int *nge; // number of general constraints which are equality + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_dim_strsize(); +// +hpipm_size_t d_ocp_qp_dim_memsize(int N); +// +void d_ocp_qp_dim_create(int N, struct d_ocp_qp_dim *qp_dim, void *memory); +// +void d_ocp_qp_dim_copy_all(struct d_ocp_qp_dim *dim_orig, struct d_ocp_qp_dim *dim_dest); +// +void d_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set(char *field, int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_ng(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_ns(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsbx(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsbu(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nsg(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbxe(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nbue(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_set_nge(int stage, int value, struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_dim_get(struct d_ocp_qp_dim *dim, char *field, int stage, int *value); +// +void d_ocp_qp_dim_get_N(struct d_ocp_qp_dim *dim, int *value); +// +void d_ocp_qp_dim_get_nx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_ng(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_ns(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsbx(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsbu(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nsg(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbxe(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nbue(struct d_ocp_qp_dim *dim, int stage, int *value); +// +void d_ocp_qp_dim_get_nge(struct d_ocp_qp_dim *dim, int stage, int *value); + + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h new file mode 100644 index 0000000..d2a2833 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_ipm.h @@ -0,0 +1,250 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_IPM_H_ +#define HPIPM_D_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_ipm_arg + { + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different steps for primal and dual variables + int var_init_scheme; // variables initialization scheme + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_ocp_qp_ipm_ws + { + double qp_res[4]; // infinity norm of residuals + struct d_core_qp_ipm_workspace *core_workspace; + struct d_ocp_qp_dim *dim; // cache dim + struct d_ocp_qp_res_ws *res_workspace; + struct d_ocp_qp_sol *sol_step; + struct d_ocp_qp_sol *sol_itref; + struct d_ocp_qp *qp_step; + struct d_ocp_qp *qp_itref; + struct d_ocp_qp_res *res_itref; + struct d_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nuxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dvec *tmp_m; + struct blasfeo_dvec *l; // cache linear part for _get_ric_xxx + struct blasfeo_dmat *L; + struct blasfeo_dmat *Ls; + struct blasfeo_dmat *P; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *tmp_nxM_nxM; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int square_root_alg; // cache from arg + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int valid_ric_vec; // meaningful riccati vectors + int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_ipm_arg_strsize(); +// +hpipm_size_t d_ocp_qp_ipm_arg_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_ipm_arg_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, void *mem); +// +void d_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_arg_set(char *field, void *value, struct d_ocp_qp_ipm_arg *arg); +// set maximum number of iterations +void d_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_ocp_qp_ipm_arg *arg); +// set minimum step lenght +void d_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_ocp_qp_ipm_arg *arg); +// set initial value of barrier parameter +void d_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_ocp_qp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_ocp_qp_ipm_arg *arg); +// set regularization of primal variables +void d_ocp_qp_ipm_arg_set_reg_prim(double *tol_comp, struct d_ocp_qp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_ocp_qp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_ocp_qp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_ocp_qp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void d_ocp_qp_ipm_arg_set_ric_alg(int *value, struct d_ocp_qp_ipm_arg *arg); +// dual solution of equality constraints (only for abs_form==1) +void d_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_ocp_qp_ipm_arg *arg); +// compute residuals after solution +void d_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_ocp_qp_ipm_arg *arg); +// compute residuals of prediction +void d_ocp_qp_ipm_arg_set_comp_res_pred(int *value, struct d_ocp_qp_ipm_arg *arg); +// min value of lam in the solution +void d_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// min value of t in the solution +void d_ocp_qp_ipm_arg_set_t_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// min value of tau in the solution +void d_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_ocp_qp_ipm_arg *arg); +// set split step: 0 same step, 1 different step for primal and dual variables +void d_ocp_qp_ipm_arg_set_split_step(int *value, struct d_ocp_qp_ipm_arg *arg); +// variables initialization scheme +void d_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct d_ocp_qp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t d_ocp_qp_ipm_ws_strsize(); +// +hpipm_size_t d_ocp_qp_ipm_ws_memsize(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, void *mem); +// +void d_ocp_qp_ipm_get(char *field, struct d_ocp_qp_ipm_ws *ws, void *value); +// +void d_ocp_qp_ipm_get_status(struct d_ocp_qp_ipm_ws *ws, int *status); +// +void d_ocp_qp_ipm_get_iter(struct d_ocp_qp_ipm_ws *ws, int *iter); +// +void d_ocp_qp_ipm_get_max_res_stat(struct d_ocp_qp_ipm_ws *ws, double *res_stat); +// +void d_ocp_qp_ipm_get_max_res_eq(struct d_ocp_qp_ipm_ws *ws, double *res_eq); +// +void d_ocp_qp_ipm_get_max_res_ineq(struct d_ocp_qp_ipm_ws *ws, double *res_ineq); +// +void d_ocp_qp_ipm_get_max_res_comp(struct d_ocp_qp_ipm_ws *ws, double *res_comp); +// +void d_ocp_qp_ipm_get_stat(struct d_ocp_qp_ipm_ws *ws, double **stat); +// +void d_ocp_qp_ipm_get_stat_m(struct d_ocp_qp_ipm_ws *ws, int *stat_m); +// +void d_ocp_qp_ipm_get_ric_Lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Lr); +// +void d_ocp_qp_ipm_get_ric_Ls(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *Ls); +// +void d_ocp_qp_ipm_get_ric_P(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *P); +// +void d_ocp_qp_ipm_get_ric_lr(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *lr); +// +void d_ocp_qp_ipm_get_ric_p(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *p); +// feedback control gain in the form u = K x + k +void d_ocp_qp_ipm_get_ric_K(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *K); +// feedback control gain in the form u = K x + k +void d_ocp_qp_ipm_get_ric_k(struct d_ocp_qp *qp, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws, int stage, double *k); +// +void d_ocp_qp_init_var(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_abs_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_delta_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_solve(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_predict(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_ipm_sens(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); + + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h new file mode 100644 index 0000000..19dcec3 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_kkt.h @@ -0,0 +1,66 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_KKT_H_ +#define HPIPM_D_OCP_QP_KKT_H_ + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + +// +void d_ocp_qp_fact_solve_kkt_unconstr(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_fact_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_fact_lq_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); +// +void d_ocp_qp_solve_kkt_step(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + +#endif // HPIPM_D_OCP_QP_KKT_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h new file mode 100644 index 0000000..f0482b0 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_red.h @@ -0,0 +1,117 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_RED_H_ +#define HPIPM_D_OCP_QP_RED_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_reduce_eq_dof_arg + { + double lam_min; + double t_min; + int alias_unchanged; // do not keep copy unchanged stage + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + hpipm_size_t memsize; // memory size in bytes + }; + + + +struct d_ocp_qp_reduce_eq_dof_ws + { + struct blasfeo_dvec *tmp_nuxM; + struct blasfeo_dvec *tmp_nbgM; + int *e_imask_ux; + int *e_imask_d; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +void d_ocp_qp_dim_reduce_eq_dof(struct d_ocp_qp_dim *dim, struct d_ocp_qp_dim *dim_red); +// +hpipm_size_t d_ocp_qp_reduce_eq_dof_arg_memsize(); +// +void d_ocp_qp_reduce_eq_dof_arg_create(struct d_ocp_qp_reduce_eq_dof_arg *arg, void *mem); +// +void d_ocp_qp_reduce_eq_dof_arg_set_default(struct d_ocp_qp_reduce_eq_dof_arg *arg); +// +void d_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct d_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +hpipm_size_t d_ocp_qp_reduce_eq_dof_ws_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_reduce_eq_dof_ws_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_reduce_eq_dof_ws *work, void *mem); +// +void d_ocp_qp_reduce_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_reduce_eq_dof_lhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_reduce_eq_dof_rhs(struct d_ocp_qp *qp, struct d_ocp_qp *qp_red, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); +// +void d_ocp_qp_restore_eq_dof(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol_red, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_reduce_eq_dof_arg *arg, struct d_ocp_qp_reduce_eq_dof_ws *work); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_RED_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h new file mode 100644 index 0000000..061c572 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_res.h @@ -0,0 +1,112 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_RES_H_ +#define HPIPM_D_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_res + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_ocp_qp_res_ws + { + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_ocp_qp_res_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_res_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res *res, void *mem); +// +hpipm_size_t d_ocp_qp_res_ws_memsize(struct d_ocp_qp_dim *ocp_dim); +// +void d_ocp_qp_res_ws_create(struct d_ocp_qp_dim *ocp_dim, struct d_ocp_qp_res_ws *workspace, void *mem); +// +void d_ocp_qp_res_compute(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws); +// +void d_ocp_qp_res_compute_lin(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_sol *qp_step, struct d_ocp_qp_res *res, struct d_ocp_qp_res_ws *ws); +// +void d_ocp_qp_res_compute_inf_norm(struct d_ocp_qp_res *res); +// +void d_ocp_qp_res_get_all(struct d_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us); +// +void d_ocp_qp_res_get_max_res_stat(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_eq(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_ineq(struct d_ocp_qp_res *res, double *value); +// +void d_ocp_qp_res_get_max_res_comp(struct d_ocp_qp_res *res, double *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_OCP_QP_RES_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h new file mode 100644 index 0000000..eb91f3a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_sol.h @@ -0,0 +1,128 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_SOL_H_ +#define HPIPM_D_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_ocp_qp_sol + { + struct d_ocp_qp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_ocp_qp_sol_strsize(); +// +hpipm_size_t d_ocp_qp_sol_memsize(struct d_ocp_qp_dim *dim); +// +void d_ocp_qp_sol_create(struct d_ocp_qp_dim *dim, struct d_ocp_qp_sol *qp_sol, void *memory); +// +void d_ocp_qp_sol_copy_all(struct d_ocp_qp_sol *qp_sol_orig, struct d_ocp_qp_sol *qp_sol_dest); +// +void d_ocp_qp_sol_get_all(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_ocp_qp_sol_get_all_rowmaj(struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_ocp_qp_sol_set_all(double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_get(char *field, int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_u(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_x(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_sl(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_su(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_pi(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lb(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lbu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lbx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ub(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ubu(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ubx(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_lg(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_get_lam_ug(int stage, struct d_ocp_qp_sol *qp_sol, double *vec); +// +void d_ocp_qp_sol_set(char *field, int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_u(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_x(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_sl(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); +// +void d_ocp_qp_sol_set_su(int stage, double *vec, struct d_ocp_qp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h new file mode 100644 index 0000000..217e9d9 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_ocp_qp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_OCP_QP_UTILS_H_ +#define HPIPM_D_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_ocp_qp_dim.h" +#include "hpipm_d_ocp_qp.h" +#include "hpipm_d_ocp_qp_sol.h" +#include "hpipm_d_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_ocp_qp_dim_print(struct d_ocp_qp_dim *qp_dim); +// +void d_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim); +// +void d_ocp_qp_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp); +// +void d_ocp_qp_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp *qp); +// +void d_ocp_qp_sol_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_sol *ocp_qp_sol); +// +void d_ocp_qp_ipm_arg_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_ipm_arg *arg); +// +void d_ocp_qp_res_print(struct d_ocp_qp_dim *qp_dim, struct d_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_UTILS_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h b/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h new file mode 100644 index 0000000..1c5f785 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_part_cond.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_PART_COND_H_ +#define HPIPM_D_PART_COND_H_ + + + +#include +#include + +#include "hpipm_common.h" +#include "hpipm_d_cond.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_part_cond_qp_arg + { + struct d_cond_qp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct d_part_cond_qp_ws + { + struct d_cond_qp_ws *cond_workspace; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_part_cond_qp_arg_memsize(int N2); +// +void d_part_cond_qp_arg_create(int N2, struct d_part_cond_qp_arg *cond_arg, void *mem); +// +void d_part_cond_qp_arg_set_default(struct d_part_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void d_part_cond_qp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_prim_sol(int value, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct d_part_cond_qp_arg *cond_arg); + +// +void d_part_cond_qp_compute_block_size(int N, int N2, int *block_size); +// +void d_part_cond_qp_compute_dim(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim); +// +hpipm_size_t d_part_cond_qp_ws_memsize(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg); +// +void d_part_cond_qp_ws_create(struct d_ocp_qp_dim *ocp_dim, int *block_size, struct d_ocp_qp_dim *part_dense_dim, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws, void *mem); +// +void d_part_cond_qp_cond(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_cond_lhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_cond_rhs(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); +// +void d_part_cond_qp_expand_sol(struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_ocp_qp_sol *part_dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); + +// +void d_part_cond_qp_update(int *idxc, struct d_ocp_qp *ocp_qp, struct d_ocp_qp *part_dense_qp, struct d_part_cond_qp_arg *cond_arg, struct d_part_cond_qp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h new file mode 100644 index 0000000..2d6624e --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_part_cond_qcqp.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_PART_COND_QCQP_H_ +#define HPIPM_D_PART_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_d_cond_qcqp.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_part_cond_qcqp_arg + { + struct d_cond_qcqp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct d_part_cond_qcqp_ws + { + struct d_cond_qcqp_ws *cond_ws; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_part_cond_qcqp_arg_memsize(int N2); +// +void d_part_cond_qcqp_arg_create(int N2, struct d_part_cond_qcqp_arg *cond_arg, void *mem); +// +void d_part_cond_qcqp_arg_set_default(struct d_part_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void d_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct d_part_cond_qcqp_arg *cond_arg); + +// +void d_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size); +// +void d_part_cond_qcqp_compute_dim(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim); +// +hpipm_size_t d_part_cond_qcqp_ws_memsize(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg); +// +void d_part_cond_qcqp_ws_create(struct d_ocp_qcqp_dim *ocp_dim, int *block_size, struct d_ocp_qcqp_dim *part_dense_dim, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws, void *mem); +// +void d_part_cond_qcqp_cond(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_cond_lhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_cond_rhs(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); +// +void d_part_cond_qcqp_expand_sol(struct d_ocp_qcqp *ocp_qp, struct d_ocp_qcqp *part_dense_qp, struct d_ocp_qcqp_sol *part_dense_qp_sol, struct d_ocp_qcqp_sol *ocp_qp_sol, struct d_part_cond_qcqp_arg *cond_arg, struct d_part_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h b/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h new file mode 100644 index 0000000..320b66b --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_sim_erk.h @@ -0,0 +1,122 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_SIM_ERK_H_ +#define HPIPM_D_SIM_ERK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_sim_erk_arg + { + struct d_sim_rk_data *rk_data; // integrator data + double h; // step size + int steps; // number of steps +// int for_sens; // compute adjoint sensitivities +// int adj_sens; // compute adjoint sensitivities + hpipm_size_t memsize; + }; + + + +struct d_sim_erk_ws + { + void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to ode + void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot); // function pointer to forward vde + void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out); // function pointer to adjoint vde + void *ode_args; // pointer to ode args + struct d_sim_erk_arg *erk_arg; // erk arg + double *K; // internal variables + double *x_for; // states and forward sensitivities + double *x_traj; // states at all steps + double *l; // adjoint sensitivities + double *p; // parameter + double *x_tmp; // temporary states and forward sensitivities + double *adj_in; + double *adj_tmp; + int nx; // number of states + int np; // number of parameters + int nf; // number of forward sensitivities + int na; // number of adjoint sensitivities + int nf_max; // max number of forward sensitivities + int na_max; // max number of adjoint sensitivities + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_sim_erk_arg_memsize(); +// +void d_sim_erk_arg_create(struct d_sim_erk_arg *erk_arg, void *mem); +// +void d_sim_erk_arg_set_all(struct d_sim_rk_data *rk_data, double h, int steps, struct d_sim_erk_arg *erk_arg); + +// +hpipm_size_t d_sim_erk_ws_memsize(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max); +// +void d_sim_erk_ws_create(struct d_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct d_sim_erk_ws *work, void *memory); +// +void d_sim_erk_ws_set_all(int nf, int na, double *x, double *fs, double *bs, double *p, void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_for)(int t, double *x, double *p, void *ode_args, double *xdot), void (*vde_adj)(int t, double *adj_in, void *ode_args, double *adj_out), void *ode_args, struct d_sim_erk_ws *work); +// number of directions for forward sensitivities +void d_sim_erk_ws_set_nf(int *nf, struct d_sim_erk_ws *work); +// parameters (e.g. inputs) +void d_sim_erk_ws_set_p(double *p, struct d_sim_erk_ws *work); +// state +void d_sim_erk_ws_set_x(double *x, struct d_sim_erk_ws *work); +// forward sensitivities +void d_sim_erk_ws_set_fs(double *fs, struct d_sim_erk_ws *work); +// ode funtion +void d_sim_erk_ws_set_ode(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work); +// forward vde function +void d_sim_erk_ws_set_vde_for(void (*ode)(int t, double *x, double *p, void *ode_args, double *xdot), struct d_sim_erk_ws *work); +// ode_args, passed straight to the ode/vde_for/vde_adj functions +void d_sim_erk_ws_set_ode_args(void *ode_args, struct d_sim_erk_ws *work); +// state +void d_sim_erk_ws_get_x(struct d_sim_erk_ws *work, double *x); +// forward sensitivities +void d_sim_erk_ws_get_fs(struct d_sim_erk_ws *work, double *fs); +// +void d_sim_erk_solve(struct d_sim_erk_arg *arg, struct d_sim_erk_ws *work); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_ERK_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h b/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h new file mode 100644 index 0000000..a50dcbb --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_sim_rk.h @@ -0,0 +1,71 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_SIM_RK_H_ +#define HPIPM_D_SIM_RK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_sim_rk_data + { + double *A_rk; // A in butcher tableau + double *B_rk; // b in butcher tableau + double *C_rk; // c in butcher tableau + int expl; // erk vs irk + int ns; // number of stages + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_sim_rk_data_memsize(int ns); +// +void d_sim_rk_data_create(int ns, struct d_sim_rk_data *rk_data, void *memory); +// +void d_sim_rk_data_init_default(char *field, struct d_sim_rk_data *rk_data); +// +void d_sim_rk_data_set_all(int expl, double *A_rk, double *B_rk, double *C_rk, struct d_sim_rk_data *rk_data); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_RK_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h new file mode 100644 index 0000000..f36b51a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp.h @@ -0,0 +1,213 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_H_ +#define HPIPM_D_TREE_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_common.h" +#include "hpipm_d_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dmat *BAbt; // Nn-1 + struct blasfeo_dmat *RSQrq; // Nn + struct blasfeo_dmat *DCt; // Nn + struct blasfeo_dmat **Hq; // Nn + struct blasfeo_dvec *b; // Nn-1 + struct blasfeo_dvec *rqz; // Nn + struct blasfeo_dvec *d; // Nn + struct blasfeo_dvec *d_mask; // Nn + struct blasfeo_dvec *m; // Nn + struct blasfeo_dvec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_memsize(struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp *qp, void *memory); +// +void d_tree_ocp_qcqp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_A(int edge, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_B(int edge, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_b(int edge, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Q(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_S(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_R(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_q(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_r(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lb(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ub(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxb(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_C(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_D(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lg(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ug(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Qq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Sq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Rq(int node, double *mat, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_qq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_rq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_uq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_uq_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Zl(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Zu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_zl(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_zu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lls(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lus(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxs(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsg(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_set_Jsq(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxe(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_idxge(int node, int *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jbue(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_Jge(int node, double *vec, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qcqp *qp); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h new file mode 100644 index 0000000..257b82b --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_dim.h @@ -0,0 +1,117 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_DIM_H_ +#define HPIPM_D_TREE_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_dim + { + struct d_tree_ocp_qp_dim *qp_dim; // dim of qp approximation + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_dim_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_dim_memsize(int Nn); +// +void d_tree_ocp_qcqp_dim_create(int Nn, struct d_tree_ocp_qcqp_dim *qp_dim, void *memory); +// +void d_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); +// +//void d_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct d_tree_ocp_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_DIM_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h new file mode 100644 index 0000000..4a0fed1 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_ipm.h @@ -0,0 +1,191 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_IPM_H_ +#define HPIPM_D_TREE_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_ipm_arg + { + struct d_tree_ocp_qp_ipm_arg *qp_arg; + double mu0; // initial value for complementarity slackness + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol +// int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constrains (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) +// int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qcqp_ipm_ws + { + struct d_tree_ocp_qp_ipm_ws *qp_ws; + struct d_tree_ocp_qp *qp; + struct d_tree_ocp_qp_sol *qp_sol; + struct d_tree_ocp_qcqp_res_ws *qcqp_res_ws; + struct d_tree_ocp_qcqp_res *qcqp_res; + struct blasfeo_dvec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_ipm_arg_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_ipm_arg_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, void *mem); +// +void d_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void d_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void d_tree_ocp_qcqp_ipm_arg_set_alpha_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void d_tree_ocp_qcqp_ipm_arg_set_mu0(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void d_tree_ocp_qcqp_ipm_arg_set_tol_stat(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void d_tree_ocp_qcqp_ipm_arg_set_tol_eq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void d_tree_ocp_qcqp_ipm_arg_set_tol_ineq(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void d_tree_ocp_qcqp_ipm_arg_set_tol_comp(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void d_tree_ocp_qcqp_ipm_arg_set_reg_prim(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void d_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void d_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void d_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +//void d_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void d_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +//void d_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void d_tree_ocp_qcqp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void d_tree_ocp_qcqp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void d_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void d_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t d_tree_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t d_tree_ocp_qcqp_ipm_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_ipm_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws, void *mem); +// +void d_tree_ocp_qcqp_ipm_get(char *field, struct d_tree_ocp_qcqp_ipm_ws *ws, void *value); +// +void d_tree_ocp_qcqp_ipm_get_status(struct d_tree_ocp_qcqp_ipm_ws *ws, int *status); +// +void d_tree_ocp_qcqp_ipm_get_iter(struct d_tree_ocp_qcqp_ipm_ws *ws, int *iter); +// +void d_tree_ocp_qcqp_ipm_get_max_res_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_stat); +// +void d_tree_ocp_qcqp_ipm_get_max_res_eq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_eq); +// +void d_tree_ocp_qcqp_ipm_get_max_res_ineq(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_ineq); +// +void d_tree_ocp_qcqp_ipm_get_max_res_comp(struct d_tree_ocp_qcqp_ipm_ws *ws, double *res_comp); +// +void d_tree_ocp_qcqp_ipm_get_stat(struct d_tree_ocp_qcqp_ipm_ws *ws, double **stat); +// +void d_tree_ocp_qcqp_ipm_get_stat_m(struct d_tree_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void d_tree_ocp_qcqp_init_var(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws); +// +void d_tree_ocp_qcqp_ipm_solve(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_ipm_arg *arg, struct d_tree_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QCQP_IPM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h new file mode 100644 index 0000000..69eebb1 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_res.h @@ -0,0 +1,108 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_RES_H_ +#define HPIPM_D_TREE_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qcqp_res + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qcqp_res_ws + { + struct blasfeo_dvec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_dvec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *q_fun; // value for evaluation of quadr constr + struct blasfeo_dvec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_res_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_res_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t d_tree_ocp_qcqp_res_ws_memsize(struct d_tree_ocp_qcqp_dim *ocp_dim); +// +void d_tree_ocp_qcqp_res_ws_create(struct d_tree_ocp_qcqp_dim *ocp_dim, struct d_tree_ocp_qcqp_res_ws *ws, void *mem); +// +void d_tree_ocp_qcqp_res_compute(struct d_tree_ocp_qcqp *qp, struct d_tree_ocp_qcqp_sol *qp_sol, struct d_tree_ocp_qcqp_res *res, struct d_tree_ocp_qcqp_res_ws *ws); +// +void d_tree_ocp_qcqp_res_compute_inf_norm(struct d_tree_ocp_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QCQP_RES_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h new file mode 100644 index 0000000..cde8de5 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_sol.h @@ -0,0 +1,99 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QCQP_SOL_H_ +#define HPIPM_D_TREE_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_tree_ocp_qcqp_sol + { + struct d_tree_ocp_qcqp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qcqp_sol_memsize(struct d_tree_ocp_qcqp_dim *dim); +// +void d_tree_ocp_qcqp_sol_create(struct d_tree_ocp_qcqp_dim *dim, struct d_tree_ocp_qcqp_sol *qp_sol, void *memory); +// +void d_tree_ocp_qcqp_sol_get(char *field, int node_edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_u(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_x(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_sl(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_su(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_pi(int edge, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_lb(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_ub(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_lg(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qcqp_sol_get_lam_ug(int node, struct d_tree_ocp_qcqp_sol *qp_sol, double *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QCQP_SOL_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h new file mode 100644 index 0000000..5ef04fc --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qcqp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_ +#define HPIPM_D_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qcqp_dim.h" +#include "hpipm_d_tree_ocp_qcqp.h" +#include "hpipm_d_tree_ocp_qcqp_sol.h" +#include "hpipm_d_tree_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_tree_ocp_qcqp_dim_print(struct d_tree_ocp_qcqp_dim *qp_dim); +// +//void d_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim); +// +void d_tree_ocp_qcqp_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp); +// +//void d_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp *qp); +// +void d_tree_ocp_qcqp_sol_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_sol *ocp_qcqp_sol); +// +void d_tree_ocp_qcqp_ipm_arg_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +//void d_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_ipm_arg *arg); +// +void d_tree_ocp_qcqp_res_print(struct d_tree_ocp_qcqp_dim *qp_dim, struct d_tree_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h new file mode 100644 index 0000000..1666ad7 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp.h @@ -0,0 +1,195 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_H_ +#define HPIPM_D_TREE_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dmat *BAbt; // Nn-1 + struct blasfeo_dmat *RSQrq; // Nn + struct blasfeo_dmat *DCt; // Nn + struct blasfeo_dvec *b; // Nn-1 + struct blasfeo_dvec *rqz; // Nn + struct blasfeo_dvec *d; // Nn + struct blasfeo_dvec *d_mask; // Nn + struct blasfeo_dvec *m; // Nn + struct blasfeo_dvec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn +// int **idxs; // index of soft constraints + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qp_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp *qp, void *memory); +// +void d_tree_ocp_qp_set_all(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **d_lb, double **d_ub, double **C, double **D, double **d_lg, double **d_ug, double **Zl, double **Zu, double **zl, double **zu, int **idxs, double **d_ls, double **d_us, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_A(int edge, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_B(int edge, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_b(int edge, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Q(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_S(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_R(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_q(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_r(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lb(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lb_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ub(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ub_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbx_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubx_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lbu_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ubu_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxb(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxbx(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxbu(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_C(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_D(int node, double *mat, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lg(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lg_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ug(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_ug_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Zl(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Zu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_zl(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_zu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lls(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lls_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lus(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_lus_mask(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxs(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsbu(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsbx(int node, double *vec, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_set_Jsg(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxe(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxbxe(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxbue(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_idxge(int node, int *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jbxe(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jbue(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_Jge(int node, double *vec, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct d_tree_ocp_qp *qp); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h new file mode 100644 index 0000000..659b664 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_dim.h @@ -0,0 +1,111 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_DIM_H_ +#define HPIPM_D_TREE_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_dim + { + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_dim_strsize(); +// +hpipm_size_t d_tree_ocp_qp_dim_memsize(int Nn); +// +void d_tree_ocp_qp_dim_create(int Nn, struct d_tree_ocp_qp_dim *qp_dim, void *memory); +// +void d_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set(char *field, int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_ng(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_ns(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nsg(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nbue(int stage, int value, struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_dim_set_nge(int stage, int value, struct d_tree_ocp_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h new file mode 100644 index 0000000..fb63420 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_ipm.h @@ -0,0 +1,209 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_D_TREE_OCP_QP_IPM_H_ +#define HPIPM_D_TREE_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_ipm_arg + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + double tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qp_ipm_ws + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_tree_ocp_qp_res_ws *res_workspace; + struct d_tree_ocp_qp_sol *sol_step; + struct d_tree_ocp_qp_sol *sol_itref; + struct d_tree_ocp_qp *qp_step; + struct d_tree_ocp_qp *qp_itref; + struct d_tree_ocp_qp_res *res_itref; + struct d_tree_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbgM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int lq_fact; // cache from arg + int mask_constr; // use constr mask + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_ipm_arg_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_ipm_arg_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, void *mem); +// +void d_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_alpha_min(double *alpha_min, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_mu0(double *mu0, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_stat(double *tol_stat, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_eq(double *tol_eq, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_ineq(double *tol_ineq, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tol_comp(double *tol_comp, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_reg_prim(double *reg, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_lam_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_t_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_tau_min(double *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct d_tree_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t d_tree_ocp_qp_ipm_ws_memsize(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_ipm_ws_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws, void *mem); +// +void d_tree_ocp_qp_ipm_get_status(struct d_tree_ocp_qp_ipm_ws *ws, int *status); +// +void d_tree_ocp_qp_ipm_get_iter(struct d_tree_ocp_qp_ipm_ws *ws, int *iter); +// +void d_tree_ocp_qp_ipm_get_max_res_stat(struct d_tree_ocp_qp_ipm_ws *ws, double *res_stat); +// +void d_tree_ocp_qp_ipm_get_max_res_eq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_eq); +// +void d_tree_ocp_qp_ipm_get_max_res_ineq(struct d_tree_ocp_qp_ipm_ws *ws, double *res_ineq); +// +void d_tree_ocp_qp_ipm_get_max_res_comp(struct d_tree_ocp_qp_ipm_ws *ws, double *res_comp); +// +void d_tree_ocp_qp_ipm_get_stat(struct d_tree_ocp_qp_ipm_ws *ws, double **stat); +// +void d_tree_ocp_qp_ipm_get_stat_m(struct d_tree_ocp_qp_ipm_ws *ws, int *stat_m); +// +void d_tree_ocp_qp_init_var(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_abs_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_delta_step(int kk, struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_ipm_solve(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h new file mode 100644 index 0000000..4afd52f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_kkt.h @@ -0,0 +1,52 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + + +// +void d_tree_ocp_qp_fact_solve_kkt_unconstr(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_fact_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_fact_lq_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); +// +void d_tree_ocp_qp_solve_kkt_step(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_ipm_arg *arg, struct d_tree_ocp_qp_ipm_ws *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h new file mode 100644 index 0000000..fe49908 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_res.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_RES_H_ +#define HPIPM_D_TREE_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct d_tree_ocp_qp_res + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + double res_max[4]; // max of residuals + double res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct d_tree_ocp_qp_res_ws + { + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t d_tree_ocp_qp_res_memsize(struct d_tree_ocp_qp_dim *ocp_dim); +// +void d_tree_ocp_qp_res_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res *res, void *mem); +// +hpipm_size_t d_tree_ocp_qp_res_ws_memsize(struct d_tree_ocp_qp_dim *ocp_dim); +// +void d_tree_ocp_qp_res_ws_create(struct d_tree_ocp_qp_dim *ocp_dim, struct d_tree_ocp_qp_res_ws *ws, void *mem); +// +void d_tree_ocp_qp_res_get_all(struct d_tree_ocp_qp_res *res, double **res_r, double **res_q, double **res_ls, double **res_us, double **res_b, double **res_d_lb, double **res_d_ub, double **res_d_lg, double **res_d_ug, double **res_d_ls, double **res_d_us, double **res_m_lb, double **res_m_ub, double **res_m_lg, double **res_m_ug, double **res_m_ls, double **res_m_us); +// +void d_tree_ocp_qp_res_compute(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws); +// +void d_tree_ocp_qp_res_compute_lin(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, struct d_tree_ocp_qp_sol *qp_step, struct d_tree_ocp_qp_res *res, struct d_tree_ocp_qp_res_ws *ws); +// +void d_tree_ocp_qp_res_compute_inf_norm(struct d_tree_ocp_qp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_TREE_OCP_QP_RES_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h new file mode 100644 index 0000000..343d5e8 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_sol.h @@ -0,0 +1,100 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_SOL_H_ +#define HPIPM_D_TREE_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct d_tree_ocp_qp_sol + { + struct d_tree_ocp_qp_dim *dim; + struct blasfeo_dvec *ux; + struct blasfeo_dvec *pi; + struct blasfeo_dvec *lam; + struct blasfeo_dvec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t d_tree_ocp_qp_sol_memsize(struct d_tree_ocp_qp_dim *dim); +// +void d_tree_ocp_qp_sol_create(struct d_tree_ocp_qp_dim *dim, struct d_tree_ocp_qp_sol *qp_sol, void *memory); +// +void d_tree_ocp_qp_sol_get_all(struct d_tree_ocp_qp *qp, struct d_tree_ocp_qp_sol *qp_sol, double **u, double **x, double **ls, double **us, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug, double **lam_ls, double **lam_us); +// +void d_tree_ocp_qp_sol_get(char *field, int node_edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_u(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_x(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_sl(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_su(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_pi(int edge, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_lb(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_ub(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_lg(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); +// +void d_tree_ocp_qp_sol_get_lam_ug(int node, struct d_tree_ocp_qp_sol *qp_sol, double *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h new file mode 100644 index 0000000..b689fdc --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_d_tree_ocp_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_D_TREE_OCP_QP_UTILS_H_ +#define HPIPM_D_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_d_tree_ocp_qp_dim.h" +#include "hpipm_d_tree_ocp_qp.h" +#include "hpipm_d_tree_ocp_qp_sol.h" +#include "hpipm_d_tree_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void d_tree_ocp_qp_dim_print(struct d_tree_ocp_qp_dim *qp_dim); +// +//void d_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim); +// +void d_tree_ocp_qp_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp); +// +//void d_tree_ocp_qp_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp *qp); +// +void d_tree_ocp_qp_sol_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_sol *ocp_qp_sol); +// +void d_tree_ocp_qp_ipm_arg_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +//void d_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_ipm_arg *arg); +// +void d_tree_ocp_qp_res_print(struct d_tree_ocp_qp_dim *qp_dim, struct d_tree_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_TREE_OCP_QP_UTILS_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h new file mode 100644 index 0000000..8bc1010 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + + +#ifndef HPIPM_M_DENSE_QP_H_ +#define HPIPM_M_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp.h" +#include "hpipm_s_dense_qp.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +void cvt_d2s_dense_qp(struct d_dense_qp *qpd, struct s_dense_qp *qps); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_M_DENSE_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h new file mode 100644 index 0000000..4610321 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_m_dense_qp_dim.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + + +#ifndef HPIPM_M_DENSE_QP_DIM_H_ +#define HPIPM_M_DENSE_QP_DIM_H_ + + + +#include +#include + +#include "hpipm_d_dense_qp_dim.h" +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +void cvt_d2s_dense_qp_dim(struct d_dense_qp_dim *qpd, struct s_dense_qp_dim *qps); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_M_DENSE_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h new file mode 100644 index 0000000..95c3dad --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp.h @@ -0,0 +1,49 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h new file mode 100644 index 0000000..1c44ace --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_ipm_hard.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + + +struct m_ipm_hard_ocp_qp_workspace + { + struct d_ipm_hard_core_qp_workspace *core_workspace; + struct blasfeo_dvec *dux; + struct blasfeo_dvec *dpi; + struct blasfeo_dvec *dt_lb; + struct blasfeo_dvec *dt_lg; + struct blasfeo_svec *sdux; // XXX + struct blasfeo_svec *sdpi; // XXX + struct blasfeo_dvec *res_g; // q-residuals + struct blasfeo_dvec *res_b; // b-residuals + struct blasfeo_dvec *res_d; // d-residuals XXX remove ??? + struct blasfeo_dvec *res_d_lb; // d-residuals + struct blasfeo_dvec *res_d_ub; // d-residuals + struct blasfeo_dvec *res_d_lg; // d-residuals + struct blasfeo_dvec *res_d_ug; // d-residuals + struct blasfeo_dvec *res_m; // m-residuals + struct blasfeo_dvec *res_m_lb; // m-residuals + struct blasfeo_dvec *res_m_ub; // m-residuals + struct blasfeo_dvec *res_m_lg; // m-residuals + struct blasfeo_dvec *res_m_ug; // m-residuals + struct blasfeo_svec *sres_g; // q-residuals // XXX + struct blasfeo_svec *sres_b; // b-residuals // XXX + struct blasfeo_dvec *Qx_lb; // hessian update + struct blasfeo_dvec *Qx_lg; // hessian update + struct blasfeo_dvec *qx_lb; // gradient update + struct blasfeo_dvec *qx_lg; // gradient update + struct blasfeo_svec *sQx_lb; // hessian update // XXX + struct blasfeo_svec *sQx_lg; // hessian update // XXX + struct blasfeo_svec *sqx_lb; // gradient update // XXX + struct blasfeo_svec *sqx_lg; // gradient update // XXX + struct blasfeo_dvec *tmp_nbM; // work space of size nbM + struct blasfeo_svec *tmp_nxM; // work space of size nxM // XXX + struct blasfeo_dvec *tmp_ngM; // work space of size ngM + struct blasfeo_svec *Pb; // Pb // XXX + struct blasfeo_smat *L; // XXX + struct blasfeo_smat *AL; // XXX + struct blasfeo_svec *sSx; // scaling + struct blasfeo_svec *sSi; // scaling inverted + double *stat; // convergence statistics + double res_mu; // mu-residual + int iter; // iteration number + int compute_Pb; + int scale; + }; + + + +struct m_ipm_hard_ocp_qp_arg + { + double alpha_min; // exit cond on step length + double mu_max; // exit cond on duality measure + double mu0; // initial value for duality measure + int iter_max; // exit cond in iter number + }; + + + +// +hpipm_size_t m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg); +// +void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *ws, void *mem); +// +void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws); +// +void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h new file mode 100644 index 0000000..032fe95 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_m_ocp_qp_kkt.h @@ -0,0 +1,45 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifdef __cplusplus +extern "C" { +#endif + +void m_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws); +void m_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h new file mode 100644 index 0000000..ba01ecb --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_cast_qcqp.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_CAST_QCQP_H_ +#define HPIPM_S_CAST_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp.h" +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_cast_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim); +// +void s_cast_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_CAST_QCQP_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond.h b/third_party/acados/include/hpipm/include/hpipm_s_cond.h new file mode 100644 index 0000000..3011679 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_cond.h @@ -0,0 +1,137 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_H_ +#define HPIPM_S_COND_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp.h" +#include "hpipm_s_dense_qp_sol.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qp_dim.h" +#include "hpipm_s_ocp_qp_sol.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_cond_qp_arg + { + int cond_last_stage; // condense last stage + int cond_alg; // condensing algorithm: 0 N2-nx3, 1 N3-nx2 + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct s_cond_qp_ws + { + struct blasfeo_smat *Gamma; + struct blasfeo_smat *GammaQ; + struct blasfeo_smat *L; + struct blasfeo_smat *Lx; + struct blasfeo_smat *AL; + struct blasfeo_svec *Gammab; + struct blasfeo_svec *l; + struct blasfeo_svec *tmp_nbgM; + struct blasfeo_svec *tmp_nuxM; + int bs; // block size + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_cond_qp_arg_memsize(); +// +void s_cond_qp_arg_create(struct s_cond_qp_arg *cond_arg, void *mem); +// +void s_cond_qp_arg_set_default(struct s_cond_qp_arg *cond_arg); +// condensing algorithm: 0 N2-nx3, 1 N3-nx2 +void s_cond_qp_arg_set_cond_alg(int cond_alg, struct s_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void s_cond_qp_arg_set_ric_alg(int ric_alg, struct s_cond_qp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void s_cond_qp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_prim_sol(int value, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_cond_qp_arg *cond_arg); + +// +void s_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, struct s_dense_qp_dim *dense_dim); +// +hpipm_size_t s_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg); +// +void s_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws, void *mem); +// +void s_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// TODO remove +void s_cond_qp_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + +// +void s_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_dense_qp *dense_qp, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h b/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h new file mode 100644 index 0000000..003472a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_cond_aux.h @@ -0,0 +1,92 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_AUX_H_ +#define HPIPM_S_COND_AUX_H_ + + + +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_cond_BAbt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_BAt(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_b(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_RSQrq(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_RSQ(struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_rq(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_DCtd(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, int *idxs_rev2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_DCt(struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, int *idxs_rev2, struct blasfeo_svec *Z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_cond_d(struct s_ocp_qp *ocp_qp, struct blasfeo_svec *d2, struct blasfeo_svec *d_mask2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_expand_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_expand_primal_sol(struct s_ocp_qp *ocp_qp, struct s_dense_qp_sol *dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + +// +void s_update_cond_BAbt(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *BAbt2, struct blasfeo_svec *b2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_update_cond_RSQrq_N2nx3(int *idxc, struct s_ocp_qp *ocp_qp, struct blasfeo_smat *RSQrq2, struct blasfeo_svec *rq2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); +// +void s_update_cond_DCtd(int *idxc, struct s_ocp_qp *ocp_qp, int *idxb2, struct blasfeo_smat *DCt2, struct blasfeo_svec *d2, int *idxs2, struct blasfeo_svec *Z2, struct blasfeo_svec *z2, struct s_cond_qp_arg *cond_arg, struct s_cond_qp_ws *cond_ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_AUX_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h new file mode 100644 index 0000000..c36678a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_cond_qcqp.h @@ -0,0 +1,130 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_COND_QCQP_H_ +#define HPIPM_S_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp.h" +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qcqp_sol.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_cond_qcqp_arg + { + struct s_cond_qp_arg *qp_arg; + int cond_last_stage; // condense last stage +// int cond_variant; // TODO + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution equality constr (lam t) + int square_root_alg; // square root algorithm (faster but requires RSQ>0) + hpipm_size_t memsize; + }; + + + +struct s_cond_qcqp_ws + { + struct s_cond_qp_ws *qp_ws; + struct blasfeo_smat *hess_array; // TODO remove + struct blasfeo_smat *zero_hess; // TODO remove + struct blasfeo_svec *zero_grad; // TODO remove + struct blasfeo_svec *grad_array; // TODO remove + struct blasfeo_svec *tmp_nvc; + struct blasfeo_svec *tmp_nuxM; + struct blasfeo_smat *GammaQ; + struct blasfeo_smat *tmp_DCt; + struct blasfeo_smat *tmp_nuM_nxM; +// struct blasfeo_svec *d_qp; +// struct blasfeo_svec *d_mask_qp; + hpipm_size_t memsize; + }; + + +// +hpipm_size_t s_cond_qcqp_arg_memsize(); +// +void s_cond_qcqp_arg_create(struct s_cond_qcqp_arg *cond_arg, void *mem); +// +void s_cond_qcqp_arg_set_default(struct s_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 square-root +void s_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_cond_qcqp_arg *cond_arg); +// condense last stage: 0 last stage disregarded, 1 last stage condensed too +void s_cond_qcqp_arg_set_cond_last_stage(int cond_last_stage, struct s_cond_qcqp_arg *cond_arg); + +// +void s_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, struct s_dense_qcqp_dim *dense_dim); +// +hpipm_size_t s_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg); +// +void s_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws, void *mem); +// +void s_cond_qcqp_qc(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_qc_lhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_smat *Hq2, int *Hq_nzero2, struct blasfeo_smat *Ct2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_qc_rhs(struct s_ocp_qcqp *ocp_qp, struct blasfeo_svec *d2, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp *dense_qp, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); +// +void s_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_dense_qcqp_sol *dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_cond_qcqp_arg *cond_arg, struct s_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_COND_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h new file mode 100644 index 0000000..480392c --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm.h @@ -0,0 +1,101 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_ +#define HPIPM_S_CORE_QP_IPM_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_core_qp_ipm_workspace + { + float *v; // primal variables + float *pi; // equality constraints multipliers + float *lam; // inequality constraints multipliers + float *t; // inequality constraints slacks + float *t_inv; // inverse of t + float *v_bkp; // backup of primal variables + float *pi_bkp; // backup of equality constraints multipliers + float *lam_bkp; // backup of inequality constraints multipliers + float *t_bkp; // backup of inequality constraints slacks + float *dv; // step in v + float *dpi; // step in pi + float *dlam; // step in lam + float *dt; // step in t + float *res_g; // q-residuals + float *res_b; // b-residuals + float *res_d; // d-residuals + float *res_m; // m-residuals + float *res_m_bkp; // m-residuals + float *Gamma; // Hessian update + float *gamma; // gradient update + float alpha_prim; // step length + float alpha_dual; // step length + float alpha; // step length + float sigma; // centering XXX + float mu; // duality measuere + float mu_aff; // affine duality measuere + float nc_inv; // 1.0/nc, where nc is the total number of constraints + float nc_mask_inv; // 1.0/nc_mask + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float t_min_inv; // inverse of min value in t vector + float tau_min; // min value of barrier parameter + int nv; // number of primal variables + int ne; // number of equality constraints + int nc; // (twice the) number of (two-sided) inequality constraints + int nc_mask; // total number of ineq constr after masking + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam also in solution, or only in Gamma computation + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_memsize_core_qp_ipm(int nv, int ne, int nc); +// +void s_create_core_qp_ipm(int nv, int ne, int nc, struct s_core_qp_ipm_workspace *workspace, void *mem); +// +void s_core_qp_ipm(struct s_core_qp_ipm_workspace *workspace); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h new file mode 100644 index 0000000..1ac3d7e --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_core_qp_ipm_aux.h @@ -0,0 +1,68 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_CORE_QP_IPM_AUX_ +#define HPIPM_S_CORE_QP_IPM_AUX_ + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_compute_Gamma_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_gamma_qp(float *res_d, float *res_m, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_lam_t_qp(float *res_d, float *res_m, float *dlam, float *dt, struct s_core_qp_ipm_workspace *rws); +// +void s_compute_alpha_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_update_var_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_mu_aff_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_backup_res_m(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_centering_correction_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_centering_qp(struct s_core_qp_ipm_workspace *rws); +// +void s_compute_tau_min_qp(struct s_core_qp_ipm_workspace *rws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_CORE_QP_IPM_AUX_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h new file mode 100644 index 0000000..d03c065 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp.h @@ -0,0 +1,200 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_H_ +#define HPIPM_S_DENSE_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_smat *Hv; // hessian of cost & vector work space + struct blasfeo_smat *A; // equality constraint matrix + struct blasfeo_smat *Ct; // inequality constraints matrix + struct blasfeo_smat *Hq; // hessians of quadratic constraints + struct blasfeo_svec *gz; // gradient of cost & gradient of slacks + struct blasfeo_svec *b; // equality constraint vector + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + int *Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_dense_qcqp_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp *qp, void *memory); + +// +void s_dense_qcqp_set(char *field, void *value, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_H(float *H, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_g(float *g, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_A(float *A, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_b(float *b, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxb(int *idxb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lb(float *lb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lb_mask(float *lb, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ub(float *ub, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ub_mask(float *ub, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_C(float *C, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lg(float *lg, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_lg_mask(float *lg, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ug(float *ug, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ug_mask(float *ug, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Hq(float *Hq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_gq(float *gq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_uq(float *uq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_uq_mask(float *uq, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxs(int *idxs, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_idxs_rev(int *idxs_rev, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Zl(float *Zl, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_Zu(float *Zu, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_zl(float *zl, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_zu(float *zu, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ls(float *ls, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_ls_mask(float *ls, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_us(float *us, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_set_us_mask(float *us, struct s_dense_qcqp *qp); + +// getters (COLMAJ) + +void s_dense_qcqp_get_H(struct s_dense_qcqp *qp, float *H); +// +void s_dense_qcqp_get_g(struct s_dense_qcqp *qp, float *g); +// +void s_dense_qcqp_get_A(struct s_dense_qcqp *qp, float *A); +// +void s_dense_qcqp_get_b(struct s_dense_qcqp *qp, float *b); +// +void s_dense_qcqp_get_idxb(struct s_dense_qcqp *qp, int *idxb); +// +void s_dense_qcqp_get_lb(struct s_dense_qcqp *qp, float *lb); +// +void s_dense_qcqp_get_lb_mask(struct s_dense_qcqp *qp, float *lb); +// +void s_dense_qcqp_get_ub(struct s_dense_qcqp *qp, float *ub); +// +void s_dense_qcqp_get_ub_mask(struct s_dense_qcqp *qp, float *ub); +// +void s_dense_qcqp_get_C(struct s_dense_qcqp *qp, float *C); +// +void s_dense_qcqp_get_lg(struct s_dense_qcqp *qp, float *lg); +// +void s_dense_qcqp_get_lg_mask(struct s_dense_qcqp *qp, float *lg); +// +void s_dense_qcqp_get_ug(struct s_dense_qcqp *qp, float *ug); +// +void s_dense_qcqp_get_ug_mask(struct s_dense_qcqp *qp, float *ug); +// +void s_dense_qcqp_get_idxs(struct s_dense_qcqp *qp, int *idxs); +// +void s_dense_qcqp_get_idxs_rev(struct s_dense_qcqp *qp, int *idxs_rev); +// +void s_dense_qcqp_get_Zl(struct s_dense_qcqp *qp, float *Zl); +// +void s_dense_qcqp_get_Zu(struct s_dense_qcqp *qp, float *Zu); +// +void s_dense_qcqp_get_zl(struct s_dense_qcqp *qp, float *zl); +// +void s_dense_qcqp_get_zu(struct s_dense_qcqp *qp, float *zu); +// +void s_dense_qcqp_get_ls(struct s_dense_qcqp *qp, float *ls); +// +void s_dense_qcqp_get_ls_mask(struct s_dense_qcqp *qp, float *ls); +// +void s_dense_qcqp_get_us(struct s_dense_qcqp *qp, float *us); +// +void s_dense_qcqp_get_us_mask(struct s_dense_qcqp *qp, float *us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h new file mode 100644 index 0000000..04908c2 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_dim.h @@ -0,0 +1,99 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_DIM_H_ +#define HPIPM_S_DENSE_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_dim + { + struct s_dense_qp_dim *qp_dim; // dim of qp approximation + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nq; // number of quadratic constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int nsq; // number of softened quadratic constraints + int ns; // number of softened constraints (nsb+nsg+nsq) TODO number of slacks + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_dim_memsize(); +// +void s_dense_qcqp_dim_create(struct s_dense_qcqp_dim *dim, void *memory); +// +void s_dense_qcqp_dim_set(char *fiels_name, int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nv(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ne(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nb(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ng(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nq(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsb(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsg(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_nsq(int value, struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_dim_set_ns(int value, struct s_dense_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_DIM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h new file mode 100644 index 0000000..8f85768 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_ipm.h @@ -0,0 +1,204 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_IPM_H_ +#define HPIPM_S_DENSE_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_ipm_arg + { + struct s_dense_qp_ipm_arg *qp_arg; + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_dense_qcqp_ipm_ws + { +// float qp_res[4]; // infinity norm of residuals + struct s_dense_qp_ipm_ws *qp_ws; + struct s_dense_qp *qp; + struct s_dense_qp_sol *qp_sol; + struct s_dense_qcqp_res_ws *qcqp_res_ws; + struct s_dense_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nv; +// float *stat; // convergence statistics +// void *lq_work0; +// void *lq_work1; + int iter; // iteration number +// int stat_max; // iterations saved in stat +// int stat_m; // numer of recorded stat per ipm iter +// int scale; +// int use_hess_fact; + int status; + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_dense_qcqp_ipm_arg_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_ipm_arg_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_ipm_arg *arg, void *mem); +// +void s_dense_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set(char *field, void *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_mu0(float *mu0, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_lam_min(float *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_t_min(float *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_split_step(int *value, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_dense_qcqp_ipm_ws_memsize(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_ipm_ws_create(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws, void *mem); +// +void s_dense_qcqp_ipm_get(char *field, struct s_dense_qcqp_ipm_ws *ws, void *value); +// +void s_dense_qcqp_ipm_get_status(struct s_dense_qcqp_ipm_ws *ws, int *status); +// +void s_dense_qcqp_ipm_get_iter(struct s_dense_qcqp_ipm_ws *ws, int *iter); +// +void s_dense_qcqp_ipm_get_max_res_stat(struct s_dense_qcqp_ipm_ws *ws, float *res_stat); +// +void s_dense_qcqp_ipm_get_max_res_eq(struct s_dense_qcqp_ipm_ws *ws, float *res_eq); +// +void s_dense_qcqp_ipm_get_max_res_ineq(struct s_dense_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_dense_qcqp_ipm_get_max_res_comp(struct s_dense_qcqp_ipm_ws *ws, float *res_comp); +// +void s_dense_qcqp_ipm_get_stat(struct s_dense_qcqp_ipm_ws *ws, float **stat); +// +void s_dense_qcqp_ipm_get_stat_m(struct s_dense_qcqp_ipm_ws *ws, int *stat_m); +#if 0 +// +void s_dense_qcqp_init_var(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#endif +// +void s_dense_qcqp_ipm_solve(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#if 0 +// +void s_dense_qcqp_ipm_predict(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +// +void s_dense_qcqp_ipm_sens(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_ipm_arg *arg, struct s_dense_qcqp_ipm_ws *ws); +#endif + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_IPM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h new file mode 100644 index 0000000..779658c --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_res.h @@ -0,0 +1,108 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_RES_H_ +#define HPIPM_S_DENSE_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_res + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // infinity norm of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_dense_qcqp_res_ws + { + struct blasfeo_svec *tmp_nv; // work space of size nv + struct blasfeo_svec *tmp_nbgq; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_ns; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_res_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_res_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res *res, void *mem); +// +hpipm_size_t s_dense_qcqp_res_ws_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_res_ws_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_res_ws *workspace, void *mem); +// +void s_dense_qcqp_res_compute(struct s_dense_qcqp *qp, struct s_dense_qcqp_sol *qp_sol, struct s_dense_qcqp_res *res, struct s_dense_qcqp_res_ws *ws); +// +void s_dense_qcqp_res_compute_inf_norm(struct s_dense_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_DENSE_QCQP_RES_H_ + + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h new file mode 100644 index 0000000..197a690 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_sol.h @@ -0,0 +1,86 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QCQP_SOL_H_ +#define HPIPM_S_DENSE_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qcqp_sol + { + struct s_dense_qcqp_dim *dim; + struct blasfeo_svec *v; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qcqp_sol_memsize(struct s_dense_qcqp_dim *dim); +// +void s_dense_qcqp_sol_create(struct s_dense_qcqp_dim *dim, struct s_dense_qcqp_sol *qp_sol, void *memory); +// +void s_dense_qcqp_sol_get_v(struct s_dense_qcqp_sol *qp_sol, float *v); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_SOL_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h new file mode 100644 index 0000000..4f5aae2 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qcqp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QCQP_UTILS_H_ +#define HPIPM_S_DENSE_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_dense_qcqp_dim.h" +#include "hpipm_s_dense_qcqp.h" +#include "hpipm_s_dense_qcqp_sol.h" +//#include "hpipm_s_dense_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_dense_qcqp_dim_print(struct s_dense_qcqp_dim *qp_dim); +// +//void s_dense_qcqp_dim_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim); +// +void s_dense_qcqp_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp); +// +//void s_dense_qcqp_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp *qp); +// +void s_dense_qcqp_sol_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_sol *dense_qcqp_sol); +// +//void s_dense_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_ipm_arg *arg); +// +void s_dense_qcqp_res_print(struct s_dense_qcqp_dim *qp_dim, struct s_dense_qcqp_res *dense_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QCQP_UTILS_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h new file mode 100644 index 0000000..3c2517f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp.h @@ -0,0 +1,207 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_H_ +#define HPIPM_S_DENSE_QP_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp + { + struct s_dense_qp_dim *dim; + struct blasfeo_smat *Hv; // hessian & gradient + struct blasfeo_smat *A; // dynamics matrix + struct blasfeo_smat *Ct; // constraints matrix + struct blasfeo_svec *gz; // gradient & gradient of slacks + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *d; // constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int *idxb; // indices of box constrained variables within [u; x] + int *idxs_rev; // index of soft constraints (reverse storage) + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_dense_qp_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_create(struct s_dense_qp_dim *dim, struct s_dense_qp *qp, void *memory); + +// setters - colmaj +// +void s_dense_qp_set_all(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp); +// +void s_dense_qp_get_all(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us); +// +void s_dense_qp_set(char *field, void *value, struct s_dense_qp *qp); +// +void s_dense_qp_set_H(float *H, struct s_dense_qp *qp); +// +void s_dense_qp_set_g(float *g, struct s_dense_qp *qp); +// +void s_dense_qp_set_A(float *A, struct s_dense_qp *qp); +// +void s_dense_qp_set_b(float *b, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxb(int *idxb, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jb(float *Jb, struct s_dense_qp *qp); +// +void s_dense_qp_set_lb(float *lb, struct s_dense_qp *qp); +// +void s_dense_qp_set_lb_mask(float *lb, struct s_dense_qp *qp); +// +void s_dense_qp_set_ub(float *ub, struct s_dense_qp *qp); +// +void s_dense_qp_set_ub_mask(float *ub, struct s_dense_qp *qp); +// +void s_dense_qp_set_C(float *C, struct s_dense_qp *qp); +// +void s_dense_qp_set_lg(float *lg, struct s_dense_qp *qp); +// +void s_dense_qp_set_lg_mask(float *lg, struct s_dense_qp *qp); +// +void s_dense_qp_set_ug(float *ug, struct s_dense_qp *qp); +// +void s_dense_qp_set_ug_mask(float *ug, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxs(int *idxs, struct s_dense_qp *qp); +// +void s_dense_qp_set_idxs_rev(int *idxs_rev, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jsb(float *Jsb, struct s_dense_qp *qp); +// +void s_dense_qp_set_Jsg(float *Jsg, struct s_dense_qp *qp); +// +void s_dense_qp_set_Zl(float *Zl, struct s_dense_qp *qp); +// +void s_dense_qp_set_Zu(float *Zu, struct s_dense_qp *qp); +// +void s_dense_qp_set_zl(float *zl, struct s_dense_qp *qp); +// +void s_dense_qp_set_zu(float *zu, struct s_dense_qp *qp); +// +void s_dense_qp_set_ls(float *ls, struct s_dense_qp *qp); +// +void s_dense_qp_set_ls_mask(float *ls, struct s_dense_qp *qp); +// +void s_dense_qp_set_us(float *us, struct s_dense_qp *qp); +// +void s_dense_qp_set_us_mask(float *us, struct s_dense_qp *qp); + +// getters - colmaj +// +void s_dense_qp_get_H(struct s_dense_qp *qp, float *H); +// +void s_dense_qp_get_g(struct s_dense_qp *qp, float *g); +// +void s_dense_qp_get_A(struct s_dense_qp *qp, float *A); +// +void s_dense_qp_get_b(struct s_dense_qp *qp, float *b); +// +void s_dense_qp_get_idxb(struct s_dense_qp *qp, int *idxb); +// +void s_dense_qp_get_lb(struct s_dense_qp *qp, float *lb); +// +void s_dense_qp_get_lb_mask(struct s_dense_qp *qp, float *lb); +// +void s_dense_qp_get_ub(struct s_dense_qp *qp, float *ub); +// +void s_dense_qp_get_ub_mask(struct s_dense_qp *qp, float *ub); +// +void s_dense_qp_get_C(struct s_dense_qp *qp, float *C); +// +void s_dense_qp_get_lg(struct s_dense_qp *qp, float *lg); +// +void s_dense_qp_get_lg_mask(struct s_dense_qp *qp, float *lg); +// +void s_dense_qp_get_ug(struct s_dense_qp *qp, float *ug); +// +void s_dense_qp_get_ug_mask(struct s_dense_qp *qp, float *ug); +// +void s_dense_qp_get_idxs(struct s_dense_qp *qp, int *idxs); +// +void s_dense_qp_get_idxs_rev(struct s_dense_qp *qp, int *idxs_rev); +// +void s_dense_qp_get_Zl(struct s_dense_qp *qp, float *Zl); +// +void s_dense_qp_get_Zu(struct s_dense_qp *qp, float *Zu); +// +void s_dense_qp_get_zl(struct s_dense_qp *qp, float *zl); +// +void s_dense_qp_get_zu(struct s_dense_qp *qp, float *zu); +// +void s_dense_qp_get_ls(struct s_dense_qp *qp, float *ls); +// +void s_dense_qp_get_ls_mask(struct s_dense_qp *qp, float *ls); +// +void s_dense_qp_get_us(struct s_dense_qp *qp, float *us); +// +void s_dense_qp_get_us_mask(struct s_dense_qp *qp, float *us); + +// setters - rowmaj +// +void s_dense_qp_set_all_rowmaj(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us, struct s_dense_qp *qp); + +// getters - rowmaj +// +void s_dense_qp_get_all_rowmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, float *Zl, float *Zu, float *zl, float *zu, int *idxs, float *d_ls, float *d_us); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h new file mode 100644 index 0000000..b979d24 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_dim.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_DIM_H_ +#define HPIPM_S_DENSE_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_dim + { + int nv; // number of variables + int ne; // number of equality constraints + int nb; // number of box constraints + int ng; // number of general constraints + int nsb; // number of softened box constraints + int nsg; // number of softened general constraints + int ns; // number of softened constraints (nsb+nsg) + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_dim_memsize(); +// +void s_dense_qp_dim_create(struct s_dense_qp_dim *qp_dim, void *memory); +// +void s_dense_qp_dim_set_all(int nv, int ne, int nb, int ng, int nsb, int nsg, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set(char *fiels_name, int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nv(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ne(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nb(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ng(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nsb(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_nsg(int value, struct s_dense_qp_dim *dim); +// +void s_dense_qp_dim_set_ns(int value, struct s_dense_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QP_DIM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h new file mode 100644 index 0000000..f2d56d4 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_ipm.h @@ -0,0 +1,260 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_IPM_H_ +#define HPIPM_S_DENSE_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_ipm_arg + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int comp_res_pred; // compute residuals of prediction + int kkt_fact_alg; // 0 null-space, 1 schur-complement + int remove_lin_dep_eq; // 0 do not, 1 do check and remove linearly dependent equality constraints + int compute_obj; // compute obj on exit + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_dense_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_dense_qp_res_ws *res_ws; + struct s_dense_qp_sol *sol_step; + struct s_dense_qp_sol *sol_itref; + struct s_dense_qp *qp_step; + struct s_dense_qp *qp_itref; + struct s_dense_qp_res *res; + struct s_dense_qp_res *res_itref; + struct s_dense_qp_res *res_step; + struct blasfeo_svec *Gamma; // + struct blasfeo_svec *gamma; // + struct blasfeo_svec *Zs_inv; // + struct blasfeo_smat *Lv; // + struct blasfeo_smat *AL; // + struct blasfeo_smat *Le; // + struct blasfeo_smat *Ctx; // + struct blasfeo_svec *lv; // + struct blasfeo_svec *sv; // scale for Lv + struct blasfeo_svec *se; // scale for Le + struct blasfeo_svec *tmp_nbg; // work space of size nb+ng + struct blasfeo_svec *tmp_ns; // work space of size ns + struct blasfeo_smat *lq0; + struct blasfeo_smat *lq1; + struct blasfeo_svec *tmp_m; + struct blasfeo_smat *A_LQ; + struct blasfeo_smat *A_Q; + struct blasfeo_smat *Zt; + struct blasfeo_smat *ZtH; + struct blasfeo_smat *ZtHZ; + struct blasfeo_svec *xy; + struct blasfeo_svec *Yxy; + struct blasfeo_svec *xz; + struct blasfeo_svec *tmp_nv; + struct blasfeo_svec *tmp_2ns; + struct blasfeo_svec *tmp_nv2ns; + struct blasfeo_smat *A_li; // A of linearly independent equality constraints + struct blasfeo_svec *b_li; // b of linearly independent equality constraints + struct blasfeo_smat *A_bkp; // pointer to backup A + struct blasfeo_svec *b_bkp; // pointer to backup b + struct blasfeo_smat *Ab_LU; + float *stat; // convergence statistics + int *ipiv_v; + int *ipiv_e; + int *ipiv_e1; + void *lq_work0; + void *lq_work1; + void *lq_work_null; + void *orglq_work_null; + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // numer of recorded stat per ipm iter + int scale; + int use_hess_fact; + int use_A_fact; + int status; + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int ne_li; // number of linearly independent equality constraints + int ne_bkp; // ne backup + hpipm_size_t memsize; // memory size (in bytes) of workspace + }; + + + +// +hpipm_size_t s_dense_qp_ipm_arg_memsize(struct s_dense_qp_dim *qp_dim); +// +void s_dense_qp_ipm_arg_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, void *mem); +// +void s_dense_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set(char *field, void *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_iter_max(int *iter_max, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_mu0(float *mu0, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_reg_prim(float *reg, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_reg_dual(float *reg, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_warm_start(int *warm_start, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_cond_pred_corr(int *cond_pred_corr, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_comp_res_pred(int *comp_res_pred, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_comp_res_exit(int *comp_res_exit, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_lam_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_t_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_tau_min(float *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_kkt_fact_alg(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_remove_lin_dep_eq(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_compute_obj(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_split_step(int *value, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_arg_set_t_lam_min(int *value, struct s_dense_qp_ipm_arg *arg); + +// +hpipm_size_t s_dense_qp_ipm_ws_memsize(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_ipm_ws_create(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws, void *mem); +// +void s_dense_qp_ipm_get(char *field, struct s_dense_qp_ipm_ws *ws, void *value); +// +void s_dense_qp_ipm_get_status(struct s_dense_qp_ipm_ws *ws, int *status); +// +void s_dense_qp_ipm_get_iter(struct s_dense_qp_ipm_ws *ws, int *iter); +// +void s_dense_qp_ipm_get_max_res_stat(struct s_dense_qp_ipm_ws *ws, float *res_stat); +// +void s_dense_qp_ipm_get_max_res_eq(struct s_dense_qp_ipm_ws *ws, float *res_eq); +// +void s_dense_qp_ipm_get_max_res_ineq(struct s_dense_qp_ipm_ws *ws, float *res_ineq); +// +void s_dense_qp_ipm_get_max_res_comp(struct s_dense_qp_ipm_ws *ws, float *res_comp); +// +void s_dense_qp_ipm_get_stat(struct s_dense_qp_ipm_ws *ws, float **stat); +// +void s_dense_qp_ipm_get_stat_m(struct s_dense_qp_ipm_ws *ws, int *stat_m); +// +void s_dense_qp_init_var(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_abs_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_delta_step(int kk, struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_solve(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_predict(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_ipm_sens(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_compute_step_length(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h new file mode 100644 index 0000000..260dc0a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_kkt.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_KKT_H_ +#define HPIPM_S_DENSE_QP_KKT_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_fact_solve_kkt_unconstr_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_fact_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_fact_lq_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_solve_kkt_step_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_remove_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_restore_lin_dep_eq(struct s_dense_qp *qp, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); +// +void s_dense_qp_compute_obj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_ipm_arg *arg, struct s_dense_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_KKT_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h new file mode 100644 index 0000000..06b609c --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_res.h @@ -0,0 +1,106 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_RES_H_ +#define HPIPM_S_DENSE_QP_RES_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_res + { + struct s_dense_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_dense_qp_res_ws + { + struct blasfeo_svec *tmp_nbg; // work space of size nbM+ngM + struct blasfeo_svec *tmp_ns; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_res_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_res_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res *res, void *mem); +// +hpipm_size_t s_dense_qp_res_ws_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_res_ws_create(struct s_dense_qp_dim *dim, struct s_dense_qp_res_ws *workspace, void *mem); +// +void s_dense_qp_res_compute(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws); +// +void s_dense_qp_res_compute_lin(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_dense_qp_sol *qp_step, struct s_dense_qp_res *res, struct s_dense_qp_res_ws *ws); +// +void s_dense_qp_res_compute_inf_norm(struct s_dense_qp_res *res); +// +void s_dense_qp_res_get_all(struct s_dense_qp_res *res, float *res_g, float *res_ls, float *res_us, float *res_b, float *res_d_lb, float *res_d_ub, float *res_d_lg, float *res_d_ug, float *res_d_ls, float *res_d_us, float *res_m_lb, float *res_m_ub, float *res_m_lg, float *res_m_ug, float *res_m_ls, float *res_m_us); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_D_DENSE_QP_RES_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h new file mode 100644 index 0000000..1f40076 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_sol.h @@ -0,0 +1,94 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_DENSE_QP_SOL_H_ +#define HPIPM_S_DENSE_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_dense_qp_sol + { + struct s_dense_qp_dim *dim; + struct blasfeo_svec *v; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + float obj; + int valid_obj; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_dense_qp_sol_memsize(struct s_dense_qp_dim *dim); +// +void s_dense_qp_sol_create(struct s_dense_qp_dim *dim, struct s_dense_qp_sol *qp_sol, void *memory); +// +void s_dense_qp_sol_get_all(struct s_dense_qp_sol *qp_sol, float *v, float *ls, float *us, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug, float *lam_ls, float *lam_us); +// +void s_dense_qp_sol_get(char *field, struct s_dense_qp_sol *sol, void *value); +// +void s_dense_qp_sol_get_v(struct s_dense_qp_sol *sol, float *v); +// +void s_dense_qp_sol_get_valid_obj(struct s_dense_qp_sol *sol, int *valid_obj); +// +void s_dense_qp_sol_get_obj(struct s_dense_qp_sol *sol, float *obj); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_DENSE_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h new file mode 100644 index 0000000..3dd9325 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_dense_qp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_DENSE_QP_UTILS_H_ +#define HPIPM_S_DENSE_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_dense_qp_dim.h" +#include "hpipm_s_dense_qp.h" +#include "hpipm_s_dense_qp_sol.h" +#include "hpipm_s_dense_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_dense_qp_dim_print(struct s_dense_qp_dim *qp_dim); +// +//void s_dense_qp_dim_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim); +// +void s_dense_qp_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp); +// +//void s_dense_qp_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp *qp); +// +void s_dense_qp_sol_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_sol *dense_qp_sol); +// +//void s_dense_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *arg); +// +void s_dense_qp_res_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_res *dense_qp_res); +// +void s_dense_qp_arg_print(struct s_dense_qp_dim *qp_dim, struct s_dense_qp_ipm_arg *qp_ipm_arg); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_DENSE_QP_UTILS_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h new file mode 100644 index 0000000..b90b2ac --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp.h @@ -0,0 +1,303 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_H_ +#define HPIPM_S_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_smat *BAbt; // dynamics matrix & vector work space + struct blasfeo_smat *RSQrq; // hessian of cost & vector work space + struct blasfeo_smat *DCt; // inequality constraints matrix + struct blasfeo_smat **Hq; // hessians of quadratic constraints + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **Hq_nzero; // for each int, the last 3 bits ...abc, {a,b,c}=0 => {R,S,Q}=0 + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qcqp_strsize(); +// +hpipm_size_t s_ocp_qcqp_memsize(struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp *qp, void *memory); +// +void s_ocp_qcqp_copy_all(struct s_ocp_qcqp *qp_orig, struct s_ocp_qcqp *qp_dest); + +// setters +// +void s_ocp_qcqp_set_all_zero(struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_rhs_zero(struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set(char *fiels_name, int stage, void *value, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_A(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_B(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_b(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Q(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_S(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_R(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_q(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_r(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lb(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lb_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ub(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ub_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbx_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubx_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lbu_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ubu_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxb(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxbx(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxbu(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_C(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_D(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lg(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lg_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ug(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_ug_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Qq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Sq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Rq(int stage, float *mat, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_qq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_rq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_uq(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_uq_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Zl(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Zu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_zl(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_zu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lls(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lls_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lus(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_lus_mask(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxs(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_idxs_rev(int stage, int *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsbu(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsbx(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsg(int stage, float *vec, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_set_Jsq(int stage, float *vec, struct s_ocp_qcqp *qp); + +// getters +// +void s_ocp_qcqp_get(char *field, int stage, struct s_ocp_qcqp *qp, void *value); +// +void s_ocp_qcqp_get_A(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_B(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_b(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Q(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_S(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_R(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_q(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_r(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ub(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ub_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lb(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lb_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbx_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubx_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lbu_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ubu_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_idxb(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_idxbx(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_idxbu(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_C(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_D(int stage, struct s_ocp_qcqp *qp, float *mat); +// +void s_ocp_qcqp_get_lg(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lg_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ug(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_ug_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Zl(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_Zu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_zl(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_zu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lls(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lls_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lus(int stage, struct s_ocp_qcqp *qp, float *vec); +// +void s_ocp_qcqp_get_lus_mask(int stage, struct s_ocp_qcqp *qp, float *vec); +// XXX only valid if there is one slack per softed constraint !!! +void s_ocp_qcqp_get_idxs(int stage, struct s_ocp_qcqp *qp, int *vec); +// +void s_ocp_qcqp_get_idxs_rev(int stage, struct s_ocp_qcqp *qp, int *vec); +// +//void s_ocp_qcqp_get_Jsbu(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_Jsbx(int stage, struct s_ocp_qcqp *qp, float *vec); +// +//void s_ocp_qcqp_get_Jsg(int stage, struct s_ocp_qcqp *qp, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h new file mode 100644 index 0000000..c09903f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_dim.h @@ -0,0 +1,119 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_DIM_H_ +#define HPIPM_S_OCP_QCQP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_dim + { + struct s_ocp_qp_dim *qp_dim; // dim of qp approximation + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of (two-sided) state box constraints + int *nbu; // number of (two-sided) input box constraints + int *ng; // number of (two-sided) general constraints + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints + int *nsbx; // number of (two-sided) soft state box constraints + int *nsbu; // number of (two-sided) soft input box constraints + int *nsg; // number of (two-sided) soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_dim_strsize(); +// +hpipm_size_t s_ocp_qcqp_dim_memsize(int N); +// +void s_ocp_qcqp_dim_create(int N, struct s_ocp_qcqp_dim *qp_dim, void *memory); +// +void s_ocp_qcqp_dim_copy_all(struct s_ocp_qcqp_dim *dim_orig, struct s_ocp_qcqp_dim *dim_dest); +// +void s_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_ng(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nq(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_ns(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_dim_get(struct s_ocp_qcqp_dim *dim, char *field, int stage, int *value); +// +void s_ocp_qcqp_dim_get_N(struct s_ocp_qcqp_dim *dim, int *value); +// +void s_ocp_qcqp_dim_get_nx(struct s_ocp_qcqp_dim *dim, int stage, int *value); +// +void s_ocp_qcqp_dim_get_nu(struct s_ocp_qcqp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_DIM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h new file mode 100644 index 0000000..c14fc1c --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_ipm.h @@ -0,0 +1,191 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_IPM_H_ +#define HPIPM_S_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_ipm_arg + { + struct s_ocp_qp_ipm_arg *qp_arg; + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_ocp_qcqp_ipm_ws + { + struct s_ocp_qp_ipm_ws *qp_ws; + struct s_ocp_qp *qp; + struct s_ocp_qp_sol *qp_sol; + struct s_ocp_qcqp_res_ws *qcqp_res_ws; + struct s_ocp_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t s_ocp_qcqp_ipm_arg_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_ipm_arg_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, void *mem); +// +void s_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void s_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void s_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void s_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void s_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void s_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void s_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +void s_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void s_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void s_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void s_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t s_ocp_qcqp_ipm_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_ipm_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws, void *mem); +// +void s_ocp_qcqp_ipm_get(char *field, struct s_ocp_qcqp_ipm_ws *ws, void *value); +// +void s_ocp_qcqp_ipm_get_status(struct s_ocp_qcqp_ipm_ws *ws, int *status); +// +void s_ocp_qcqp_ipm_get_iter(struct s_ocp_qcqp_ipm_ws *ws, int *iter); +// +void s_ocp_qcqp_ipm_get_max_res_stat(struct s_ocp_qcqp_ipm_ws *ws, float *res_stat); +// +void s_ocp_qcqp_ipm_get_max_res_eq(struct s_ocp_qcqp_ipm_ws *ws, float *res_eq); +// +void s_ocp_qcqp_ipm_get_max_res_ineq(struct s_ocp_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_ocp_qcqp_ipm_get_max_res_comp(struct s_ocp_qcqp_ipm_ws *ws, float *res_comp); +// +void s_ocp_qcqp_ipm_get_stat(struct s_ocp_qcqp_ipm_ws *ws, float **stat); +// +void s_ocp_qcqp_ipm_get_stat_m(struct s_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void s_ocp_qcqp_init_var(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws); +// +void s_ocp_qcqp_ipm_solve(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_ipm_arg *arg, struct s_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QCQP_IPM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h new file mode 100644 index 0000000..1ceeec9 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_res.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_RES_H_ +#define HPIPM_S_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_res + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_ocp_qcqp_res_ws + { + struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qcqp_res_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_res_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t s_ocp_qcqp_res_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim); +// +void s_ocp_qcqp_res_ws_create(struct s_ocp_qcqp_dim *ocp_dim, struct s_ocp_qcqp_res_ws *workspace, void *mem); +// +void s_ocp_qcqp_res_compute(struct s_ocp_qcqp *qp, struct s_ocp_qcqp_sol *qp_sol, struct s_ocp_qcqp_res *res, struct s_ocp_qcqp_res_ws *ws); +// +void s_ocp_qcqp_res_compute_inf_norm(struct s_ocp_qcqp_res *res); +// +void s_ocp_qcqp_res_get_max_res_stat(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_eq(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_ineq(struct s_ocp_qcqp_res *res, float *value); +// +void s_ocp_qcqp_res_get_max_res_comp(struct s_ocp_qcqp_res *res, float *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QCQP_RES_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h new file mode 100644 index 0000000..3d58022 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_sol.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_SOL_H_ +#define HPIPM_S_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qcqp_sol + { + struct s_ocp_qcqp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qcqp_sol_strsize(); +// +hpipm_size_t s_ocp_qcqp_sol_memsize(struct s_ocp_qcqp_dim *dim); +// +void s_ocp_qcqp_sol_create(struct s_ocp_qcqp_dim *dim, struct s_ocp_qcqp_sol *qp_sol, void *memory); +// +void s_ocp_qcqp_sol_copy_all(struct s_ocp_qcqp_sol *qp_sol_orig, struct s_ocp_qcqp_sol *qp_sol_dest); +// +void s_ocp_qcqp_sol_get(char *field, int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_u(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_x(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_sl(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_su(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_pi(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_lb(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_ub(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_lg(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_get_lam_ug(int stage, struct s_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_ocp_qcqp_sol_set(char *field, int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_u(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_x(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_sl(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); +// +void s_ocp_qcqp_sol_set_su(int stage, float *vec, struct s_ocp_qcqp_sol *qp_sol); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_SOL_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h new file mode 100644 index 0000000..d64e3aa --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qcqp_utils.h @@ -0,0 +1,82 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QCQP_UTILS_H_ +#define HPIPM_S_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qcqp_dim.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qcqp_sol.h" +#include "hpipm_s_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_ocp_qcqp_dim_print(struct s_ocp_qcqp_dim *qcqp_dim); +// +void s_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim); +// +void s_ocp_qcqp_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp *qp); +// +void s_ocp_qcqp_sol_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_sol *ocp_qcqp_sol); +// +void s_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_ipm_arg *arg); +// +void s_ocp_qcqp_res_print(struct s_ocp_qcqp_dim *qcqp_dim, struct s_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QCQP_UTILS_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h new file mode 100644 index 0000000..b49191f --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp.h @@ -0,0 +1,306 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_H_ +#define HPIPM_S_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp + { + struct s_ocp_qp_dim *dim; + struct blasfeo_smat *BAbt; // dynamics matrix & vector work space + struct blasfeo_smat *RSQrq; // hessian of cost & vector work space + struct blasfeo_smat *DCt; // inequality constraints matrix + struct blasfeo_svec *b; // dynamics vector + struct blasfeo_svec *rqz; // gradient of cost & gradient of slacks + struct blasfeo_svec *d; // inequality constraints vector + struct blasfeo_svec *d_mask; // inequality constraints mask vector + struct blasfeo_svec *m; // rhs of complementarity condition + struct blasfeo_svec *Z; // (diagonal) hessian of slacks + int **idxb; // indices of box constrained variables within [u; x] + int **idxs_rev; // index of soft constraints (reverse storage) + int **idxe; // indices of constraints within [bu, bx, g] that are equalities, subset of [0, ..., nbu+nbx+ng-1] + int *diag_H_flag; // flag the fact that Hessian is diagonal + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qp_strsize(); +// +hpipm_size_t s_ocp_qp_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp *qp, void *memory); +// +void s_ocp_qp_copy_all(struct s_ocp_qp *qp_orig, struct s_ocp_qp *qp_dest); + +// setters +// +void s_ocp_qp_set_all_zero(struct s_ocp_qp *qp); +// +void s_ocp_qp_set_rhs_zero(struct s_ocp_qp *qp); +// +void s_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_all_rowmaj(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxbx, float **lbx, float **ubx, int **idxbu, float **lbu, float **ubu, float **C, float **D, float **lg, float **ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **ls, float **us, struct s_ocp_qp *qp); +// +void s_ocp_qp_set(char *fiels_name, int stage, void *value, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el(char *fiels_name, int stage, int index, void *value, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_A(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_B(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_b(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Q(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_S(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_R(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_q(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_r(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lb(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lb_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ub(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ub_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbx_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el_lbx(int stage, int index, float *elem, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubx_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_el_ubx(int stage, int index, float *elem, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lbu_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ubu_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxb(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbx(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbu(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_C(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_D(int stage, float *mat, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lg(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lg_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ug(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_ug_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Zl(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Zu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_zl(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_zu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lls(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lls_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lus(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_lus_mask(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxs(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxs_rev(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsbu(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsbx(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jsg(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxe(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbxe(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxbue(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_idxge(int stage, int *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbxe(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jbue(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_Jge(int stage, float *vec, struct s_ocp_qp *qp); +// +void s_ocp_qp_set_diag_H_flag(int stage, int *value, struct s_ocp_qp *qp); + +// getters +// +void s_ocp_qp_get(char *field, int stage, struct s_ocp_qp *qp, void *value); +// +void s_ocp_qp_get_A(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_B(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_b(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Q(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_S(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_R(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_q(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_r(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ub(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ub_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lb(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lb_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbx(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbx_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubx(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubx_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lbu_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ubu_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_idxb(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_idxbx(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jbx(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_idxbu(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jbu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_C(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_D(int stage, struct s_ocp_qp *qp, float *mat); +// +void s_ocp_qp_get_lg(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lg_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ug(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_ug_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Zl(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_Zu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_zl(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_zu(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lls(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lls_mask(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lus(int stage, struct s_ocp_qp *qp, float *vec); +// +void s_ocp_qp_get_lus_mask(int stage, struct s_ocp_qp *qp, float *vec); +// XXX only valid if there is one slack per softed constraint !!! +void s_ocp_qp_get_idxs(int stage, struct s_ocp_qp *qp, int *vec); +// +void s_ocp_qp_get_idxs_rev(int stage, struct s_ocp_qp *qp, int *vec); +// +//void s_ocp_qp_get_Jsbu(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_Jsbx(int stage, struct s_ocp_qp *qp, float *vec); +// +//void s_ocp_qp_get_Jsg(int stage, struct s_ocp_qp *qp, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h new file mode 100644 index 0000000..bce8024 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_dim.h @@ -0,0 +1,141 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_DIM_H_ +#define HPIPM_S_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_dim + { + int *nx; // number of states + int *nu; // number of inputs + int *nb; // number of box constraints + int *nbx; // number of state box constraints + int *nbu; // number of input box constraints + int *ng; // number of general constraints + int *ns; // number of soft constraints + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nbxe; // number of state box constraints which are equality + int *nbue; // number of input box constraints which are equality + int *nge; // number of general constraints which are equality + int N; // horizon length + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_dim_strsize(); +// +hpipm_size_t s_ocp_qp_dim_memsize(int N); +// +void s_ocp_qp_dim_create(int N, struct s_ocp_qp_dim *qp_dim, void *memory); +// +void s_ocp_qp_dim_copy_all(struct s_ocp_qp_dim *dim_orig, struct s_ocp_qp_dim *dim_dest); +// +void s_ocp_qp_dim_set_all(int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set(char *field, int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_ng(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_ns(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsbx(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsbu(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nsg(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbxe(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nbue(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_set_nge(int stage, int value, struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_dim_get(struct s_ocp_qp_dim *dim, char *field, int stage, int *value); +// +void s_ocp_qp_dim_get_N(struct s_ocp_qp_dim *dim, int *value); +// +void s_ocp_qp_dim_get_nx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_ng(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_ns(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsbx(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsbu(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nsg(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbxe(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nbue(struct s_ocp_qp_dim *dim, int stage, int *value); +// +void s_ocp_qp_dim_get_nge(struct s_ocp_qp_dim *dim, int stage, int *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h new file mode 100644 index 0000000..11f3c47 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_ipm.h @@ -0,0 +1,250 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_IPM_H_ +#define HPIPM_S_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_ipm_arg + { + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) + int comp_res_pred; // compute residuals of prediction + int split_step; // use different steps for primal and dual variables + int var_init_scheme; // variables initialization scheme + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_ocp_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_ocp_qp_res_ws *res_workspace; + struct s_ocp_qp_dim *dim; // cache dim + struct s_ocp_qp_sol *sol_step; + struct s_ocp_qp_sol *sol_itref; + struct s_ocp_qp *qp_step; + struct s_ocp_qp *qp_itref; + struct s_ocp_qp_res *res; + struct s_ocp_qp_res *res_itref; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nuxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_svec *tmp_m; + struct blasfeo_svec *l; // cache linear part for _get_ric_xxx + struct blasfeo_smat *L; + struct blasfeo_smat *Ls; + struct blasfeo_smat *P; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_smat *tmp_nxM_nxM; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int square_root_alg; // cache from arg + int lq_fact; // cache from arg + int mask_constr; // use constr mask + int valid_ric_vec; // meaningful riccati vectors + int valid_ric_p; // form of riccati p: 0 p*inv(L), 1 p + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_ipm_arg_strsize(); +// +hpipm_size_t s_ocp_qp_ipm_arg_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_ipm_arg_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, void *mem); +// +void s_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_arg_set(char *field, void *value, struct s_ocp_qp_ipm_arg *arg); +// set maximum number of iterations +void s_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_ocp_qp_ipm_arg *arg); +// set minimum step lenght +void s_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_ocp_qp_ipm_arg *arg); +// set initial value of barrier parameter +void s_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_ocp_qp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_ocp_qp_ipm_arg *arg); +// set regularization of primal variables +void s_ocp_qp_ipm_arg_set_reg_prim(float *tol_comp, struct s_ocp_qp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_ocp_qp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_ocp_qp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_ocp_qp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +void s_ocp_qp_ipm_arg_set_ric_alg(int *alg, struct s_ocp_qp_ipm_arg *arg); +// dual solution of equality constraints (only for abs_form==1) +void s_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_ocp_qp_ipm_arg *arg); +// compute residuals after solution +void s_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_ocp_qp_ipm_arg *arg); +// compute residuals of prediction +void s_ocp_qp_ipm_arg_set_comp_res_pred(int *alg, struct s_ocp_qp_ipm_arg *arg); +// min value of lam in the solution +void s_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// min value of t in the solution +void s_ocp_qp_ipm_arg_set_t_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// min value of tau in the solution +void s_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_ocp_qp_ipm_arg *arg); +// set split step: 0 same step, 1 different step for primal and dual variables +void s_ocp_qp_ipm_arg_set_split_step(int *value, struct s_ocp_qp_ipm_arg *arg); +// variables initialization scheme +void s_ocp_qp_ipm_arg_set_var_init_scheme(int *value, struct s_ocp_qp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t s_ocp_qp_ipm_ws_strsize(); +// +hpipm_size_t s_ocp_qp_ipm_ws_memsize(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, void *mem); +// +void s_ocp_qp_ipm_get(char *field, struct s_ocp_qp_ipm_ws *ws, void *value); +// +void s_ocp_qp_ipm_get_status(struct s_ocp_qp_ipm_ws *ws, int *status); +// +void s_ocp_qp_ipm_get_iter(struct s_ocp_qp_ipm_ws *ws, int *iter); +// +void s_ocp_qp_ipm_get_max_res_stat(struct s_ocp_qp_ipm_ws *ws, float *res_stat); +// +void s_ocp_qp_ipm_get_max_res_eq(struct s_ocp_qp_ipm_ws *ws, float *res_eq); +// +void s_ocp_qp_ipm_get_max_res_ineq(struct s_ocp_qp_ipm_ws *ws, float *res_ineq); +// +void s_ocp_qp_ipm_get_max_res_comp(struct s_ocp_qp_ipm_ws *ws, float *res_comp); +// +void s_ocp_qp_ipm_get_stat(struct s_ocp_qp_ipm_ws *ws, float **stat); +// +void s_ocp_qp_ipm_get_stat_m(struct s_ocp_qp_ipm_ws *ws, int *stat_m); +// +void s_ocp_qp_ipm_get_ric_Lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Lr); +// +void s_ocp_qp_ipm_get_ric_Ls(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *Ls); +// +void s_ocp_qp_ipm_get_ric_P(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *P); +// +void s_ocp_qp_ipm_get_ric_lr(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *lr); +// +void s_ocp_qp_ipm_get_ric_p(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *p); +// feedback control gain in the form u = K x + k +void s_ocp_qp_ipm_get_ric_K(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *K); +// feedback control gain in the form u = K x + k +void s_ocp_qp_ipm_get_ric_k(struct s_ocp_qp *qp, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws, int stage, float *k); +// +void s_ocp_qp_init_var(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_abs_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_delta_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_solve(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_predict(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_ipm_sens(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h new file mode 100644 index 0000000..3eb1f4a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_kkt.h @@ -0,0 +1,66 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef HPIPM_S_OCP_QP_KKT_H_ +#define HPIPM_S_OCP_QP_KKT_H_ + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_ocp_qp_fact_solve_kkt_unconstr(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_fact_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_fact_lq_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); +// +void s_ocp_qp_solve_kkt_step(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + +#endif // HPIPM_S_OCP_QP_KKT_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h new file mode 100644 index 0000000..5a7b3b0 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_red.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_RED_H_ +#define HPIPM_S_OCP_QP_RED_H_ + + + +#include +#include + +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_reduce_eq_dof_arg + { + float lam_min; + float t_min; + int alias_unchanged; // do not keep copy unchanged stage + int comp_prim_sol; // primal solution (v) + int comp_dual_sol_eq; // dual solution equality constr (pi) + int comp_dual_sol_ineq; // dual solution inequality constr (lam t) + hpipm_size_t memsize; // memory size in bytes + }; + + + +struct s_ocp_qp_reduce_eq_dof_ws + { + struct blasfeo_svec *tmp_nuxM; + struct blasfeo_svec *tmp_nbgM; + int *e_imask_ux; + int *e_imask_d; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +void s_ocp_qp_dim_reduce_eq_dof(struct s_ocp_qp_dim *dim, struct s_ocp_qp_dim *dim_red); +// +hpipm_size_t s_ocp_qp_reduce_eq_dof_arg_memsize(); +// +void s_ocp_qp_reduce_eq_dof_arg_create(struct s_ocp_qp_reduce_eq_dof_arg *arg, void *mem); +// +void s_ocp_qp_reduce_eq_dof_arg_set_default(struct s_ocp_qp_reduce_eq_dof_arg *arg); +// +void s_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_prim_sol(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +void s_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(struct s_ocp_qp_reduce_eq_dof_arg *arg, int value); +// +hpipm_size_t s_ocp_qp_reduce_eq_dof_ws_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_reduce_eq_dof_ws_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_reduce_eq_dof_ws *work, void *mem); +// +void s_ocp_qp_reduce_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_reduce_eq_dof_lhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_reduce_eq_dof_rhs(struct s_ocp_qp *qp, struct s_ocp_qp *qp_red, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); +// +void s_ocp_qp_restore_eq_dof(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol_red, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_reduce_eq_dof_arg *arg, struct s_ocp_qp_reduce_eq_dof_ws *work); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_OCP_QP_RED_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h new file mode 100644 index 0000000..821585d --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_res.h @@ -0,0 +1,114 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_RES_H_ +#define HPIPM_S_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_res + { + struct s_ocp_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_ocp_qp_res_ws + { + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_ocp_qp_res_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_res_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res *res, void *mem); +// +hpipm_size_t s_ocp_qp_res_ws_memsize(struct s_ocp_qp_dim *ocp_dim); +// +void s_ocp_qp_res_ws_create(struct s_ocp_qp_dim *ocp_dim, struct s_ocp_qp_res_ws *workspace, void *mem); +// +void s_ocp_qp_res_compute(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws); +// +void s_ocp_qp_res_compute_lin(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_sol *qp_step, struct s_ocp_qp_res *res, struct s_ocp_qp_res_ws *ws); +// +void s_ocp_qp_res_compute_inf_norm(struct s_ocp_qp_res *res); +// +void s_ocp_qp_res_get_all(struct s_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us); +// +void s_ocp_qp_res_get_max_res_stat(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_eq(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_ineq(struct s_ocp_qp_res *res, float *value); +// +void s_ocp_qp_res_get_max_res_comp(struct s_ocp_qp_res *res, float *value); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_OCP_QP_RES_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h new file mode 100644 index 0000000..94dfa0d --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_sol.h @@ -0,0 +1,128 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_SOL_H_ +#define HPIPM_S_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_ocp_qp_sol + { + struct s_ocp_qp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_ocp_qp_sol_strsize(); +// +hpipm_size_t s_ocp_qp_sol_memsize(struct s_ocp_qp_dim *dim); +// +void s_ocp_qp_sol_create(struct s_ocp_qp_dim *dim, struct s_ocp_qp_sol *qp_sol, void *memory); +// +void s_ocp_qp_sol_copy_all(struct s_ocp_qp_sol *qp_sol_orig, struct s_ocp_qp_sol *qp_sol_dest); +// +void s_qp_sol_get_all(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_qp_sol_get_all_rowmaj(struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_ocp_qp_sol_set_all(float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_get(char *field, int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_u(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_x(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_sl(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_su(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_pi(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lb(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lbu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lbx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ub(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ubu(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ubx(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_lg(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_get_lam_ug(int stage, struct s_ocp_qp_sol *qp_sol, float *vec); +// +void s_ocp_qp_sol_set(char *field, int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_u(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_x(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_sl(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); +// +void s_ocp_qp_sol_set_su(int stage, float *vec, struct s_ocp_qp_sol *qp_sol); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_OCP_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h new file mode 100644 index 0000000..a4f832a --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_ocp_qp_utils.h @@ -0,0 +1,83 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_OCP_QP_UTILS_H_ +#define HPIPM_S_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_ocp_qp_dim.h" +#include "hpipm_s_ocp_qp.h" +#include "hpipm_s_ocp_qp_sol.h" +#include "hpipm_s_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_ocp_qp_dim_print(struct s_ocp_qp_dim *qp_dim); +// +void s_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim); +// +void s_ocp_qp_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp); +// +void s_ocp_qp_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp *qp); +// +void s_ocp_qp_sol_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_sol *ocp_qp_sol); +// +void s_ocp_qp_ipm_arg_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_ipm_arg *arg); +// +void s_ocp_qp_res_print(struct s_ocp_qp_dim *qp_dim, struct s_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_D_OCP_QP_UTILS_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h b/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h new file mode 100644 index 0000000..e40511e --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_part_cond.h @@ -0,0 +1,115 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_PART_COND_H_ +#define HPIPM_S_PART_COND_H_ + + + +#include +#include + +#include "hpipm_s_cond.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_part_cond_qp_arg + { + struct s_cond_qp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct s_part_cond_qp_ws + { + struct s_cond_qp_ws *cond_workspace; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_part_cond_qp_arg_memsize(int N2); +// +void s_part_cond_qp_arg_create(int N2, struct s_part_cond_qp_arg *cond_arg, void *mem); +// +void s_part_cond_qp_arg_set_default(struct s_part_cond_qp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void s_part_cond_qp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_prim_sol(int value, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_dual_sol_eq(int value, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_arg_set_comp_dual_sol_ineq(int value, struct s_part_cond_qp_arg *cond_arg); + +// +void s_part_cond_qp_compute_block_size(int N, int N2, int *block_size); +// +void s_part_cond_qp_compute_dim(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim); +// +hpipm_size_t s_part_cond_qp_ws_memsize(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg); +// +void s_part_cond_qp_ws_create(struct s_ocp_qp_dim *ocp_dim, int *block_size, struct s_ocp_qp_dim *part_dense_dim, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws, void *mem); +// +void s_part_cond_qp_cond(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_cond_lhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_cond_rhs(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); +// +void s_part_cond_qp_expand_sol(struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_ocp_qp_sol *part_dense_qp_sol, struct s_ocp_qp_sol *ocp_qp_sol, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); + +// +void s_part_cond_qp_update(int *idxc, struct s_ocp_qp *ocp_qp, struct s_ocp_qp *part_dense_qp, struct s_part_cond_qp_arg *cond_arg, struct s_part_cond_qp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_PART_COND_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h new file mode 100644 index 0000000..311f700 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_part_cond_qcqp.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_PART_COND_QCQP_H_ +#define HPIPM_S_PART_COND_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_cond_qcqp.h" + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_part_cond_qcqp_arg + { + struct s_cond_qcqp_arg *cond_arg; + int N2; + hpipm_size_t memsize; + }; + + + +struct s_part_cond_qcqp_ws + { + struct s_cond_qcqp_ws *cond_ws; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_part_cond_qcqp_arg_memsize(int N2); +// +void s_part_cond_qcqp_arg_create(int N2, struct s_part_cond_qcqp_arg *cond_arg, void *mem); +// +void s_part_cond_qcqp_arg_set_default(struct s_part_cond_qcqp_arg *cond_arg); +// set riccati-like algorithm: 0 classical, 1 squre-root +void s_part_cond_qcqp_arg_set_ric_alg(int ric_alg, struct s_part_cond_qcqp_arg *cond_arg); + +// +void s_part_cond_qcqp_compute_block_size(int N, int N2, int *block_size); +// +void s_part_cond_qcqp_compute_dim(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim); +// +hpipm_size_t s_part_cond_qcqp_ws_memsize(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg); +// +void s_part_cond_qcqp_ws_create(struct s_ocp_qcqp_dim *ocp_dim, int *block_size, struct s_ocp_qcqp_dim *part_dense_dim, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws, void *mem); +// +void s_part_cond_qcqp_cond(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_cond_lhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_cond_rhs(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); +// +void s_part_cond_qcqp_expand_sol(struct s_ocp_qcqp *ocp_qp, struct s_ocp_qcqp *part_dense_qp, struct s_ocp_qcqp_sol *part_dense_qp_sol, struct s_ocp_qcqp_sol *ocp_qp_sol, struct s_part_cond_qcqp_arg *cond_arg, struct s_part_cond_qcqp_ws *cond_ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_D_PART_COND_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h b/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h new file mode 100644 index 0000000..1a05ad1 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_sim_erk.h @@ -0,0 +1,121 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_SIM_ERK_H_ +#define HPIPM_S_SIM_ERK_H_ + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_sim_erk_arg + { + struct s_sim_rk_data *rk_data; // integrator data + float h; // step size + int steps; // number of steps +// int for_sens; // compute adjoint sensitivities +// int adj_sens; // compute adjoint sensitivities + hpipm_size_t memsize; + }; + + + +struct s_sim_erk_ws + { + void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to ode + void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot); // function pointer to forward vde + void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out); // function pointer to adjoint vde + void *ode_args; // pointer to ode args + struct s_sim_erk_arg *erk_arg; // erk arg + float *K; // internal variables + float *x_for; // states and forward sensitivities + float *x_traj; // states at all steps + float *l; // adjoint sensitivities + float *p; // parameter + float *x_tmp; // temporary states and forward sensitivities + float *adj_in; + float *adj_tmp; + int nx; // number of states + int np; // number of parameters + int nf; // number of forward sensitivities + int na; // number of adjoint sensitivities + int nf_max; // max number of forward sensitivities + int na_max; // max number of adjoint sensitivities + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_sim_erk_arg_memsize(); +// +void s_sim_erk_arg_create(struct s_sim_erk_arg *erk_arg, void *mem); +// +void s_sim_erk_arg_set_all(struct s_sim_rk_data *rk_data, float h, int steps, struct s_sim_erk_arg *erk_arg); + +// +hpipm_size_t s_sim_erk_ws_memsize(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max); +// +void s_sim_erk_ws_create(struct s_sim_erk_arg *erk_arg, int nx, int np, int nf_max, int na_max, struct s_sim_erk_ws *work, void *memory); +// +void s_sim_erk_ws_set_all(int nf, int na, float *x, float *fs, float *bs, float *p, void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_for)(int t, float *x, float *p, void *ode_args, float *xdot), void (*vde_adj)(int t, float *adj_in, void *ode_args, float *adj_out), void *ode_args, struct s_sim_erk_ws *work); +// number of directions for forward sensitivities +void s_sim_erk_ws_set_nf(int *nf, struct s_sim_erk_ws *work); +// parameters (e.g. inputs) +void s_sim_erk_ws_set_p(float *p, struct s_sim_erk_ws *work); +// state +void s_sim_erk_ws_set_x(float *x, struct s_sim_erk_ws *work); +// forward sensitivities +void s_sim_erk_ws_set_fs(float *fs, struct s_sim_erk_ws *work); +// ode funtion +void s_sim_erk_ws_set_ode(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work); +// forward vde function +void s_sim_erk_ws_set_vde_for(void (*ode)(int t, float *x, float *p, void *ode_args, float *xdot), struct s_sim_erk_ws *work); +// ode_args, passed straight to the ode/vde_for/vde_adj functions +void s_sim_erk_ws_set_ode_args(void *ode_args, struct s_sim_erk_ws *work); +// state +void s_sim_erk_ws_get_x(struct s_sim_erk_ws *work, float *x); +// forward sensitivities +void s_sim_erk_ws_get_fs(struct s_sim_erk_ws *work, float *fs); +// +void s_sim_erk_solve(struct s_sim_erk_arg *arg, struct s_sim_erk_ws *work); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_D_SIM_ERK_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h b/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h new file mode 100644 index 0000000..53acd71 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_sim_rk.h @@ -0,0 +1,72 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_SIM_RK_H_ +#define HPIPM_S_SIM_RK_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_sim_rk_data + { + float *A_rk; // A in butcher tableau + float *B_rk; // b in butcher tableau + float *C_rk; // c in butcher tableau + int expl; // erk vs irk + int ns; // number of stages + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_sim_rk_data_memsize(int ns); +// +void s_sim_rk_data_create(int ns, struct s_sim_rk_data *rk_data, void *memory); +// +void s_sim_rk_data_init_default(char *field, struct s_sim_rk_data *rk_data); +// +void s_sim_rk_data_set_all(int expl, float *A_rk, float *B_rk, float *C_rk, struct s_sim_rk_data *rk_data); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_S_SIM_RK_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h new file mode 100644 index 0000000..450e992 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp.h @@ -0,0 +1,213 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_H_ +#define HPIPM_S_TREE_OCP_QCQP_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_smat *BAbt; // Nn-1 + struct blasfeo_smat *RSQrq; // Nn + struct blasfeo_smat *DCt; // Nn + struct blasfeo_smat **Hq; // Nn + struct blasfeo_svec *b; // Nn-1 + struct blasfeo_svec *rqz; // Nn + struct blasfeo_svec *d; // Nn + struct blasfeo_svec *d_mask; // Nn + struct blasfeo_svec *m; // Nn + struct blasfeo_svec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_memsize(struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp *qp, void *memory); +// +void s_tree_ocp_qcqp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_A(int edge, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_B(int edge, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_b(int edge, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Q(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_S(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_R(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_q(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_r(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lb(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ub(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxb(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxbx(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxbu(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_C(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_D(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lg(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ug(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Qq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Sq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Rq(int node, float *mat, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_qq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_rq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_uq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_uq_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Zl(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Zu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_zl(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_zu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lls(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lus(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxs(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsg(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_set_Jsq(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxe(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxbue(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_idxge(int node, int *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jbue(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_Jge(int node, float *vec, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qcqp *qp); + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h new file mode 100644 index 0000000..7a49d51 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_dim.h @@ -0,0 +1,118 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_DIM_H_ +#define HPIPM_S_TREE_OCP_QCQP_DIM_H_ + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_dim + { + struct s_tree_ocp_qp_dim *qp_dim; // dim of qp approximation + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *nq; // number of (upper) quadratic constraints + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int *nsq; // number of (upper) soft quadratic constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_dim_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_dim_memsize(int Nn); +// +void s_tree_ocp_qcqp_dim_create(int Nn, struct s_tree_ocp_qcqp_dim *qp_dim, void *memory); +// +void s_tree_ocp_qcqp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_ng(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_ns(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_dim_set_nsq(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); +// +//void s_tree_ocp_qcqp_dim_set_nge(int stage, int value, struct s_tree_ocp_qcqp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_DIM_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h new file mode 100644 index 0000000..69d6591 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_ipm.h @@ -0,0 +1,192 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_IPM_H_ +#define HPIPM_S_TREE_OCP_QCQP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_ipm_arg + { + struct s_tree_ocp_qp_ipm_arg *qp_arg; + float mu0; // initial value for complementarity slackness + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol +// int square_root_alg; // 0 classical Riccati, 1 square-root Riccati + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq (for square_root_alg==1) + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution of equality constraints (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol_eq==1) +// int comp_res_pred; // compute residuals of prediction + int split_step; // use different step for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qcqp_ipm_ws + { + struct s_tree_ocp_qp_ipm_ws *qp_ws; + struct s_tree_ocp_qp *qp; + struct s_tree_ocp_qp_sol *qp_sol; + struct s_tree_ocp_qcqp_res_ws *qcqp_res_ws; + struct s_tree_ocp_qcqp_res *qcqp_res; + struct blasfeo_svec *tmp_nuxM; + int iter; // iteration number + int status; + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_ipm_arg_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_ipm_arg_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_ipm_arg_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, void *mem); +// +void s_tree_ocp_qcqp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_ipm_arg_set(char *field, void *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set maximum number of iterations +void s_tree_ocp_qcqp_ipm_arg_set_iter_max(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set minimum step lenght +void s_tree_ocp_qcqp_ipm_arg_set_alpha_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set initial value of barrier parameter +void s_tree_ocp_qcqp_ipm_arg_set_mu0(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on stationarity condition +void s_tree_ocp_qcqp_ipm_arg_set_tol_stat(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on equality constr +void s_tree_ocp_qcqp_ipm_arg_set_tol_eq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on inequality constr +void s_tree_ocp_qcqp_ipm_arg_set_tol_ineq(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set exit tolerance on complementarity condition +void s_tree_ocp_qcqp_ipm_arg_set_tol_comp(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set regularization of primal variables +void s_tree_ocp_qcqp_ipm_arg_set_reg_prim(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set warm start: 0 no warm start, 1 primal var +void s_tree_ocp_qcqp_ipm_arg_set_warm_start(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// Mehrotra's predictor-corrector IPM algorithm: 0 no predictor-corrector, 1 use predictor-corrector +void s_tree_ocp_qcqp_ipm_arg_set_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// conditional predictor-corrector: 0 no conditinal predictor-corrector, 1 conditional predictor-corrector +void s_tree_ocp_qcqp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// set riccati algorithm: 0 classic, 1 square-root +//void s_tree_ocp_qcqp_ipm_arg_set_ric_alg(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals after solution +void s_tree_ocp_qcqp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// compute residuals of prediction +//void s_tree_ocp_qcqp_ipm_arg_set_comp_res_pred(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// min value of lam in the solution +void s_tree_ocp_qcqp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// min value of t in the solution +void s_tree_ocp_qcqp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// use different step for primal and dual variables +void s_tree_ocp_qcqp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); +// clip t and lam: 0 no, 1 in Gamma computation, 2 in solution +void s_tree_ocp_qcqp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qcqp_ipm_arg *arg); + +// +hpipm_size_t s_tree_ocp_qcqp_ipm_ws_strsize(); +// +hpipm_size_t s_tree_ocp_qcqp_ipm_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_ipm_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws, void *mem); +// +void s_tree_ocp_qcqp_ipm_get(char *field, struct s_tree_ocp_qcqp_ipm_ws *ws, void *value); +// +void s_tree_ocp_qcqp_ipm_get_status(struct s_tree_ocp_qcqp_ipm_ws *ws, int *status); +// +void s_tree_ocp_qcqp_ipm_get_iter(struct s_tree_ocp_qcqp_ipm_ws *ws, int *iter); +// +void s_tree_ocp_qcqp_ipm_get_max_res_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_stat); +// +void s_tree_ocp_qcqp_ipm_get_max_res_eq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_eq); +// +void s_tree_ocp_qcqp_ipm_get_max_res_ineq(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_ineq); +// +void s_tree_ocp_qcqp_ipm_get_max_res_comp(struct s_tree_ocp_qcqp_ipm_ws *ws, float *res_comp); +// +void s_tree_ocp_qcqp_ipm_get_stat(struct s_tree_ocp_qcqp_ipm_ws *ws, float **stat); +// +void s_tree_ocp_qcqp_ipm_get_stat_m(struct s_tree_ocp_qcqp_ipm_ws *ws, int *stat_m); +// +void s_tree_ocp_qcqp_init_var(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws); +// +void s_tree_ocp_qcqp_ipm_solve(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_ipm_arg *arg, struct s_tree_ocp_qcqp_ipm_ws *ws); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QCQP_IPM_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h new file mode 100644 index 0000000..d0d84a6 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_res.h @@ -0,0 +1,109 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_RES_H_ +#define HPIPM_S_TREE_OCP_QCQP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qcqp_res + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qcqp_res_ws + { + struct blasfeo_svec *tmp_nuxM; // work space of size nuM+nxM + struct blasfeo_svec *tmp_nbgqM; // work space of size nbM+ngM+nqM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *q_fun; // value for evaluation of quadr constr + struct blasfeo_svec *q_adj; // value for adjoint of quadr constr + int use_q_fun; // reuse cached value for evaluation of quadr constr + int use_q_adj; // reuse cached value for adjoint of quadr constr + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_res_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_res_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res *res, void *mem); +// +hpipm_size_t s_tree_ocp_qcqp_res_ws_memsize(struct s_tree_ocp_qcqp_dim *ocp_dim); +// +void s_tree_ocp_qcqp_res_ws_create(struct s_tree_ocp_qcqp_dim *ocp_dim, struct s_tree_ocp_qcqp_res_ws *ws, void *mem); +// +void s_tree_ocp_qcqp_res_compute(struct s_tree_ocp_qcqp *qp, struct s_tree_ocp_qcqp_sol *qp_sol, struct s_tree_ocp_qcqp_res *res, struct s_tree_ocp_qcqp_res_ws *ws); +// +void s_tree_ocp_qcqp_res_compute_inf_norm(struct s_tree_ocp_qcqp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QCQP_RES_H_ + + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h new file mode 100644 index 0000000..47f038c --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_sol.h @@ -0,0 +1,97 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_SOL_H_ +#define HPIPM_S_TREE_OCP_QCQP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_tree_ocp_qcqp_sol + { + struct s_tree_ocp_qcqp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qcqp_sol_memsize(struct s_tree_ocp_qcqp_dim *dim); +// +void s_tree_ocp_qcqp_sol_create(struct s_tree_ocp_qcqp_dim *dim, struct s_tree_ocp_qcqp_sol *qp_sol, void *memory); +// +void s_tree_ocp_qcqp_sol_get_u(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_x(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_sl(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_su(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_pi(int edge, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_lb(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_ub(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_lg(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qcqp_sol_get_lam_ug(int node, struct s_tree_ocp_qcqp_sol *qp_sol, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_SOL_H_ + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h new file mode 100644 index 0000000..79528de --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qcqp_utils.h @@ -0,0 +1,85 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QCQP_UTILS_H_ +#define HPIPM_S_TREE_OCP_QCQP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qcqp_dim.h" +#include "hpipm_s_tree_ocp_qcqp.h" +#include "hpipm_s_tree_ocp_qcqp_sol.h" +#include "hpipm_s_tree_ocp_qcqp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_tree_ocp_qcqp_dim_print(struct s_tree_ocp_qcqp_dim *qp_dim); +// +//void s_tree_ocp_qcqp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim); +// +void s_tree_ocp_qcqp_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp); +// +//void s_tree_ocp_qcqp_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp *qp); +// +void s_tree_ocp_qcqp_sol_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_sol *ocp_qcqp_sol); +// +void s_tree_ocp_qcqp_ipm_arg_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +//void s_tree_ocp_qcqp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_ipm_arg *arg); +// +void s_tree_ocp_qcqp_res_print(struct s_tree_ocp_qcqp_dim *qp_dim, struct s_tree_ocp_qcqp_res *ocp_qcqp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QCQP_UTILS_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h new file mode 100644 index 0000000..722b930 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp.h @@ -0,0 +1,196 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_H_ +#define HPIPM_S_TREE_OCP_QP_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_smat *BAbt; // Nn-1 + struct blasfeo_smat *RSQrq; // Nn + struct blasfeo_smat *DCt; // Nn + struct blasfeo_svec *b; // Nn-1 + struct blasfeo_svec *rqz; // Nn + struct blasfeo_svec *d; // Nn + struct blasfeo_svec *d_mask; // Nn + struct blasfeo_svec *m; // Nn + struct blasfeo_svec *Z; // Nn + int **idxb; // indices of box constrained variables within [u; x] // Nn +// int **idxs; // index of soft constraints + int **idxs_rev; // index of soft constraints + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qp_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp *qp, void *memory); +// +void s_tree_ocp_qp_set_all(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **d_lb, float **d_ub, float **C, float **D, float **d_lg, float **d_ug, float **Zl, float **Zu, float **zl, float **zu, int **idxs, float **d_ls, float **d_us, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set(char *field_name, int node_edge, void *value, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_A(int edge, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_B(int edge, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_b(int edge, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Q(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_S(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_R(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_q(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_r(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lb(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lb_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ub(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ub_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbx_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubx_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lbu_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ubu_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxb(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxbx(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxbu(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_C(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_D(int node, float *mat, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lg(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lg_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ug(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_ug_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Zl(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Zu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_zl(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_zu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lls(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lls_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lus(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_lus_mask(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxs(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_idxs_rev(int node, int *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsbu(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsbx(int node, float *vec, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_set_Jsg(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxe(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxbxe(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxbue(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_idxge(int node, int *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jbxe(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jbue(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_Jge(int node, float *vec, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_set_diag_H_flag(int node, int *value, struct s_tree_ocp_qp *qp); + + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h new file mode 100644 index 0000000..90df571 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_dim.h @@ -0,0 +1,111 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_DIM_H_ +#define HPIPM_S_TREE_OCP_QP_DIM_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_dim + { + struct tree *ttree; // tree describing node conndection + int *nx; // number of states // Nn + int *nu; // number of inputs // Nn + int *nb; // number of box constraints // Nn + int *nbx; // number of state box constraints // Nn + int *nbu; // number of input box constraints // Nn + int *ng; // number of general constraints // Nn + int *ns; // number of soft constraints // Nn + int *nsbx; // number of soft state box constraints + int *nsbu; // number of soft input box constraints + int *nsg; // number of soft general constraints + int Nn; // number of nodes + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_dim_strsize(); +// +hpipm_size_t s_tree_ocp_qp_dim_memsize(int Nn); +// +void s_tree_ocp_qp_dim_create(int Nn, struct s_tree_ocp_qp_dim *qp_dim, void *memory); +// +void s_tree_ocp_qp_dim_set_all(struct tree *ttree, int *nx, int *nu, int *nbx, int *nbu, int *ng, int *nsbx, int *nsbu, int *nsg, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_tree(struct tree *ttree, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set(char *field, int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_ng(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_ns(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsbx(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsbu(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nsg(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbxe(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nbue(int stage, int value, struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_dim_set_nge(int stage, int value, struct s_tree_ocp_qp_dim *dim); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_DIM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h new file mode 100644 index 0000000..f8c26d3 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_ipm.h @@ -0,0 +1,208 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + + +#ifndef HPIPM_S_TREE_OCP_QP_IPM_H_ +#define HPIPM_S_TREE_OCP_QP_IPM_H_ + + + +#include +#include + +#include +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_ipm_arg + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + float tau_min; // min value of barrier parameter + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol, 2 warm start primal and dual sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol_eq; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int split_step; // use different steps for primal and dual variables + int t_lam_min; // clip t and lam: 0 no, 1 in Gamma computation, 2 in solution + int mode; + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qp_ipm_ws + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_tree_ocp_qp_res_ws *res_workspace; + struct s_tree_ocp_qp_sol *sol_step; + struct s_tree_ocp_qp_sol *sol_itref; + struct s_tree_ocp_qp *qp_step; + struct s_tree_ocp_qp *qp_itref; + struct s_tree_ocp_qp_res *res_itref; + struct s_tree_ocp_qp_res *res; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbgM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_smat *L; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int stat_m; // number of recorded stat per IPM iter + int use_Pb; + int status; // solver status + int lq_fact; // cache from arg + int mask_constr; // use constr mask + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_ipm_arg_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_ipm_arg_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, void *mem); +// +void s_tree_ocp_qp_ipm_arg_set_default(enum hpipm_mode mode, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_iter_max(int *iter_max, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_alpha_min(float *alpha_min, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_mu0(float *mu0, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_stat(float *tol_stat, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_eq(float *tol_eq, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_ineq(float *tol_ineq, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tol_comp(float *tol_comp, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_reg_prim(float *reg, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_warm_start(int *warm_start, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_pred_corr(int *pred_corr, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_cond_pred_corr(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_comp_dual_sol_eq(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_comp_res_exit(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_lam_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_t_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_tau_min(float *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_split_step(int *value, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_arg_set_t_lam_min(int *value, struct s_tree_ocp_qp_ipm_arg *arg); + +// +hpipm_size_t s_tree_ocp_qp_ipm_ws_memsize(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_ipm_ws_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws, void *mem); +// +void s_tree_ocp_qp_ipm_get_status(struct s_tree_ocp_qp_ipm_ws *ws, int *status); +// +void s_tree_ocp_qp_ipm_get_iter(struct s_tree_ocp_qp_ipm_ws *ws, int *iter); +// +void s_tree_ocp_qp_ipm_get_max_res_stat(struct s_tree_ocp_qp_ipm_ws *ws, float *res_stat); +// +void s_tree_ocp_qp_ipm_get_max_res_eq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_eq); +// +void s_tree_ocp_qp_ipm_get_max_res_ineq(struct s_tree_ocp_qp_ipm_ws *ws, float *res_ineq); +// +void s_tree_ocp_qp_ipm_get_max_res_comp(struct s_tree_ocp_qp_ipm_ws *ws, float *res_comp); +// +void s_tree_ocp_qp_ipm_get_stat(struct s_tree_ocp_qp_ipm_ws *ws, float **stat); +// +void s_tree_ocp_qp_ipm_get_stat_m(struct s_tree_ocp_qp_ipm_ws *ws, int *stat_m); +// +void s_tree_ocp_qp_init_var(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_abs_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_delta_step(int kk, struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_ipm_solve(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_IPM_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h new file mode 100644 index 0000000..bcf7354 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_kkt.h @@ -0,0 +1,54 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifdef __cplusplus +extern "C" { +#endif + +// +void s_tree_ocp_qp_fact_solve_kkt_unconstr(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_fact_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_fact_lq_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); +// +void s_tree_ocp_qp_solve_kkt_step(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_ipm_arg *arg, struct s_tree_ocp_qp_ipm_ws *ws); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h new file mode 100644 index 0000000..5c67972 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_res.h @@ -0,0 +1,107 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_RES_H_ +#define HPIPM_S_TREE_OCP_QP_RES_H_ + + + +#include +#include + +#include +#include +#include +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct s_tree_ocp_qp_res + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_svec *res_g; // q-residuals + struct blasfeo_svec *res_b; // b-residuals + struct blasfeo_svec *res_d; // d-residuals + struct blasfeo_svec *res_m; // m-residuals + float res_max[4]; // max of residuals + float res_mu; // mu-residual + hpipm_size_t memsize; + }; + + + +struct s_tree_ocp_qp_res_ws + { + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t s_tree_ocp_qp_res_memsize(struct s_tree_ocp_qp_dim *ocp_dim); +// +void s_tree_ocp_qp_res_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res *res, void *mem); +// +hpipm_size_t s_tree_ocp_qp_res_ws_memsize(struct s_tree_ocp_qp_dim *ocp_dim); +// +void s_tree_ocp_qp_res_ws_create(struct s_tree_ocp_qp_dim *ocp_dim, struct s_tree_ocp_qp_res_ws *ws, void *mem); +// +void s_tree_ocp_qp_res_get_all(struct s_tree_ocp_qp_res *res, float **res_r, float **res_q, float **res_ls, float **res_us, float **res_b, float **res_d_lb, float **res_d_ub, float **res_d_lg, float **res_d_ug, float **res_d_ls, float **res_d_us, float **res_m_lb, float **res_m_ub, float **res_m_lg, float **res_m_ug, float **res_m_ls, float **res_m_us); +// +void s_tree_ocp_qp_res_compute(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws); +// +void s_tree_ocp_qp_res_compute_lin(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, struct s_tree_ocp_qp_sol *qp_step, struct s_tree_ocp_qp_res *res, struct s_tree_ocp_qp_res_ws *ws); +// +void s_tree_ocp_qp_res_compute_inf_norm(struct s_tree_ocp_qp_res *res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_S_TREE_OCP_QP_RES_H_ + + + diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h new file mode 100644 index 0000000..71e8876 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_sol.h @@ -0,0 +1,98 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_SOL_H_ +#define HPIPM_S_TREE_OCP_QP_SOL_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + +struct s_tree_ocp_qp_sol + { + struct s_tree_ocp_qp_dim *dim; + struct blasfeo_svec *ux; + struct blasfeo_svec *pi; + struct blasfeo_svec *lam; + struct blasfeo_svec *t; + void *misc; + hpipm_size_t memsize; // memory size in bytes + }; + + + +// +hpipm_size_t s_tree_ocp_qp_sol_memsize(struct s_tree_ocp_qp_dim *dim); +// +void s_tree_ocp_qp_sol_create(struct s_tree_ocp_qp_dim *dim, struct s_tree_ocp_qp_sol *qp_sol, void *memory); +// +void s_tree_ocp_qp_sol_get_all(struct s_tree_ocp_qp *qp, struct s_tree_ocp_qp_sol *qp_sol, float **u, float **x, float **ls, float **us, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug, float **lam_ls, float **lam_us); +// +void s_tree_ocp_qp_sol_get_u(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_x(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_sl(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_su(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_pi(int edge, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_lb(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_ub(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_lg(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); +// +void s_tree_ocp_qp_sol_get_lam_ug(int node, struct s_tree_ocp_qp_sol *qp_sol, float *vec); + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_SOL_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h new file mode 100644 index 0000000..ec17475 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_s_tree_ocp_qp_utils.h @@ -0,0 +1,84 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_S_TREE_OCP_QP_UTILS_H_ +#define HPIPM_S_TREE_OCP_QP_UTILS_H_ + + + +#include +#include + +#include "hpipm_s_tree_ocp_qp_dim.h" +#include "hpipm_s_tree_ocp_qp.h" +#include "hpipm_s_tree_ocp_qp_sol.h" +#include "hpipm_s_tree_ocp_qp_ipm.h" + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +// +void s_tree_ocp_qp_dim_print(struct s_tree_ocp_qp_dim *qp_dim); +// +//void s_tree_ocp_qp_dim_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim); +// +void s_tree_ocp_qp_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp); +// +//void s_tree_ocp_qp_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp *qp); +// +void s_tree_ocp_qp_sol_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_sol *ocp_qp_sol); +// +void s_tree_ocp_qp_ipm_arg_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +//void s_tree_ocp_qp_ipm_arg_codegen(char *file_name, char *mode, struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_ipm_arg *arg); +// +void s_tree_ocp_qp_res_print(struct s_tree_ocp_qp_dim *qp_dim, struct s_tree_ocp_qp_res *ocp_qp_res); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + + +#endif // HPIPM_S_TREE_OCP_QP_UTILS_H_ + + diff --git a/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h b/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h new file mode 100644 index 0000000..a3c77a6 --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_scenario_tree.h @@ -0,0 +1,70 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_SCENARIO_TREE_H_ +#define HPIPM_SCENARIO_TREE_H_ + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct sctree + { + struct node *root; // pointer to root + int *kids; // pointer to array of kids + int Nn; // numer of nodes + int md; // number of realizations + int Nr; // robust horizion + int Nh; // control horizion + hpipm_size_t memsize; + }; + + + +// +hpipm_size_t sctree_memsize(int md, int Nr, int Nh); +// +void sctree_create(int md, int Nr, int Nh, struct sctree *st, void *memory); +// +void sctree_cast_to_tree(struct sctree *st, struct tree *tt); + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_SCENARIO_TREE_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_timing.h b/third_party/acados/include/hpipm/include/hpipm_timing.h new file mode 100644 index 0000000..bd0f2db --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_timing.h @@ -0,0 +1,67 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + +#ifndef HPIPM_TIMING_H_ +#define HPIPM_TIMING_H_ + + + +#include + + + +#ifdef __cplusplus +extern "C" { +#endif + + + +typedef blasfeo_timer hpipm_timer; + + + +// +void hpipm_tic(hpipm_timer *t); +// +double hpipm_toc(hpipm_timer *t); + + + +#ifdef __cplusplus +} // #extern "C" +#endif + + +#endif // HPIPM_TIMING_H_ diff --git a/third_party/acados/include/hpipm/include/hpipm_tree.h b/third_party/acados/include/hpipm/include/hpipm_tree.h new file mode 100644 index 0000000..f9d69ec --- /dev/null +++ b/third_party/acados/include/hpipm/include/hpipm_tree.h @@ -0,0 +1,76 @@ +/************************************************************************************************** +* * +* This file is part of HPIPM. * +* * +* HPIPM -- High-Performance Interior Point Method. * +* Copyright (C) 2019 by Gianluca Frison. * +* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. * +* All rights reserved. * +* * +* The 2-Clause BSD License * +* * +* Redistribution and use in source and binary forms, with or without * +* modification, are permitted provided that the following conditions are met: * +* * +* 1. Redistributions of source code must retain the above copyright notice, this * +* list of conditions and the following disclaimer. * +* 2. Redistributions in binary form must reproduce the above copyright notice, * +* this list of conditions and the following disclaimer in the documentation * +* and/or other materials provided with the distribution. * +* * +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND * +* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED * +* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * +* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR * +* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES * +* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; * +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND * +* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * +* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS * +* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * +* * +* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de * +* * +**************************************************************************************************/ + + +#ifndef HPIPM_TREE_H_ +#define HPIPM_TREE_H_ + +#include "hpipm_common.h" + +#ifdef __cplusplus +extern "C" { +#endif + + + +struct node + { + int *kids; // 64 bits + int idx; // 32 bits + int dad; // 32 bits + int nkids; // 32 bits + int stage; // 32 bits + int real; // 32 bits + int idxkid; // 32 bits // XXX needed ??? + // total 256 bits + }; + + + +struct tree + { + struct node *root; // pointer to root + int *kids; // pointer to array of kids + int Nn; // numer of nodes + hpipm_size_t memsize; + }; + + + +#ifdef __cplusplus +} /* extern "C" */ +#endif + +#endif // HPIPM_TREE_H_ diff --git a/third_party/acados/include/qpOASES_e/Bounds.h b/third_party/acados/include/qpOASES_e/Bounds.h new file mode 100644 index 0000000..4e41c1d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Bounds.h @@ -0,0 +1,543 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Bounds.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Bounds class designed to manage working sets of + * bounds within a QProblem. + */ + + +#ifndef QPOASES_BOUNDS_H +#define QPOASES_BOUNDS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages working sets of bounds (= box constraints). + * + * This class manages working sets of bounds (= box constraints) + * by storing index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Indexlist *freee; /**< Index list of free variables. */ + Indexlist *fixed; /**< Index list of fixed variables. */ + + Indexlist *shiftedFreee; /**< Memory for shifting free variables. */ + Indexlist *shiftedFixed; /**< Memory for shifting fixed variables. */ + + Indexlist *rotatedFreee; /**< Memory for rotating free variables. */ + Indexlist *rotatedFixed; /**< Memory for rotating fixed variables. */ + + SubjectToType *type; /**< Type of bounds. */ + SubjectToStatus *status; /**< Status of bounds. */ + + SubjectToType *typeTmp; /**< Temp memory for type of bounds. */ + SubjectToStatus *statusTmp; /**< Temp memory for status of bounds. */ + + BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */ + BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */ + + int n; /**< Total number of bounds. */ +} Bounds; + +int Bounds_calculateMemorySize( int n); + +char *Bounds_assignMemory(int n, Bounds **mem, void *raw_memory); + +Bounds *Bounds_createMemory( int n ); + +/** Constructor which takes the number of bounds. */ +void BoundsCON( Bounds* _THIS, + int _n /**< Number of bounds. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void BoundsCPY( Bounds* FROM, + Bounds* TO + ); + + +/** Initialises object with given number of bounds. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_init( Bounds* _THIS, + int _n /**< Number of bounds. */ + ); + + +/** Initially adds number of a new (i.e. not yet in the list) bound to + * given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_setupBound( Bounds* _THIS, + int number, /**< Number of new bound. */ + SubjectToStatus _status /**< Status of new bound. */ + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of free bounds; the order depends on the SujectToType + * of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllFree( Bounds* _THIS + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of fixed bounds (on their lower bounds); + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllLower( Bounds* _THIS + ); + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set of fixed bounds (on their upper bounds); + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAllUpper( Bounds* _THIS + ); + + +/** Moves index of a bound from index list of fixed to that of free bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_moveFixedToFree( Bounds* _THIS, + int number /**< Number of bound to be freed. */ + ); + +/** Moves index of a bound from index list of free to that of fixed bounds. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_moveFreeToFixed( Bounds* _THIS, + int number, /**< Number of bound to be fixed. */ + SubjectToStatus _status /**< Status of bound to be fixed. */ + ); + +/** Flip fixed bound. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_BOUND_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Bounds_flipFixed( Bounds* _THIS, + int number + ); + +/** Swaps the indices of two free bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED */ +returnValue Bounds_swapFree( Bounds* _THIS, + int number1, /**< Number of first bound. */ + int number2 /**< Number of second bound. */ + ); + + +/** Returns number of variables. + * \return Number of variables. */ +static inline int Bounds_getNV( Bounds* _THIS + ); + +/** Returns number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int Bounds_getNFV( Bounds* _THIS + ); + +/** Returns number of bounded (but possibly free) variables. + * \return Number of bounded (but possibly free) variables. */ +static inline int Bounds_getNBV( Bounds* _THIS + ); + +/** Returns number of unbounded variables. + * \return Number of unbounded variables. */ +static inline int Bounds_getNUV( Bounds* _THIS + ); + +/** Returns number of free variables. + * \return Number of free variables. */ +static inline int Bounds_getNFR( Bounds* _THIS + ); + +/** Returns number of fixed variables. + * \return Number of fixed variables. */ +static inline int Bounds_getNFX( Bounds* _THIS + ); + + +/** Returns a pointer to free variables index list. + * \return Pointer to free variables index list. */ +static inline Indexlist* Bounds_getFree( Bounds* _THIS + ); + +/** Returns a pointer to fixed variables index list. + * \return Pointer to fixed variables index list. */ +static inline Indexlist* Bounds_getFixed( Bounds* _THIS + ); + + +/** Returns number of bounds with given SubjectTo type. + * \return Number of bounds with given type. */ +static inline int Bounds_getNumberOfType( Bounds* _THIS, + SubjectToType _type /**< Type of bound. */ + ); + + +/** Returns type of bound. + * \return Type of bound \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline SubjectToType Bounds_getType( Bounds* _THIS, + int i /**< Number of bound. */ + ); + +/** Returns status of bound. + * \return Status of bound \n + ST_UNDEFINED */ +static inline SubjectToStatus Bounds_getStatus( Bounds* _THIS, + int i /**< Number of bound. */ + ); + + +/** Sets type of bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Bounds_setType( Bounds* _THIS, + int i, /**< Number of bound. */ + SubjectToType value /**< Type of bound. */ + ); + +/** Sets status of bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Bounds_setStatus( Bounds* _THIS, + int i, /**< Number of bound. */ + SubjectToStatus value /**< Status of bound. */ + ); + + +/** Sets status of lower bounds. */ +static inline void Bounds_setNoLower( Bounds* _THIS, + BooleanType _status /**< Status of lower bounds. */ + ); + +/** Sets status of upper bounds. */ +static inline void Bounds_setNoUpper( Bounds* _THIS, + BooleanType _status /**< Status of upper bounds. */ + ); + + +/** Returns status of lower bounds. + * \return BT_TRUE if there is no lower bound on any variable. */ +static inline BooleanType Bounds_hasNoLower( Bounds* _THIS + ); + +/** Returns status of upper bounds. + * \return BT_TRUE if there is no upper bound on any variable. */ +static inline BooleanType Bounds_hasNoUpper( Bounds* _THIS + ); + + +/** Shifts forward type and status of all bounds by a given + * offset. This offset has to lie within the range [0,n/2] and has to + * be an integer divisor of the total number of bounds n. + * Type and status of the first \ bounds is thrown away, + * type and status of the last \ bounds is doubled, + * e.g. for offset = 2: \n + * shift( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b5,b6} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS \n + RET_SHIFTING_FAILED */ +returnValue Bounds_shift( Bounds* _THIS, + int offset /**< Shift offset within the range [0,n/2] and integer divisor of n. */ + ); + +/** Rotates forward type and status of all bounds by a given + * offset. This offset has to lie within the range [0,n]. + * Example for offset = 2: \n + * rotate( {b1,b2,b3,b4,b5,b6} ) = {b3,b4,b5,b6,b1,b2} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_ROTATING_FAILED */ +returnValue Bounds_rotate( Bounds* _THIS, + int offset /**< Rotation offset within the range [0,n]. */ + ); + + +/** Prints information on bounds object + * (in particular, lists of free and fixed bounds. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Bounds_print( Bounds* _THIS + ); + + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set corresponding to the desired status; + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_BOUND_FAILED */ +returnValue Bounds_setupAll( Bounds* _THIS, + SubjectToStatus _status /**< Desired initial status for all bounds. */ + ); + + +/** Adds the index of a new bound to index set. + * \return SUCCESSFUL_RETURN \n + RET_ADDINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_addIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list to which the new index shall be added. */ + int newnumber, /**< Number of new bound. */ + SubjectToStatus newstatus /**< Status of new bound. */ + ); + +/** Removes the index of a bound from index set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVEINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_removeIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */ + int removenumber /**< Number of bound to be removed. */ + ); + +/** Swaps the indices of two constraints or bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Bounds_swapIndex( Bounds* _THIS, + Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */ + int number1, /**< Number of first bound. */ + int number2 /**< Number of second bound. */ + ); + + + +/* + * g e t N u m b e r O f T y p e + */ +static inline int Bounds_getNumberOfType( Bounds* _THIS, SubjectToType _type ) +{ + int i; + int numberOfType = 0; + + if ( _THIS->type != 0 ) + { + for( i=0; i<_THIS->n; ++i ) + if ( _THIS->type[i] == _type ) + ++numberOfType; + } + + return numberOfType; +} + + +/* + * g e t T y p e + */ +static inline SubjectToType Bounds_getType( Bounds* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->type[i]; + + return ST_UNKNOWN; +} + + +/* + * g e t S t a t u s + */ +static inline SubjectToStatus Bounds_getStatus( Bounds* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->status[i]; + + return ST_UNDEFINED; +} + + +/* + * s e t T y p e + */ +static inline returnValue Bounds_setType( Bounds* _THIS, int i, SubjectToType value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->type[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t S t a t u s + */ +static inline returnValue Bounds_setStatus( Bounds* _THIS, int i, SubjectToStatus value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->status[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t N o L o w e r + */ +static inline void Bounds_setNoLower( Bounds* _THIS, BooleanType _status ) +{ + _THIS->noLower = _status; +} + + +/* + * s e t N o U p p e r + */ +static inline void Bounds_setNoUpper( Bounds* _THIS, BooleanType _status ) +{ + _THIS->noUpper = _status; +} + + +/* + * h a s N o L o w e r + */ +static inline BooleanType Bounds_hasNoLower( Bounds* _THIS ) +{ + return _THIS->noLower; +} + + +/* + * h a s N o U p p p e r + */ +static inline BooleanType Bounds_hasNoUpper( Bounds* _THIS ) +{ + return _THIS->noUpper; +} + + + +/* + * g e t N V + */ +static inline int Bounds_getNV( Bounds* _THIS ) +{ + return _THIS->n; +} + + +/* + * g e t N F V + */ +static inline int Bounds_getNFV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_EQUALITY ); +} + + +/* + * g e t N B V + */ +static inline int Bounds_getNBV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_BOUNDED ); +} + + +/* + * g e t N U V + */ +static inline int Bounds_getNUV( Bounds* _THIS ) +{ + return Bounds_getNumberOfType( _THIS,ST_UNBOUNDED ); +} + + +/* + * g e t N F R + */ +static inline int Bounds_getNFR( Bounds* _THIS ) +{ + return Indexlist_getLength( _THIS->freee ); +} + + +/* + * g e t N F X + */ +static inline int Bounds_getNFX( Bounds* _THIS ) +{ + return Indexlist_getLength( _THIS->fixed ); +} + + +/* + * g e t F r e e + */ +static inline Indexlist* Bounds_getFree( Bounds* _THIS ) +{ + return _THIS->freee; +} + + +/* + * g e t F i x e d + */ +static inline Indexlist* Bounds_getFixed( Bounds* _THIS ) +{ + return _THIS->fixed; +} + + +END_NAMESPACE_QPOASES + +#endif /* QPOASES_BOUNDS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Constants.h b/third_party/acados/include/qpOASES_e/Constants.h new file mode 100644 index 0000000..13c777d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Constants.h @@ -0,0 +1,134 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Constants.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Definition of all global constants. + */ + + +#ifndef QPOASES_CONSTANTS_H +#define QPOASES_CONSTANTS_H + + +#include + +#ifdef __CODE_GENERATION__ + + #define CONVERTTOSTRINGAUX(x) #x + #define CONVERTTOSTRING(x) CONVERTTOSTRINGAUX(x) + + #ifndef QPOASES_CUSTOM_INTERFACE + #include "acado_qpoases3_interface.h" + #else + #include CONVERTTOSTRING(QPOASES_CUSTOM_INTERFACE) + #endif + +#endif + + +BEGIN_NAMESPACE_QPOASES + + +#ifndef __EXTERNAL_DIMENSIONS__ + + /*#define QPOASES_NVMAX 50 + #define QPOASES_NCMAX 100*/ + #define QPOASES_NVMAX 287 + #define QPOASES_NCMAX 709 + +#endif /* __EXTERNAL_DIMENSIONS__ */ + + +/** Maximum number of variables within a QP formulation. + * Note: this value has to be positive! */ +#define NVMAX QPOASES_NVMAX + +/** Maximum number of constraints within a QP formulation. + * Note: this value has to be positive! */ +#define NCMAX QPOASES_NCMAX + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMAX QPOASES_NVMAX +#else +#define NVCMAX QPOASES_NCMAX +#endif + +#if ( QPOASES_NVMAX > QPOASES_NCMAX ) +#define NVCMIN QPOASES_NCMAX +#else +#define NVCMIN QPOASES_NVMAX +#endif + + +/** Maximum number of QPs in a sequence solved by means of the OQP interface. + * Note: this value has to be positive! */ +#define NQPMAX 1000 + + +/** Numerical value of machine precision (min eps, s.t. 1+eps > 1). + * Note: this value has to be positive! */ +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + static const real_t QPOASES_EPS = 1.193e-07; + #else + static const real_t QPOASES_EPS = 2.221e-16; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ + + +/** Numerical value of zero (for situations in which it would be + * unreasonable to compare with 0.0). + * Note: this value has to be positive! */ +static const real_t QPOASES_ZERO = 1.0e-25; + +/** Numerical value of infinity (e.g. for non-existing bounds). + * Note: this value has to be positive! */ +static const real_t QPOASES_INFTY = 1.0e20; + +/** Tolerance to used for isEqual, isZero etc. + * Note: this value has to be positive! */ +static const real_t QPOASES_TOL = 1.0e-25; + + +/** Maximum number of characters within a string. + * Note: this value should be at least 41! */ +#define QPOASES_MAX_STRING_LENGTH 160 + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_CONSTANTS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/ConstraintProduct.h b/third_party/acados/include/qpOASES_e/ConstraintProduct.h new file mode 100644 index 0000000..eb5400c --- /dev/null +++ b/third_party/acados/include/qpOASES_e/ConstraintProduct.h @@ -0,0 +1,62 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/ConstraintProduct.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to D. Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + * + * Declaration of the ConstraintProduct interface which allows to specify a + * user-defined function for evaluating the constraint product at the + * current iterate to speed-up QP solution in case of a specially structured + * constraint matrix. + */ + + + +#ifndef QPOASES_CONSTRAINT_PRODUCT_H +#define QPOASES_CONSTRAINT_PRODUCT_H + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interface for specifying user-defined evaluations of constraint products. + * + * An interface which allows to specify a user-defined function for evaluating the + * constraint product at the current iterate to speed-up QP solution in case + * of a specially structured constraint matrix. + * + * \author Hans Joachim Ferreau (thanks to Kwame Minde Kufoalor) + * \version 3.1embedded + * \date 2009-2015 + */ +typedef int(*ConstraintProduct)( int, const real_t* const, real_t* const ); + + +END_NAMESPACE_QPOASES + +#endif /* QPOASES_CONSTRAINT_PRODUCT_H */ diff --git a/third_party/acados/include/qpOASES_e/Constraints.h b/third_party/acados/include/qpOASES_e/Constraints.h new file mode 100644 index 0000000..8aca10d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Constraints.h @@ -0,0 +1,535 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Constraints.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Constraints class designed to manage working sets of + * constraints within a QProblem. + */ + + +#ifndef QPOASES_CONSTRAINTS_H +#define QPOASES_CONSTRAINTS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages working sets of constraints. + * + * This class manages working sets of constraints by storing + * index sets and other status information. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Indexlist *active; /**< Index list of active constraints. */ + Indexlist *inactive; /**< Index list of inactive constraints. */ + + Indexlist *shiftedActive; /**< Memory for shifting active constraints. */ + Indexlist *shiftedInactive; /**< Memory for shifting inactive constraints. */ + + Indexlist *rotatedActive; /**< Memory for rotating active constraints. */ + Indexlist *rotatedInactive; /**< Memory for rotating inactive constraints. */ + + SubjectToType *type; /**< Type of constraints. */ + SubjectToStatus *status; /**< Status of constraints. */ + + SubjectToType *typeTmp; /**< Temp memory for type of constraints. */ + SubjectToStatus *statusTmp; /**< Temp memory for status of constraints. */ + + BooleanType noLower; /**< This flag indicates if there is no lower bound on any variable. */ + BooleanType noUpper; /**< This flag indicates if there is no upper bound on any variable. */ + + int n; /**< Total number of constraints. */ +} Constraints; + +int Constraints_calculateMemorySize( int n); + +char *Constraints_assignMemory(int n, Constraints **mem, void *raw_memory); + +Constraints *Constraints_createMemory( int n ); + +/** Constructor which takes the number of constraints. */ +void ConstraintsCON( Constraints* _THIS, + int _n /**< Number of constraints. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void ConstraintsCPY( Constraints* FROM, + Constraints* TO + ); + + +/** Initialises object with given number of constraints. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_init( Constraints* _THIS, + int _n /**< Number of constraints. */ + ); + + +/** Initially adds number of a new (i.e. not yet in the list) constraint to + * a given index set. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_setupConstraint( Constraints* _THIS, + int number, /**< Number of new constraint. */ + SubjectToStatus _status /**< Status of new constraint. */ + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of inactive constraints; the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllInactive( Constraints* _THIS + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of active constraints (on their lower bounds); the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllLower( Constraints* _THIS + ); + +/** Initially adds all enabled numbers of new (i.e. not yet in the list) constraints to + * to the index set of active constraints (on their upper bounds); the order depends on the SujectToType + * of each index. Only disabled constraints are added to index set of disabled constraints! + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAllUpper( Constraints* _THIS + ); + + +/** Moves index of a constraint from index list of active to that of inactive constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ +returnValue Constraints_moveActiveToInactive( Constraints* _THIS, + int number /**< Number of constraint to become inactive. */ + ); + +/** Moves index of a constraint from index list of inactive to that of active constraints. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED */ +returnValue Constraints_moveInactiveToActive( Constraints* _THIS, + int number, /**< Number of constraint to become active. */ + SubjectToStatus _status /**< Status of constraint to become active. */ + ); + +/** Flip fixed constraint. + * \return SUCCESSFUL_RETURN \n + RET_MOVING_CONSTRAINT_FAILED \n + RET_INDEX_OUT_OF_BOUNDS */ +returnValue Constraints_flipFixed( Constraints* _THIS, + int number + ); + + +/** Returns the number of constraints. + * \return Number of constraints. */ +static inline int Constraints_getNC( Constraints* _THIS + ); + +/** Returns the number of implicit equality constraints. + * \return Number of implicit equality constraints. */ +static inline int Constraints_getNEC( Constraints* _THIS + ); + +/** Returns the number of "real" inequality constraints. + * \return Number of "real" inequality constraints. */ +static inline int Constraints_getNIC( Constraints* _THIS + ); + +/** Returns the number of unbounded constraints (i.e. without any bounds). + * \return Number of unbounded constraints (i.e. without any bounds). */ +static inline int Constraints_getNUC( Constraints* _THIS + ); + +/** Returns the number of active constraints. + * \return Number of active constraints. */ +static inline int Constraints_getNAC( Constraints* _THIS + ); + +/** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ +static inline int Constraints_getNIAC( Constraints* _THIS + ); + + +/** Returns a pointer to active constraints index list. + * \return Pointer to active constraints index list. */ +static inline Indexlist* Constraints_getActive( Constraints* _THIS + ); + +/** Returns a pointer to inactive constraints index list. + * \return Pointer to inactive constraints index list. */ +static inline Indexlist* Constraints_getInactive( Constraints* _THIS + ); + + +/** Returns number of constraints with given SubjectTo type. + * \return Number of constraints with given type. */ +static inline int Constraints_getNumberOfType( Constraints* _THIS, + SubjectToType _type /**< Type of constraints' bound. */ + ); + + +/** Returns type of constraints' bound. + * \return Type of constraints' bound \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline SubjectToType Constraints_getType( Constraints* _THIS, + int i /**< Number of constraints' bound. */ + ); + +/** Returns status of constraints' bound. + * \return Status of constraints' bound \n + ST_UNDEFINED */ +static inline SubjectToStatus Constraints_getStatus( Constraints* _THIS, + int i /**< Number of constraints' bound. */ + ); + + +/** Sets type of constraints' bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Constraints_setType( Constraints* _THIS, + int i, /**< Number of constraints' bound. */ + SubjectToType value /**< Type of constraints' bound. */ + ); + +/** Sets status of constraints' bound. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue Constraints_setStatus( Constraints* _THIS, + int i, /**< Number of constraints' bound. */ + SubjectToStatus value /**< Status of constraints' bound. */ + ); + + +/** Sets status of lower constraints' bounds. */ +static inline void Constraints_setNoLower( Constraints* _THIS, + BooleanType _status /**< Status of lower constraints' bounds. */ + ); + +/** Sets status of upper constraints' bounds. */ +static inline void Constraints_setNoUpper( Constraints* _THIS, + BooleanType _status /**< Status of upper constraints' bounds. */ + ); + + +/** Returns status of lower constraints' bounds. + * \return BT_TRUE if there is no lower constraints' bound on any variable. */ +static inline BooleanType Constraints_hasNoLower( Constraints* _THIS + ); + +/** Returns status of upper bounds. + * \return BT_TRUE if there is no upper constraints' bound on any variable. */ +static inline BooleanType Constraints_hasNoUpper( Constraints* _THIS + ); + + +/** Shifts forward type and status of all constraints by a given + * offset. This offset has to lie within the range [0,n/2] and has to + * be an integer divisor of the total number of constraints n. + * Type and status of the first \ constraints is thrown away, + * type and status of the last \ constraints is doubled, + * e.g. for offset = 2: \n + * shift( {c/b1,c/b2,c/b3,c/b4,c/b5,c/b6} ) = {c/b3,c/b4,c/b5,c/b6,c/b5,c/b6} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_INVALID_ARGUMENTS \n + RET_SHIFTING_FAILED */ +returnValue Constraints_shift( Constraints* _THIS, + int offset /**< Shift offset within the range [0,n/2] and integer divisor of n. */ + ); + +/** Rotates forward type and status of all constraints by a given + * offset. This offset has to lie within the range [0,n]. + * Example for offset = 2: \n + * rotate( {c1,c2,c3,c4,c5,c6} ) = {c3,c4,c5,c6,c1,c2} + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS \n + RET_ROTATING_FAILED */ +returnValue Constraints_rotate( Constraints* _THIS, + int offset /**< Rotation offset within the range [0,n]. */ + ); + + +/** Prints information on constraints object + * (in particular, lists of inactive and active constraints. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Constraints_print( Constraints* _THIS + ); + + +/** Initially adds all numbers of new (i.e. not yet in the list) bounds to + * to the index set corresponding to the desired status; + * the order depends on the SujectToType of each index. + * \return SUCCESSFUL_RETURN \n + RET_SETUP_CONSTRAINT_FAILED */ +returnValue Constraints_setupAll( Constraints* _THIS, + SubjectToStatus _status /**< Desired initial status for all bounds. */ + ); + + +/** Adds the index of a new constraint to index set. + * \return SUCCESSFUL_RETURN \n + RET_ADDINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_addIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list to which the new index shall be added. */ + int newnumber, /**< Number of new constraint. */ + SubjectToStatus newstatus /**< Status of new constraint. */ + ); + +/** Removes the index of a constraint from index set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVEINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_removeIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list from which the new index shall be removed. */ + int removenumber /**< Number of constraint to be removed. */ + ); + +/** Swaps the indices of two constraints or bounds within the index set. + * \return SUCCESSFUL_RETURN \n + RET_SWAPINDEX_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue Constraints_swapIndex( Constraints* _THIS, + Indexlist* const indexlist, /**< Index list in which the indices shold be swapped. */ + int number1, /**< Number of first constraint. */ + int number2 /**< Number of second constraint. */ + ); + + + +/* + * g e t N u m b e r O f T y p e + */ +static inline int Constraints_getNumberOfType( Constraints* _THIS, SubjectToType _type ) +{ + int i; + int numberOfType = 0; + + if ( _THIS->type != 0 ) + { + for( i=0; i<_THIS->n; ++i ) + if ( _THIS->type[i] == _type ) + ++numberOfType; + } + + return numberOfType; +} + + +/* + * g e t T y p e + */ +static inline SubjectToType Constraints_getType( Constraints* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->type[i]; + + return ST_UNKNOWN; +} + + +/* + * g e t S t a t u s + */ +static inline SubjectToStatus Constraints_getStatus( Constraints* _THIS, int i ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + return _THIS->status[i]; + + return ST_UNDEFINED; +} + + +/* + * s e t T y p e + */ +static inline returnValue Constraints_setType( Constraints* _THIS, int i, SubjectToType value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->type[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t S t a t u s + */ +static inline returnValue Constraints_setStatus( Constraints* _THIS, int i, SubjectToStatus value ) +{ + if ( ( i >= 0 ) && ( i < _THIS->n ) ) + { + _THIS->status[i] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t N o L o w e r + */ +static inline void Constraints_setNoLower( Constraints* _THIS, BooleanType _status ) +{ + _THIS->noLower = _status; +} + + +/* + * s e t N o U p p e r + */ +static inline void Constraints_setNoUpper( Constraints* _THIS, BooleanType _status ) +{ + _THIS->noUpper = _status; +} + + +/* + * h a s N o L o w e r + */ +static inline BooleanType Constraints_hasNoLower( Constraints* _THIS ) +{ + return _THIS->noLower; +} + + +/* + * h a s N o U p p p e r + */ +static inline BooleanType Constraints_hasNoUpper( Constraints* _THIS ) +{ + return _THIS->noUpper; +} + + + +/* + * g e t N C + */ +static inline int Constraints_getNC( Constraints* _THIS ) +{ + return _THIS->n; +} + + +/* + * g e t N E C + */ +static inline int Constraints_getNEC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_EQUALITY ); +} + + +/* + * g e t N I C + */ +static inline int Constraints_getNIC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_BOUNDED ); +} + + +/* + * g e t N U C + */ +static inline int Constraints_getNUC( Constraints* _THIS ) +{ + return Constraints_getNumberOfType( _THIS,ST_UNBOUNDED ); +} + + +/* + * g e t N A C + */ +static inline int Constraints_getNAC( Constraints* _THIS ) +{ + return Indexlist_getLength( _THIS->active ); +} + + +/* + * g e t N I A C + */ +static inline int Constraints_getNIAC( Constraints* _THIS ) +{ + return Indexlist_getLength( _THIS->inactive ); +} + + + +/* + * g e t A c t i v e + */ +static inline Indexlist* Constraints_getActive( Constraints* _THIS ) +{ + return _THIS->active; +} + + +/* + * g e t I n a c t i v e + */ +static inline Indexlist* Constraints_getInactive( Constraints* _THIS ) +{ + return _THIS->inactive; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_CONSTRAINTS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Flipper.h b/third_party/acados/include/qpOASES_e/Flipper.h new file mode 100644 index 0000000..63526a3 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Flipper.h @@ -0,0 +1,129 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Flipper.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Options class designed to manage user-specified + * options for solving a QProblem. + */ + + +#ifndef QPOASES_FLIPPER_H +#define QPOASES_FLIPPER_H + + +#include +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Auxiliary class for storing a copy of the current matrix factorisations. + * + * This auxiliary class stores a copy of the current matrix factorisations. It + * is used by the classe QProblemB and QProblem in case flipping bounds are enabled. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + Bounds *bounds; /**< Data structure for problem's bounds. */ + Constraints *constraints; /**< Data structure for problem's constraints. */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + + unsigned int nV; /**< Number of variables. */ + unsigned int nC; /**< Number of constraints. */ +} Flipper; + +int Flipper_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *Flipper_assignMemory( unsigned int nV, unsigned int nC, Flipper **mem, void *raw_memory ); + +Flipper *Flipper_createMemory( unsigned int nV, unsigned int nC ); + +/** Constructor which takes the number of bounds and constraints. */ +void FlipperCON( Flipper* _THIS, + unsigned int _nV, /**< Number of bounds. */ + unsigned int _nC /**< Number of constraints. */ + ); + +/** Copy constructor (deep copy). */ +void FlipperCPY( Flipper* FROM, + Flipper* TO + ); + +/** Initialises object with given number of bounds and constraints. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Flipper_init( Flipper* _THIS, + unsigned int _nV, /**< Number of bounds. */ + unsigned int _nC /**< Number of constraints. */ + ); + + +/** Copies current values to non-null arguments (assumed to be allocated with consistent size). + * \return SUCCESSFUL_RETURN */ +returnValue Flipper_get( Flipper* _THIS, + Bounds* const _bounds, /**< Pointer to new bounds. */ + real_t* const R, /**< New matrix R. */ + Constraints* const _constraints, /**< Pointer to new constraints. */ + real_t* const _Q, /**< New matrix Q. */ + real_t* const _T /**< New matrix T. */ + ); + +/** Assigns new values to non-null arguments. + * \return SUCCESSFUL_RETURN */ +returnValue Flipper_set( Flipper* _THIS, + const Bounds* const _bounds, /**< Pointer to new bounds. */ + const real_t* const _R, /**< New matrix R. */ + const Constraints* const _constraints, /**< Pointer to new constraints. */ + const real_t* const _Q, /**< New matrix Q. */ + const real_t* const _T /**< New matrix T. */ + ); + +/** Returns dimension of matrix T. + * \return Dimension of matrix T. */ +unsigned int Flipper_getDimT( Flipper* _THIS ); + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_FLIPPER_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Indexlist.h b/third_party/acados/include/qpOASES_e/Indexlist.h new file mode 100644 index 0000000..02d259d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Indexlist.h @@ -0,0 +1,221 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Indexlist.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Indexlist class designed to manage index lists of + * constraints and bounds within a SubjectTo object. + */ + + +#ifndef QPOASES_INDEXLIST_H +#define QPOASES_INDEXLIST_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Stores and manages index lists. + * + * This class manages index lists of active/inactive bounds/constraints. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + int *number; /**< Array to store numbers of constraints or bounds. */ + int *iSort; /**< Index list to sort vector \a number */ + + int length; /**< Length of index list. */ + int first; /**< Physical index of first element. */ + int last; /**< Physical index of last element. */ + int lastusedindex; /**< Physical index of last entry in index list. */ + int physicallength; /**< Physical length of index list. */ +} Indexlist; + +int Indexlist_calculateMemorySize( int n); + +char *Indexlist_assignMemory(int n, Indexlist **mem, void *raw_memory); + +Indexlist *Indexlist_createMemory( int n ); + +/** Constructor which takes the desired physical length of the index list. */ +void IndexlistCON( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void IndexlistCPY( Indexlist* FROM, + Indexlist* TO + ); + +/** Initialises index list of desired physical length. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue Indexlist_init( Indexlist* _THIS, + int n /**< Physical length of index list. */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getNumberArray( Indexlist* _THIS, + int** const numberarray /**< Output: Array of numbers (NULL on error). */ + ); + +/** Creates an array of all numbers within the index set in correct order. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue Indexlist_getISortArray( Indexlist* _THIS, + int** const iSortArray /**< Output: iSort Array. */ + ); + + +/** Determines the index within the index list at which a given number is stored. + * \return >= 0: Index of given number. \n + -1: Number not found. */ +int Indexlist_getIndex( Indexlist* _THIS, + int givennumber /**< Number whose index shall be determined. */ + ); + +/** Returns the number stored at a given physical index. + * \return >= 0: Number stored at given physical index. \n + -RET_INDEXLIST_OUTOFBOUNDS */ +static inline int Indexlist_getNumber( Indexlist* _THIS, + int physicalindex /**< Physical index of the number to be returned. */ + ); + + +/** Returns the current length of the index list. + * \return Current length of the index list. */ +static inline int Indexlist_getLength( Indexlist* _THIS + ); + +/** Returns last number within the index list. + * \return Last number within the index list. */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS + ); + + +/** Adds number to index list. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_MUST_BE_REORDERD \n + RET_INDEXLIST_EXCEEDS_MAX_LENGTH */ +returnValue Indexlist_addNumber( Indexlist* _THIS, + int addnumber /**< Number to be added. */ + ); + +/** Removes number from index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_removeNumber( Indexlist* _THIS, + int removenumber /**< Number to be removed. */ + ); + +/** Swaps two numbers within index list. + * \return SUCCESSFUL_RETURN */ +returnValue Indexlist_swapNumbers( Indexlist* _THIS, + int number1, /**< First number for swapping. */ + int number2 /**< Second number for swapping. */ + ); + +/** Determines if a given number is contained in the index set. + * \return BT_TRUE iff number is contain in the index set */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, + int _number /**< Number to be tested for membership. */ + ); + + +/** Find first index j between -1 and length in sorted list of indices + * iSort such that numbers[iSort[j]] <= i < numbers[iSort[j+1]]. Uses + * bisection. + * \return j. */ +int Indexlist_findInsert( Indexlist* _THIS, + int i + ); + + + +/* + * g e t N u m b e r + */ +static inline int Indexlist_getNumber( Indexlist* _THIS, int physicalindex ) +{ + /* consistency check */ + if ( ( physicalindex < 0 ) || ( physicalindex > _THIS->length ) ) + return -RET_INDEXLIST_OUTOFBOUNDS; + + return _THIS->number[physicalindex]; +} + + +/* + * g e t L e n g t h + */ +static inline int Indexlist_getLength( Indexlist* _THIS ) +{ + return _THIS->length; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline int Indexlist_getLastNumber( Indexlist* _THIS ) +{ + return _THIS->number[_THIS->length-1]; +} + + +/* + * g e t L a s t N u m b e r + */ +static inline BooleanType Indexlist_isMember( Indexlist* _THIS, int _number ) +{ + if ( Indexlist_getIndex( _THIS,_number ) >= 0 ) + return BT_TRUE; + else + return BT_FALSE; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_INDEXLIST_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Matrices.h b/third_party/acados/include/qpOASES_e/Matrices.h new file mode 100644 index 0000000..e2a46b3 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Matrices.h @@ -0,0 +1,287 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Matrices.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2009-2015 + * + * Various matrix classes: Abstract base matrix class, dense and sparse matrices, + * including symmetry exploiting specializations. + */ + + + +#ifndef QPOASES_MATRICES_H +#define QPOASES_MATRICES_H + +#ifdef __USE_SINGLE_PRECISION__ + + // single precision + #define GEMM sgemm_ + #define GEMV sgemv_ +// #define SYR ssyr_ +// #define SYR2 ssyr2_ + #define POTRF spotrf_ + +#else + + // double precision + #define GEMM dgemm_ + #define GEMV dgemv_ +// #define SYR dsyr_ +// #define SYR2 dsyr2_ + #define POTRF dpotrf_ + +#endif /* __USE_SINGLE_PRECISION__ */ + + +#ifdef EXTERNAL_BLAS + // double precision + void dgemm_(char *ta, char *tb, int *m, int *n, int *k, double *alpha, double *A, int *lda, double *B, int ldb, double *beta, double *C, int *ldc); + void dgemv_(char *ta, int *m, int *n, double *alpha, double *A, int *lda, double *x, int *incx, double *beta, double *y, int *incy); + void dpotrf_(char *uplo, int *m, double *A, int *lda, int *info); + // single precision + void sgemm_(char *ta, char *tb, int *m, int *n, int *k, float *alpha, float *A, int *lda, float *B, int ldb, float *beta, float *C, int *ldc); + void sgemv_(char *ta, int *m, int *n, float *alpha, float *A, int *lda, float *x, int *incx, float *beta, float *y, int *incy); + void spotrf_(char *uplo, int *m, float *A, int *lda, int *info); +#else + /** Performs one of the matrix-matrix operation in double precision. */ + void dgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const double*, const double*, const unsigned long*, const double*, const unsigned long*, + const double*, double*, const unsigned long* ); + /** Performs one of the matrix-matrix operation in single precision. */ + void sgemm_ ( const char*, const char*, const unsigned long*, const unsigned long*, const unsigned long*, + const float*, const float*, const unsigned long*, const float*, const unsigned long*, + const float*, float*, const unsigned long* ); + + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in double precision. */ + void dpotrf_ ( const char *, const unsigned long *, double *, const unsigned long *, long * ); + /** Calculates the Cholesky factorization of a real symmetric positive definite matrix in single precision. */ + void spotrf_ ( const char *, const unsigned long *, float *, const unsigned long *, long * ); + +#endif + + /** Performs a symmetric rank 1 operation in double precision. */ +// void dsyr_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 1 operation in single precision. */ +// void ssyr_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, float *, const unsigned long *); + + /** Performs a symmetric rank 2 operation in double precision. */ +// void dsyr2_ ( const char *, const unsigned long *, const double *, const double *, +// const unsigned long *, const double *, const unsigned long *, double *, const unsigned long *); + /** Performs a symmetric rank 2 operation in single precision. */ +// void ssyr2_ ( const char *, const unsigned long *, const float *, const float *, +// const unsigned long *, const float *, const unsigned long *, float *, const unsigned long *); + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Interfaces matrix-vector operations tailored to general dense matrices. + * + * Dense matrix class (row major format). + * + * \author Andreas Potschka, Christian Kirches, Hans Joachim Ferreau + * \version 3.1embedded + * \date 2011-2015 + */ +typedef struct +{ + real_t *val; /**< Vector of entries. */ + int nRows; /**< Number of rows. */ + int nCols; /**< Number of columns. */ + int leaDim; /**< Leading dimension. */ +} DenseMatrix; + +int DenseMatrix_calculateMemorySize( int m, int n ); + +char *DenseMatrix_assignMemory( int m, int n, DenseMatrix **mem, void *raw_memory ); + +DenseMatrix *DenseMatrix_createMemory( int m, int n ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +void DenseMatrixCON( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + +void DenseMatrixCPY( DenseMatrix* FROM, + DenseMatrix* TO + ); + + +/** Frees all internal memory. */ +void DenseMatrix_free( DenseMatrix* _THIS ); + +/** Constructor from vector of values. + * Caution: Data pointer must be valid throughout lifetime + */ +returnValue DenseMatrix_init( DenseMatrix* _THIS, + int m, /**< Number of rows. */ + int n, /**< Number of columns. */ + int lD, /**< Leading dimension. */ + real_t *v /**< Values. */ + ); + + +/** Returns i-th diagonal entry. + * \return i-th diagonal entry */ +real_t DenseMatrix_diag( DenseMatrix* _THIS, + int i /**< Index. */ + ); + +/** Checks whether matrix is square and diagonal. + * \return BT_TRUE iff matrix is square and diagonal; \n + * BT_FALSE otherwise. */ +BooleanType DenseMatrix_isDiag( DenseMatrix* _THIS ); + +/** Get the N-norm of the matrix + * \return N-norm of the matrix + */ +real_t DenseMatrix_getNorm( DenseMatrix* _THIS, + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Get the N-norm of a row + * \return N-norm of row \a rNum + */ +real_t DenseMatrix_getRowNorm( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Retrieve indexed entries of matrix row multiplied by alpha. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_getRow( DenseMatrix* _THIS, + int rNum, /**< Row number. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + real_t alpha, /**< Scalar factor. */ + real_t *row /**< Output row vector. */ + ); + +/** Retrieve indexed entries of matrix column multiplied by alpha. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_getCol( DenseMatrix* _THIS, + int cNum, /**< Column number. */ + const Indexlist* const irows, /**< Index list specifying rows. */ + real_t alpha, /**< Scalar factor. */ + real_t *col /**< Output column vector. */ + ); + +/** Evaluate Y=alpha*A*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_times( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate Y=alpha*A'*X + beta*Y. + * \return SUCCESSFUL_RETURN. */ +returnValue DenseMatrix_transTimes( DenseMatrix* _THIS, + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Evaluate matrix vector product with submatrix given by Indexlist. + * \return SUCCESSFUL_RETURN */ + returnValue DenseMatrix_subTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD, /**< Leading dimension of output y. */ + BooleanType yCompr /**< Compressed storage for y. */ + ); + +/** Evaluate matrix transpose vector product. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_subTransTimes( DenseMatrix* _THIS, + const Indexlist* const irows, /**< Index list specifying rows. */ + const Indexlist* const icols, /**< Index list specifying columns. */ + int xN, /**< Number of vectors to multiply. */ + real_t alpha, /**< Scalar factor for matrix vector product. */ + const real_t *x, /**< Input vector to be multiplied. */ + int xLD, /**< Leading dimension of input x. */ + real_t beta, /**< Scalar factor for y. */ + real_t *y, /**< Output vector of results. */ + int yLD /**< Leading dimension of output y. */ + ); + +/** Adds given offset to diagonal of matrix. + * \return SUCCESSFUL_RETURN \n + RET_NO_DIAGONAL_AVAILABLE */ +returnValue DenseMatrix_addToDiag( DenseMatrix* _THIS, + real_t alpha /**< Diagonal offset. */ + ); + +/** Prints matrix to screen. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_print( DenseMatrix* _THIS + ); + +static inline real_t* DenseMatrix_getVal( DenseMatrix* _THIS ) { return _THIS->val; } + +/** Compute bilinear form y = x'*H*x using submatrix given by index list. + * \return SUCCESSFUL_RETURN */ +returnValue DenseMatrix_bilinear( DenseMatrix* _THIS, + const Indexlist* const icols, /**< Index list specifying columns of x. */ + int xN, /**< Number of vectors to multiply. */ + const real_t *x, /**< Input vector to be multiplied (uncompressed). */ + int xLD, /**< Leading dimension of input x. */ + real_t *y, /**< Output vector of results (compressed). */ + int yLD /**< Leading dimension of output y. */ + ); + + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_MATRICES_H */ diff --git a/third_party/acados/include/qpOASES_e/MessageHandling.h b/third_party/acados/include/qpOASES_e/MessageHandling.h new file mode 100644 index 0000000..fe55249 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/MessageHandling.h @@ -0,0 +1,544 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/MessageHandling.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches (thanks to Leonard Wirsching) + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the MessageHandling class including global return values. + */ + + +#ifndef QPOASES_MESSAGEHANDLING_H +#define QPOASES_MESSAGEHANDLING_H + + +#include +#include +#include +#include + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** Default file to display messages. */ +#define stdFile stderr + + +/** + * \brief Defines all symbols for global return values. + * + * The enumeration returnValueType defines all symbols for global return values. + * Important: All return values are assumed to be nonnegative! + * + * \author Hans Joachim Ferreau + */ +typedef enum +{ +TERMINAL_LIST_ELEMENT = -1, /**< Terminal list element, internal usage only! */ +/* miscellaneous */ +SUCCESSFUL_RETURN = 0, /**< Successful return. */ +RET_DIV_BY_ZERO, /**< Division by zero. */ +RET_INDEX_OUT_OF_BOUNDS, /**< Index out of bounds. */ +RET_INVALID_ARGUMENTS, /**< At least one of the arguments is invalid. */ +RET_ERROR_UNDEFINED, /**< Error number undefined. */ +RET_WARNING_UNDEFINED, /**< Warning number undefined. */ +RET_INFO_UNDEFINED, /**< Info number undefined. */ +RET_EWI_UNDEFINED, /**< Error/warning/info number undefined. */ +RET_AVAILABLE_WITH_LINUX_ONLY, /**< This function is available under Linux only. */ +RET_UNKNOWN_BUG, /**< The error occurred is not yet known. */ +RET_PRINTLEVEL_CHANGED, /**< Print level changed. (10) */ +RET_NOT_YET_IMPLEMENTED, /**< Requested function is not yet implemented in this version of qpOASES. */ +/* Indexlist */ +RET_INDEXLIST_MUST_BE_REORDERD, /**< Index list has to be reordered. */ +RET_INDEXLIST_EXCEEDS_MAX_LENGTH, /**< Index list exceeds its maximal physical length. */ +RET_INDEXLIST_CORRUPTED, /**< Index list corrupted. */ +RET_INDEXLIST_OUTOFBOUNDS, /**< Physical index is out of bounds. */ +RET_INDEXLIST_ADD_FAILED, /**< Adding indices from another index set failed. */ +RET_INDEXLIST_INTERSECT_FAILED, /**< Intersection with another index set failed. */ +/* SubjectTo / Bounds / Constraints */ +RET_INDEX_ALREADY_OF_DESIRED_STATUS, /**< Index is already of desired status. (18) */ +RET_ADDINDEX_FAILED, /**< Adding index to index set failed. */ +RET_REMOVEINDEX_FAILED, /**< Removing index from index set failed. (20) */ +RET_SWAPINDEX_FAILED, /**< Cannot swap between different indexsets. */ +RET_NOTHING_TO_DO, /**< Nothing to do. */ +RET_SETUP_BOUND_FAILED, /**< Setting up bound index failed. */ +RET_SETUP_CONSTRAINT_FAILED, /**< Setting up constraint index failed. */ +RET_MOVING_BOUND_FAILED, /**< Moving bound between index sets failed. */ +RET_MOVING_CONSTRAINT_FAILED, /**< Moving constraint between index sets failed. */ +RET_SHIFTING_FAILED, /**< Shifting of bounds/constraints failed. */ +RET_ROTATING_FAILED, /**< Rotating of bounds/constraints failed. */ +/* QProblem */ +RET_QPOBJECT_NOT_SETUP, /**< The QP object has not been setup correctly, use another constructor. */ +RET_QP_ALREADY_INITIALISED, /**< QProblem has already been initialised. (30) */ +RET_NO_INIT_WITH_STANDARD_SOLVER, /**< Initialisation via extern QP solver is not yet implemented. */ +RET_RESET_FAILED, /**< Reset failed. */ +RET_INIT_FAILED, /**< Initialisation failed. */ +RET_INIT_FAILED_TQ, /**< Initialisation failed due to TQ factorisation. */ +RET_INIT_FAILED_CHOLESKY, /**< Initialisation failed due to Cholesky decomposition. */ +RET_INIT_FAILED_HOTSTART, /**< Initialisation failed! QP could not be solved! */ +RET_INIT_FAILED_INFEASIBILITY, /**< Initial QP could not be solved due to infeasibility! */ +RET_INIT_FAILED_UNBOUNDEDNESS, /**< Initial QP could not be solved due to unboundedness! */ +RET_INIT_FAILED_REGULARISATION, /**< Initialisation failed as Hessian matrix could not be regularised. */ +RET_INIT_SUCCESSFUL, /**< Initialisation done. (40) */ +RET_OBTAINING_WORKINGSET_FAILED, /**< Failed to obtain working set for auxiliary QP. */ +RET_SETUP_WORKINGSET_FAILED, /**< Failed to setup working set for auxiliary QP. */ +RET_SETUP_AUXILIARYQP_FAILED, /**< Failed to setup auxiliary QP for initialised homotopy. */ +RET_NO_CHOLESKY_WITH_INITIAL_GUESS, /**< Externally computed Cholesky factor cannot be combined with an initial guess. */ +RET_NO_EXTERN_SOLVER, /**< No extern QP solver available. */ +RET_QP_UNBOUNDED, /**< QP is unbounded. */ +RET_QP_INFEASIBLE, /**< QP is infeasible. */ +RET_QP_NOT_SOLVED, /**< Problems occurred while solving QP with standard solver. */ +RET_QP_SOLVED, /**< QP successfully solved. */ +RET_UNABLE_TO_SOLVE_QP, /**< Problems occurred while solving QP. (50) */ +RET_INITIALISATION_STARTED, /**< Starting problem initialisation... */ +RET_HOTSTART_FAILED, /**< Unable to perform homotopy due to internal error. */ +RET_HOTSTART_FAILED_TO_INIT, /**< Unable to initialise problem. */ +RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED, /**< Unable to perform homotopy as previous QP is not solved. */ +RET_ITERATION_STARTED, /**< Iteration... */ +RET_SHIFT_DETERMINATION_FAILED, /**< Determination of shift of the QP data failed. */ +RET_STEPDIRECTION_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_STEPLENGTH_DETERMINATION_FAILED, /**< Determination of step direction failed. */ +RET_OPTIMAL_SOLUTION_FOUND, /**< Optimal solution of neighbouring QP found. */ +RET_HOMOTOPY_STEP_FAILED, /**< Unable to perform homotopy step. (60) */ +RET_HOTSTART_STOPPED_INFEASIBILITY, /**< Premature homotopy termination because QP is infeasible. */ +RET_HOTSTART_STOPPED_UNBOUNDEDNESS, /**< Premature homotopy termination because QP is unbounded. */ +RET_WORKINGSET_UPDATE_FAILED, /**< Unable to update working sets according to initial guesses. */ +RET_MAX_NWSR_REACHED, /**< Maximum number of working set recalculations performed. */ +RET_CONSTRAINTS_NOT_SPECIFIED, /**< Problem does comprise constraints! You also have to specify new constraints' bounds. */ +RET_INVALID_FACTORISATION_FLAG, /**< Invalid factorisation flag. */ +RET_UNABLE_TO_SAVE_QPDATA, /**< Unable to save QP data. */ +RET_STEPDIRECTION_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_STEPDIRECTION_FAILED_CHOLESKY, /**< Abnormal termination due to Cholesky factorisation. */ +RET_CYCLING_DETECTED, /**< Cycling detected. (70) */ +RET_CYCLING_NOT_RESOLVED, /**< Cycling cannot be resolved, QP probably infeasible. */ +RET_CYCLING_RESOLVED, /**< Cycling probably resolved. */ +RET_STEPSIZE, /**< For displaying performed stepsize. */ +RET_STEPSIZE_NONPOSITIVE, /**< For displaying non-positive stepsize. */ +RET_SETUPSUBJECTTOTYPE_FAILED, /**< Setup of SubjectToTypes failed. */ +RET_ADDCONSTRAINT_FAILED, /**< Addition of constraint to working set failed. */ +RET_ADDCONSTRAINT_FAILED_INFEASIBILITY, /**< Addition of constraint to working set failed (due to QP infeasibility). */ +RET_ADDBOUND_FAILED, /**< Addition of bound to working set failed. */ +RET_ADDBOUND_FAILED_INFEASIBILITY, /**< Addition of bound to working set failed (due to QP infeasibility). */ +RET_REMOVECONSTRAINT_FAILED, /**< Removal of constraint from working set failed. (80) */ +RET_REMOVEBOUND_FAILED, /**< Removal of bound from working set failed. */ +RET_REMOVE_FROM_ACTIVESET, /**< Removing from active set... */ +RET_ADD_TO_ACTIVESET, /**< Adding to active set... */ +RET_REMOVE_FROM_ACTIVESET_FAILED, /**< Removing from active set failed. */ +RET_ADD_TO_ACTIVESET_FAILED, /**< Adding to active set failed. */ +RET_CONSTRAINT_ALREADY_ACTIVE, /**< Constraint is already active. */ +RET_ALL_CONSTRAINTS_ACTIVE, /**< All constraints are active, no further constraint can be added. */ +RET_LINEARLY_DEPENDENT, /**< New bound/constraint is linearly dependent. */ +RET_LINEARLY_INDEPENDENT, /**< New bound/constraint is linearly independent. */ +RET_LI_RESOLVED, /**< Linear independence of active constraint matrix successfully resolved. (90) */ +RET_ENSURELI_FAILED, /**< Failed to ensure linear independence of active constraint matrix. */ +RET_ENSURELI_FAILED_TQ, /**< Abnormal termination due to TQ factorisation. */ +RET_ENSURELI_FAILED_NOINDEX, /**< QP is infeasible. */ +RET_ENSURELI_FAILED_CYCLING, /**< QP is infeasible. */ +RET_BOUND_ALREADY_ACTIVE, /**< Bound is already active. */ +RET_ALL_BOUNDS_ACTIVE, /**< All bounds are active, no further bound can be added. */ +RET_CONSTRAINT_NOT_ACTIVE, /**< Constraint is not active. */ +RET_BOUND_NOT_ACTIVE, /**< Bound is not active. */ +RET_HESSIAN_NOT_SPD, /**< Projected Hessian matrix not positive definite. */ +RET_HESSIAN_INDEFINITE, /**< Hessian matrix is indefinite. (100) */ +RET_MATRIX_SHIFT_FAILED, /**< Unable to update matrices or to transform vectors. */ +RET_MATRIX_FACTORISATION_FAILED, /**< Unable to calculate new matrix factorisations. */ +RET_PRINT_ITERATION_FAILED, /**< Unable to print information on current iteration. */ +RET_NO_GLOBAL_MESSAGE_OUTPUTFILE, /**< No global message output file initialised. */ +RET_DISABLECONSTRAINTS_FAILED, /**< Unable to disbable constraints. */ +RET_ENABLECONSTRAINTS_FAILED, /**< Unable to enbable constraints. */ +RET_ALREADY_ENABLED, /**< Bound or constraint is already enabled. */ +RET_ALREADY_DISABLED, /**< Bound or constraint is already disabled. */ +RET_NO_HESSIAN_SPECIFIED, /**< No Hessian matrix has been specified. */ +RET_USING_REGULARISATION, /**< Using regularisation as Hessian matrix is not positive definite. (110) */ +RET_EPS_MUST_BE_POSITVE, /**< Eps for regularisation must be sufficiently positive. */ +RET_REGSTEPS_MUST_BE_POSITVE, /**< Maximum number of regularisation steps must be non-negative. */ +RET_HESSIAN_ALREADY_REGULARISED, /**< Hessian has been already regularised. */ +RET_CANNOT_REGULARISE_IDENTITY, /**< Identity Hessian matrix cannot be regularised. */ +RET_CANNOT_REGULARISE_SPARSE, /**< Sparse matrix cannot be regularised as diagonal entry is missing. */ +RET_NO_REGSTEP_NWSR, /**< No additional regularisation step could be performed due to limits. */ +RET_FEWER_REGSTEPS_NWSR, /**< Fewer additional regularisation steps have been performed due to limits. */ +RET_CHOLESKY_OF_ZERO_HESSIAN, /**< Cholesky decomposition of (unregularised) zero Hessian matrix. */ +RET_ZERO_HESSIAN_ASSUMED, /**< Zero Hessian matrix assumed as null pointer passed without specifying hessianType. */ +RET_CONSTRAINTS_ARE_NOT_SCALED, /**< (no longer in use) (120) */ +RET_INITIAL_BOUNDS_STATUS_NYI, /**< (no longer in use) */ +RET_ERROR_IN_CONSTRAINTPRODUCT, /**< Error in user-defined constraint product function. */ +RET_FIX_BOUNDS_FOR_LP, /**< All initial bounds must be fixed when solving an (unregularised) LP. */ +RET_USE_REGULARISATION_FOR_LP, /**< Set options.enableRegularisation=BT_TRUE for solving LPs. */ +/* SQProblem */ +RET_UPDATEMATRICES_FAILED, /**< Unable to update QP matrices. */ +RET_UPDATEMATRICES_FAILED_AS_QP_NOT_SOLVED, /**< Unable to update matrices as previous QP is not solved. */ +/* Utils */ +RET_UNABLE_TO_OPEN_FILE, /**< Unable to open file. */ +RET_UNABLE_TO_WRITE_FILE, /**< Unable to write into file. */ +RET_UNABLE_TO_READ_FILE, /**< Unable to read from file. */ +RET_FILEDATA_INCONSISTENT, /**< File contains inconsistent data. (130) */ +/* Options */ +RET_OPTIONS_ADJUSTED, /**< Options needed to be adjusted for consistency reasons. */ +/* SolutionAnalysis */ +RET_UNABLE_TO_ANALYSE_QPROBLEM, /**< Unable to analyse (S)QProblem(B) object. */ +/* Benchmark */ +RET_NWSR_SET_TO_ONE, /**< Maximum number of working set changes was set to 1. */ +RET_UNABLE_TO_READ_BENCHMARK, /**< Unable to read benchmark data. */ +RET_BENCHMARK_ABORTED, /**< Benchmark aborted. */ +RET_INITIAL_QP_SOLVED, /**< Initial QP solved. */ +RET_QP_SOLUTION_STARTED, /**< Solving QP... */ +RET_BENCHMARK_SUCCESSFUL, /**< Benchmark terminated successfully. */ +/* Sparse matrices */ +RET_NO_DIAGONAL_AVAILABLE, /**< Sparse matrix does not have entries on full diagonal. */ +RET_DIAGONAL_NOT_INITIALISED, /**< Diagonal data of sparse matrix has not been initialised. (140) */ +/* Dropping of infeasible constraints */ +RET_ENSURELI_DROPPED, /**< Linear independence resolved by dropping blocking constraint. */ +/* Simple exitflags */ +RET_SIMPLE_STATUS_P1, /**< QP problem could not be solved within given number of iterations. */ +RET_SIMPLE_STATUS_P0, /**< QP problem solved. */ +RET_SIMPLE_STATUS_M1, /**< QP problem could not be solved due to an internal error. */ +RET_SIMPLE_STATUS_M2, /**< QP problem is infeasible (and thus could not be solved). */ +RET_SIMPLE_STATUS_M3 /**< QP problem is unbounded (and thus could not be solved). (146) */ +} returnValue; + + +/** + * \brief Data structure for entries in global message list. + * + * Data structure for entries in global message list. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + returnValue key; /**< Global return value. */ + const char* data; /**< Corresponding message. */ + VisibilityStatus globalVisibilityStatus; /**< Determines if message can be printed. + * If this value is set to VS_HIDDEN, no message is printed! */ +} ReturnValueList; + + + +/** + * \brief Handles all kind of error messages, warnings and other information. + * + * This class handles all kinds of messages (errors, warnings, infos) initiated + * by qpOASES modules and stores the corresponding global preferences. + * + * \author Hans Joachim Ferreau (thanks to Leonard Wirsching) + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + VisibilityStatus errorVisibility; /**< Error messages visible? */ + VisibilityStatus warningVisibility; /**< Warning messages visible? */ + VisibilityStatus infoVisibility; /**< Info messages visible? */ + + FILE* outputFile; /**< Output file for messages. */ + + int errorCount; /**< Counts number of errors (for nicer output only). */ +} MessageHandling; + + + +/** Constructor which takes the desired output file and desired visibility states. */ +void MessageHandlingCON( MessageHandling* _THIS, + FILE* _outputFile, /**< Output file. */ + VisibilityStatus _errorVisibility, /**< Visibility status for error messages. */ + VisibilityStatus _warningVisibility,/**< Visibility status for warning messages. */ + VisibilityStatus _infoVisibility /**< Visibility status for info messages. */ + ); + +void MessageHandlingCPY( MessageHandling* FROM, + MessageHandling* TO + ); + + +/** Prints an error message(a simplified macro THROWERROR is also provided). \n + * Errors are definied as abnormal events which cause an immediate termination of the current (sub) function. + * Errors of a sub function should be commented by the calling function by means of a warning message + * (if this error does not cause an error of the calling function, either)! + * \return Error number returned by sub function call + */ +returnValue MessageHandling_throwError( MessageHandling* _THIS, + returnValue Enumber, /**< Error number returned by sub function call. */ + const char* additionaltext, /**< Additional error text (0, if none). */ + const char* functionname, /**< Name of function which caused the error. */ + const char* filename, /**< Name of file which caused the error. */ + const unsigned long linenumber, /**< Number of line which caused the error.incompatible binary file */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if error message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + +/** Prints a warning message (a simplified macro THROWWARNING is also provided). + * Warnings are definied as abnormal events which does NOT cause an immediate termination of the current (sub) function. + * \return Warning number returned by sub function call + */ +returnValue MessageHandling_throwWarning( MessageHandling* _THIS, + returnValue Wnumber, /**< Warning number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the warning. */ + const char* filename, /**< Name of file which caused the warning. */ + const unsigned long linenumber, /**< Number of line which caused the warning. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if warning message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + +/** Prints a info message (a simplified macro THROWINFO is also provided). + * \return Info number returned by sub function call + */ +returnValue MessageHandling_throwInfo( MessageHandling* _THIS, + returnValue Inumber, /**< Info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which submitted the info. */ + const char* filename, /**< Name of file which submitted the info. */ + const unsigned long linenumber, /**< Number of line which submitted the info. */ + VisibilityStatus localVisibilityStatus /**< Determines (locally) if info message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + ); + + +/** Resets all preferences to default values. + * \return SUCCESSFUL_RETURN */ +returnValue MessageHandling_reset( MessageHandling* _THIS ); + + +/** Prints a complete list of all messages to output file. + * \return SUCCESSFUL_RETURN */ +returnValue MessageHandling_listAllMessages( MessageHandling* _THIS ); + + +/** Returns visibility status for error messages. + * \return Visibility status for error messages. */ +static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS ); + +/** Returns visibility status for warning messages. + * \return Visibility status for warning messages. */ +static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS ); + +/** Returns visibility status for info messages. + * \return Visibility status for info messages. */ +static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS ); + +/** Returns pointer to output file. + * \return Pointer to output file. */ +static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS ); + +/** Returns error count value. + * \return Error count value. */ +static inline int MessageHandling_getErrorCount( MessageHandling* _THIS ); + + +/** Changes visibility status for error messages. */ +static inline void MessageHandling_setErrorVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _errorVisibility /**< New visibility status for error messages. */ + ); + +/** Changes visibility status for warning messages. */ +static inline void MessageHandling_setWarningVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _warningVisibility /**< New visibility status for warning messages. */ + ); + +/** Changes visibility status for info messages. */ +static inline void MessageHandling_setInfoVisibilityStatus( MessageHandling* _THIS, + VisibilityStatus _infoVisibility /**< New visibility status for info messages. */ + ); + +/** Changes output file for messages. */ +static inline void MessageHandling_setOutputFile( MessageHandling* _THIS, + FILE* _outputFile /**< New output file for messages. */ + ); + +/** Changes error count. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENT */ +static inline returnValue MessageHandling_setErrorCount( MessageHandling* _THIS, + int _errorCount /**< New error count value. */ + ); + +/** Provides message text corresponding to given \a returnValue. + * \return String containing message text. */ +const char* MessageHandling_getErrorCodeMessage( MessageHandling* _THIS, + const returnValue _returnValue + ); + + +returnValue MessageHandling_throwMessage( MessageHandling* _THIS, + returnValue RETnumber, /**< Error/warning/info number returned by sub function call. */ + const char* additionaltext, /**< Additional warning text (0, if none). */ + const char* functionname, /**< Name of function which caused the error/warning/info. */ + const char* filename, /**< Name of file which caused the error/warning/info. */ + const unsigned long linenumber, /**< Number of line which caused the error/warning/info. */ + VisibilityStatus localVisibilityStatus, /**< Determines (locally) if info message can be printed to stderr. + * If GLOBAL visibility status of the message is set to VS_HIDDEN, + * no message is printed, anyway! */ + const char* RETstring /**< Leading string of error/warning/info message. */ + ); + + +#ifndef __FILE__ + /** Ensures that __FILE__ macro is defined. */ + #define __FILE__ 0 +#endif + +#ifndef __LINE__ + /** Ensures that __LINE__ macro is defined. */ + #define __LINE__ 0 +#endif + +/** Define __FUNC__ macro providing current function for debugging. */ +/*#define __FUNC__ 0*/ +#define __FUNC__ ("(no function name provided)") +/*#define __FUNC__ __func__*/ +/*#define __FUNC__ __FUNCTION__*/ + + +/** Short version of throwError with default values, only returnValue is needed */ +#define THROWERROR(retval) ( MessageHandling_throwError( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwWarning with default values, only returnValue is needed */ +#define THROWWARNING(retval) ( MessageHandling_throwWarning( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + +/** Short version of throwInfo with default values, only returnValue is needed */ +#define THROWINFO(retval) ( MessageHandling_throwInfo( qpOASES_getGlobalMessageHandler(),(retval),0,__FUNC__,__FILE__,__LINE__,VS_VISIBLE) ) + + +/** Returns a pointer to global message handler. + * \return Pointer to global message handler. + */ +MessageHandling* qpOASES_getGlobalMessageHandler( ); + + +/* + * g e t E r r o r V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getErrorVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->errorVisibility; +} + + +/* + * g e t W a r n i n g V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getWarningVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->warningVisibility; +} + + +/* + * g e t I n f o V i s i b i l i t y S t a t u s + */ +static inline VisibilityStatus MessageHandling_getInfoVisibilityStatus( MessageHandling* _THIS ) +{ + return _THIS->infoVisibility; +} + + +/* + * g e t O u t p u t F i l e + */ +static inline FILE* MessageHandling_getOutputFile( MessageHandling* _THIS ) +{ + return _THIS->outputFile; +} + + +/* + * g e t E r r o r C o u n t + */ +static inline int MessageHandling_getErrorCount( MessageHandling* _THIS ) +{ + return _THIS->errorCount; +} + + +/* + * s e t E r r o r V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setErrorVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _errorVisibility ) +{ + _THIS->errorVisibility = _errorVisibility; +} + + +/* + * s e t W a r n i n g V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setWarningVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _warningVisibility ) +{ + _THIS->warningVisibility = _warningVisibility; +} + + +/* + * s e t I n f o V i s i b i l i t y S t a t u s + */ +static inline void MessageHandling_setInfoVisibilityStatus( MessageHandling* _THIS, VisibilityStatus _infoVisibility ) +{ + _THIS->infoVisibility = _infoVisibility; +} + + +/* + * s e t O u t p u t F i l e + */ +static inline void MessageHandling_setOutputFile( MessageHandling* _THIS, FILE* _outputFile ) +{ + _THIS->outputFile = _outputFile; +} + + +/* + * s e t E r r o r C o u n t + */ +static inline returnValue MessageHandling_setErrorCount( MessageHandling* _THIS, int _errorCount ) +{ + if ( _errorCount >= 0 ) + { + _THIS->errorCount = _errorCount; + return SUCCESSFUL_RETURN; + } + else + return RET_INVALID_ARGUMENTS; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_MESSAGEHANDLING_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Options.h b/third_party/acados/include/qpOASES_e/Options.h new file mode 100644 index 0000000..ca8086d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Options.h @@ -0,0 +1,153 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Options.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the Options class designed to manage user-specified + * options for solving a QProblem. + */ + + +#ifndef QPOASES_OPTIONS_H +#define QPOASES_OPTIONS_H + + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** + * \brief Manages all user-specified options for solving QPs. + * + * This class manages all user-specified options used for solving + * quadratic programs. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + PrintLevel printLevel; /**< Print level. */ + + BooleanType enableRamping; /**< Specifies whether ramping shall be enabled or not. */ + BooleanType enableFarBounds; /**< Specifies whether far bounds shall be used or not. */ + BooleanType enableFlippingBounds; /**< Specifies whether flipping bounds shall be used or not. */ + BooleanType enableRegularisation; /**< Specifies whether Hessian matrix shall be regularised in case semi-definiteness is detected. */ + BooleanType enableFullLITests; /**< Specifies whether condition-hardened LI test shall be used or not. */ + BooleanType enableNZCTests; /**< Specifies whether nonzero curvature tests shall be used. */ + int enableDriftCorrection; /**< Specifies the frequency of drift corrections (0 = off). */ + int enableCholeskyRefactorisation; /**< Specifies the frequency of full refactorisation of proj. Hessian (otherwise updates). */ + BooleanType enableEqualities; /**< Specifies whether equalities shall be always treated as active constraints. */ + + real_t terminationTolerance; /**< Termination tolerance. */ + real_t boundTolerance; /**< Lower/upper (constraints') bound tolerance (an inequality constraint whose lower and upper bounds differ by less is regarded to be an equality constraint). */ + real_t boundRelaxation; /**< Offset for relaxing (constraints') bounds at beginning of an initial homotopy. It is also as initial value for far bounds. */ + real_t epsNum; /**< Numerator tolerance for ratio tests. */ + real_t epsDen; /**< Denominator tolerance for ratio tests. */ + real_t maxPrimalJump; /**< Maximum allowed jump in primal variables in nonzero curvature tests. */ + real_t maxDualJump; /**< Maximum allowed jump in dual variables in linear independence tests. */ + + real_t initialRamping; /**< Start value for Ramping Strategy. */ + real_t finalRamping; /**< Final value for Ramping Strategy. */ + real_t initialFarBounds; /**< Initial size of Far Bounds. */ + real_t growFarBounds; /**< Factor to grow Far Bounds. */ + SubjectToStatus initialStatusBounds; /**< Initial status of bounds at first iteration. */ + real_t epsFlipping; /**< Tolerance of squared Cholesky diagonal factor which triggers flipping bound. */ + int numRegularisationSteps; /**< Maximum number of successive regularisation steps. */ + real_t epsRegularisation; /**< Scaling factor of identity matrix used for Hessian regularisation. */ + int numRefinementSteps; /**< Maximum number of iterative refinement steps. */ + real_t epsIterRef; /**< Early termination tolerance for iterative refinement. */ + real_t epsLITests; /**< Tolerance for linear independence tests. */ + real_t epsNZCTests; /**< Tolerance for nonzero curvature tests. */ + + BooleanType enableDropInfeasibles; /**< ... */ + int dropBoundPriority; /**< ... */ + int dropEqConPriority; /**< ... */ + int dropIneqConPriority; /**< ... */ +} Options; + + +void OptionsCON( Options* _THIS + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void OptionsCPY( Options* FROM, + Options* TO + ); + + +/** Sets all options to default values. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToDefault( Options* _THIS + ); + +/** Sets all options to values resulting in maximum reliabilty. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToReliable( Options* _THIS + ); + +/** Sets all options to values resulting in minimum solution time. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToMPC( Options* _THIS + ); + +/** Same as setToMPC( ), for ensuring backwards compatibility. + * \return SUCCESSFUL_RETURN */ +returnValue Options_setToFast( Options* _THIS + ); + + +/** Ensures that all options have consistent values by automatically + * adjusting inconsistent ones. + * Note: This routine cannot (and does not try to) ensure that values + * are set to reasonable values that make the QP solution work! + * \return SUCCESSFUL_RETURN \n + * RET_OPTIONS_ADJUSTED */ +returnValue Options_ensureConsistency( Options* _THIS + ); + + +/** Prints values of all options. + * \return SUCCESSFUL_RETURN */ +returnValue Options_print( Options* _THIS + ); + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_OPTIONS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/QProblem.h b/third_party/acados/include/qpOASES_e/QProblem.h new file mode 100644 index 0000000..91a4a6f --- /dev/null +++ b/third_party/acados/include/qpOASES_e/QProblem.h @@ -0,0 +1,2369 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/QProblem.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the QProblem class which is able to use the newly + * developed online active set strategy for parametric quadratic programming. + */ + + + +#ifndef QPOASES_QPROBLEM_H +#define QPOASES_QPROBLEM_H + + +#include +#include +#include +#include +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + Bounds *auxiliaryBounds; + Constraints *auxiliaryConstraints; + + real_t *ub_new_far; + real_t *lb_new_far; + real_t *ubA_new_far; + real_t *lbA_new_far; + + real_t *g_new; + real_t *lb_new; + real_t *ub_new; + real_t *lbA_new; + real_t *ubA_new; + + real_t *g_new2; + real_t *lb_new2; + real_t *ub_new2; + real_t *lbA_new2; + real_t *ubA_new2; + + real_t *delta_xFX5; + real_t *delta_xFR5; + real_t *delta_yAC5; + real_t *delta_yFX5; + + real_t *Hx; + + real_t *_H; + + real_t *g_original; + real_t *lb_original; + real_t *ub_original; + real_t *lbA_original; + real_t *ubA_original; + + real_t *delta_xFR; + real_t *delta_xFX; + real_t *delta_yAC; + real_t *delta_yFX; + real_t *delta_g; + real_t *delta_lb; + real_t *delta_ub; + real_t *delta_lbA; + real_t *delta_ubA; + + real_t *gMod; + + real_t *aFR; + real_t *wZ; + + real_t *delta_g2; + real_t *delta_xFX2; + real_t *delta_xFR2; + real_t *delta_yAC2; + real_t *delta_yFX2; + real_t *nul; + real_t *Arow; + + real_t *xiC; + real_t *xiC_TMP; + real_t *xiB; + real_t *Arow2; + real_t *num; + + real_t *w; + real_t *tmp; + + real_t *delta_g3; + real_t *delta_xFX3; + real_t *delta_xFR3; + real_t *delta_yAC3; + real_t *delta_yFX3; + real_t *nul2; + + real_t *xiC2; + real_t *xiC_TMP2; + real_t *xiB2; + real_t *num2; + + real_t *Hz; + real_t *z; + real_t *ZHz; + real_t *r; + + real_t *tmp2; + real_t *Hz2; + real_t *z2; + real_t *r2; + real_t *rhs; + + real_t *delta_xFX4; + real_t *delta_xFR4; + real_t *delta_yAC4; + real_t *delta_yFX4; + real_t *nul3; + real_t *ek; + real_t *x_W; + real_t *As; + real_t *Ax_W; + + real_t *num3; + real_t *den; + real_t *delta_Ax_l; + real_t *delta_Ax_u; + real_t *delta_Ax; + real_t *delta_x; + + real_t *_A; + + real_t *grad; + real_t *AX; +} QProblem_ws; + +int QProblem_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_ws_assignMemory( unsigned int nV, unsigned int nC, QProblem_ws **mem, void *raw_memory ); + +QProblem_ws *QProblem_ws_createMemory( unsigned int nV, unsigned int nC ); + +/** + * \brief Implements the online active set strategy for QPs with general constraints. + * + * A class for setting up and solving quadratic programs. The main feature is + * the possibily to use the newly developed online active set strategy for + * parametric quadratic programming. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + QProblem_ws *ws; /**< Workspace */ + Bounds *bounds; /**< Data structure for problem's bounds. */ + Constraints *constraints; /**< Data structure for problem's constraints. */ + Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ + + DenseMatrix* H; /**< Hessian matrix pointer. */ + DenseMatrix* A; /**< Constraint matrix pointer. */ + + Options options; /**< Struct containing all user-defined options for solving QPs. */ + TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ + + real_t *g; /**< Gradient. */ + + real_t *lb; /**< Lower bound vector (on variables). */ + real_t *ub; /**< Upper bound vector (on variables). */ + real_t *lbA; /**< Lower constraints' bound vector. */ + real_t *ubA; /**< Upper constraints' bound vector. */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + + real_t *T; /**< Reverse triangular matrix, A = [0 T]*Q'. */ + real_t *Q; /**< Orthonormal quadratic matrix, A = [0 T]*Q'. */ + + real_t *Ax; /**< Stores the current A*x \n + * (for increased efficiency only). */ + real_t *Ax_l; /**< Stores the current distance to lower constraints' bounds A*x-lbA \n + * (for increased efficiency only). */ + real_t *Ax_u; /**< Stores the current distance to lower constraints' bounds ubA-A*x \n + * (for increased efficiency only). */ + + real_t *x; /**< Primal solution vector. */ + real_t *y; /**< Dual solution vector. */ + + real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ + real_t *tempA; /**< Temporary for determineStepDirection. */ + real_t *tempB; /**< Temporary for determineStepDirection. */ + real_t *ZFR_delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRy; /**< Temporary for determineStepDirection. */ + real_t *delta_xFRz; /**< Temporary for determineStepDirection. */ + real_t *delta_yAC_TMP; /**< Temporary for determineStepDirection. */ + + ConstraintProduct constraintProduct; /**< Pointer to user-defined constraint product function. */ + + real_t tau; /**< Last homotopy step length. */ + real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ + + real_t ramp0; /**< Start value for Ramping Strategy. */ + real_t ramp1; /**< Final value for Ramping Strategy. */ + + QProblemStatus status; /**< Current status of the solution process. */ + HessianType hessianType; /**< Type of Hessian matrix. */ + + BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + int rampOffset; /**< Offset index for Ramping. */ + unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ + + int sizeT; /**< Matrix T is stored in a (sizeT x sizeT) array. */ +} QProblem; + +int QProblem_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *QProblem_assignMemory( unsigned int nV, unsigned int nC, QProblem **mem, void *raw_memory ); + +QProblem *QProblem_createMemory( unsigned int nV, unsigned int nC ); + + +/** Constructor which takes the QP dimension and Hessian type + * information. If the Hessian is the zero (i.e. HST_ZERO) or the + * identity matrix (i.e. HST_IDENTITY), respectively, no memory + * is allocated for it and a NULL pointer can be passed for it + * to the init() functions. */ +void QProblemCON( QProblem* _THIS, + int _nV, /**< Number of variables. */ + int _nC, /**< Number of constraints. */ + HessianType _hessianType /**< Type of Hessian matrix. */ + ); + +/** Copies all members from given rhs object. + * \return SUCCESSFUL_RETURN */ +void QProblemCPY( QProblem* FROM, + QProblem* TO + ); + + +/** Clears all data structures of QProblem except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ +returnValue QProblem_reset( QProblem* _THIS ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_init( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data to be read from files and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initF( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initMW( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_initW( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA, /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + * Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a QP problem with given QP data to be ream from files and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB/gC empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB/gC by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB/gC from yOpt != 0, \n + * 4. 0, 0, gB/gC: starts with xOpt = 0, yOpt = 0 and gB/gC, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB/gC from yOpt != 0, \n + * 6. xOpt, 0, gB/gC: starts with xOpt, yOpt = 0 and gB/gC, \n + * 7. xOpt, yOpt, gB/gC: starts with xOpt, yOpt and gB/gC (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblem_initFW( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const A_file, /**< Name of file where constraint matrix is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bound vector. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const char* const R_file /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_hotstart( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartF( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblem_hotstartW( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_hotstartFW( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of bounds is kept!) */ + Constraints* const guessedConstraints /**< Optimal working set of constraints for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set of constraints is kept!) */ + ); + + +/** Solves using the current working set + * \return SUCCESSFUL_RETURN \n + * RET_STEPDIRECTION_FAILED_TQ \n + * RET_STEPDIRECTION_FAILED_CHOLESKY \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_solveCurrentEQP ( QProblem* _THIS, + const int n_rhs, /**< Number of consecutive right hand sides */ + const real_t* g_in, /**< Gradient of neighbouring QP to be solved. */ + const real_t* lb_in, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* ub_in, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* lbA_in, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* ubA_in, /**< Upper constraints' bounds of neighbouring QP to be solved. \n */ + real_t* x_out, /**< Output: Primal solution */ + real_t* y_out /**< Output: Dual solution */ + ); + + + +/** Returns current constraints object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, + Constraints* _constraints /** Output: Constraints object. */ + ); + + +/** Returns the number of constraints. + * \return Number of constraints. */ +static inline int QProblem_getNC( QProblem* _THIS ); + +/** Returns the number of (implicitly defined) equality constraints. + * \return Number of (implicitly defined) equality constraints. */ +static inline int QProblem_getNEC( QProblem* _THIS ); + +/** Returns the number of active constraints. + * \return Number of active constraints. */ +static inline int QProblem_getNAC( QProblem* _THIS ); + +/** Returns the number of inactive constraints. + * \return Number of inactive constraints. */ +static inline int QProblem_getNIAC( QProblem* _THIS ); + +/** Returns the dimension of null space. + * \return Dimension of null space. */ +int QProblem_getNZ( QProblem* _THIS ); + + +/** Returns the dual solution vector (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getDualSolution( QProblem* _THIS, + real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ); + + +/** Defines user-defined routine for calculating the constraint product A*x + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_setConstraintProduct( QProblem* _THIS, + ConstraintProduct _constraintProduct + ); + + +/** Prints concise list of properties of the current QP. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printProperties( QProblem* _THIS ); + + + +/** Writes a vector with the state of the working set +* \return SUCCESSFUL_RETURN */ +returnValue QProblem_getWorkingSet( QProblem* _THIS, + real_t* workingSet /** Output: array containing state of the working set. */ + ); + +/** Writes a vector with the state of the working set of bounds + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetBounds( QProblem* _THIS, + real_t* workingSetB /** Output: array containing state of the working set of bounds. */ + ); + +/** Writes a vector with the state of the working set of constraints + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblem_getWorkingSetConstraints( QProblem* _THIS, + real_t* workingSetC /** Output: array containing state of the working set of constraints. */ + ); + + +/** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, + Bounds* _bounds /** Output: Bounds object. */ + ); + + +/** Returns the number of variables. + * \return Number of variables. */ +static inline int QProblem_getNV( QProblem* _THIS ); + +/** Returns the number of free variables. + * \return Number of free variables. */ +static inline int QProblem_getNFR( QProblem* _THIS ); + +/** Returns the number of fixed variables. + * \return Number of fixed variables. */ +static inline int QProblem_getNFX( QProblem* _THIS ); + +/** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int QProblem_getNFV( QProblem* _THIS ); + + +/** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ +real_t QProblem_getObjVal( QProblem* _THIS ); + +/** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ +real_t QProblem_getObjValX( QProblem* _THIS, + const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ); + +/** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblem_getPrimalSolution( QProblem* _THIS, + real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ); + + +/** Returns status of the solution process. + * \return Status of solution process. */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ); + + +/** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblem initialised \n + BT_FALSE: QProblem not initialised */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ); + +/** Returns if the QP has been solved. + * \return BT_TRUE: QProblem solved \n + BT_FALSE: QProblem not solved */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ); + +/** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ); + +/** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ); + + +/** Returns Hessian type flag (type is not determined due to _THIS call!). + * \return Hessian type. */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, + HessianType _hessianType /**< New Hessian type. */ + ); + +/** Returns if the QP has been internally regularised. + * \return BT_TRUE: Hessian is internally regularised for QP solution \n + BT_FALSE: No internal Hessian regularisation is used for QP solution */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ); + +/** Returns current options struct. + * \return Current options struct. */ +static inline Options QProblem_getOptions( QProblem* _THIS ); + +/** Overrides current options with given ones. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options /**< New options. */ + ); + +/** Returns the print level. + * \return Print level. */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setPrintLevel( QProblem* _THIS, + PrintLevel _printlevel /**< New print level. */ + ); + + +/** Returns the current number of QP problems solved. + * \return Number of QP problems solved. */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ); + +/** Resets QP problem counter (to zero). + * \return SUCCESSFUL_RETURN. */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ); + + +/** Prints a list of all options and their current values. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printOptions( QProblem* _THIS ); + + +/** Solves a QProblem whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * working sets of bounds and constraints can be provided. + * Note: This function is internally called by all init functions! + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_TQ \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ +returnValue QProblem_solveInitialQP( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector.*/ + const real_t* const yOpt, /**< Optimal dual solution vector. */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Optimal working set of constraints for solution (xOpt,yOpt). */ + const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveQP() calls. This number is + always zero, except for successive calls from solveRegularisedQP() + or when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Solves QProblem using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_solveRegularisedQP( QProblem* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const lbA_new, /**< Lower constraints' bounds of neighbouring QP to be solved. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const ubA_new, /**< Upper constraints' bounds of neighbouring QP to be solved. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveRegularisedQP() calls. This number is + always zero, except for successive calls when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToType( QProblem* _THIS ); + +/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblem_setupSubjectToTypeNew( QProblem* _THIS, + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new /**< New upper constraints' bounds. */ + ); + +/** Computes the Cholesky decomposition of the projected Hessian (i.e. R^T*R = Z^T*H*Z). + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_computeProjectedCholesky( QProblem* _THIS ); + +/** Computes initial Cholesky decomposition of the projected Hessian making + * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupInitialCholesky( QProblem* _THIS ); + +/** Initialises TQ factorisation of A (i.e. A*Q = [0 T]) if NO constraint is active. + * \return SUCCESSFUL_RETURN \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_setupTQfactorisation( QProblem* _THIS ); + + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * (assumes that member AX has already been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set of bounds for solution (xOpt,yOpt). */ + Constraints* const guessedConstraints, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds, /**< Input: Allocated bound object. \n + * Ouput: Working set of constraints for auxiliary QP. */ + Constraints* auxiliaryConstraints /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ); + +/** Sets up bound and constraints data structures according to auxiliaryBounds/Constraints. + * (If the working set shall be setup afresh, make sure that + * bounds and constraints data structure have been resetted + * and the TQ factorisation has been initialised!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryWorkingSet( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + +/** Sets up the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPsolution( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + +/** Sets up gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_setupAuxiliaryQPgradient( QProblem* _THIS ); + +/** Sets up (constraints') bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS, CONSTRAINTS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ +returnValue QProblem_setupAuxiliaryQPbounds( QProblem* _THIS, + Bounds* const auxiliaryBounds, /**< Working set of bounds for auxiliary QP. */ + Constraints* const auxiliaryConstraints, /**< Working set of constraints for auxiliary QP. */ + BooleanType useRelaxation /**< Flag indicating if inactive (constraints') bounds shall be relaxed. */ + ); + + +/** Adds a constraint to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDCONSTRAINT_FAILED \n + RET_ADDCONSTRAINT_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status, /**< Status of new active constraint. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active constraint to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT \n + RET_INDEXLIST_CORRUPTED */ +returnValue QProblem_addConstraint_checkLI( QProblem* _THIS, + int number /**< Number of constraint to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new constraint is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addConstraint_ensureLI( QProblem* _THIS, + int number, /**< Number of constraint to be added to active set. */ + SubjectToStatus C_status /**< Status of new active bound. */ + ); + +/** Adds a bound to active set. + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED \n + RET_ADDBOUND_FAILED_INFEASIBILITY \n + RET_ENSURELI_FAILED */ +returnValue QProblem_addBound( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType ensureLI /**< Ensure linear independence by exchange rules by default. */ + ); + +/** Checks if new active bound to be added is linearly dependent from + * from row of the active constraints matrix. + * \return RET_LINEARLY_DEPENDENT \n + RET_LINEARLY_INDEPENDENT */ +returnValue QProblem_addBound_checkLI( QProblem* _THIS, + int number /**< Number of bound to be added to active set. */ + ); + +/** Ensures linear independence of constraint matrix when a new bound is added. + * To _THIS end a bound or constraint is removed simultaneously if necessary. + * \return SUCCESSFUL_RETURN \n + RET_LI_RESOLVED \n + RET_ENSURELI_FAILED \n + RET_ENSURELI_FAILED_TQ \n + RET_ENSURELI_FAILED_NOINDEX \n + RET_REMOVE_FROM_ACTIVESET */ +returnValue QProblem_addBound_ensureLI( QProblem* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status /**< Status of new active bound. */ + ); + +/** Removes a constraint from active set. + * \return SUCCESSFUL_RETURN \n + RET_CONSTRAINT_NOT_ACTIVE \n + RET_REMOVECONSTRAINT_FAILED \n + RET_HESSIAN_NOT_SPD */ +returnValue QProblem_removeConstraint( QProblem* _THIS, + int number, /**< Number of constraint to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + +/** Removes a bounds from active set. + * \return SUCCESSFUL_RETURN \n + RET_BOUND_NOT_ACTIVE \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ +returnValue QProblem_removeBound( QProblem* _THIS, + int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky, /**< Flag indicating if Cholesky decomposition shall be updated. */ + BooleanType allowFlipping, /**< Flag indicating if flipping bounds are allowed. */ + BooleanType ensureNZC /**< Flag indicating if non-zero curvature is ensured by exchange rules. */ + ); + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performPlainRatioTest( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Ensure non-zero curvature by primal jump. + * \return SUCCESSFUL_RETURN \n + * RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblem_ensureNonzeroCurvature( QProblem* _THIS, + BooleanType removeBoundNotConstraint, /**< SubjectTo to be removed is a bound. */ + int remIdx, /**< Index of bound/constraint to be removed. */ + BooleanType* exchangeHappened, /**< Output: Exchange was necessary to ensure. */ + BooleanType* addBoundNotConstraint, /**< SubjectTo to be added is a bound. */ + int* addIdx, /**< Index of bound/constraint to be added. */ + SubjectToStatus* addStatus /**< Status of bound/constraint to be added. */ + ); + + +/** Solves the system Ta = b or T^Ta = b where T is a reverse upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveT( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lbA_new, /**< New lower constraints' bounds. */ + const real_t* const ubA_new, /**< New upper constraints' bounds. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lbA, /**< Output: Step direction of lower constraints' bounds. */ + real_t* const delta_ubA, /**< Output: Step direction of upper constraints' bounds. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bC_isZero, /**< Output: Indicates if active constraints' bounds are to be shifted. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + +/** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_TQ \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ +returnValue QProblem_determineStepDirection( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bC_isZero, /**< Indicates if active constraints' bounds are to be shifted. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yAC, /**< Output: Dual homotopy step direction of active constraints' multiplier. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + +/** Determines the maximum possible step length along the homotopy path + * and performs _THIS step (without changing working set). + * \return SUCCESSFUL_RETURN \n + * RET_ERROR_IN_CONSTRAINTPRODUCT \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_performStep( QProblem* _THIS, + const real_t* const delta_g, /**< Step direction of gradient. */ + const real_t* const delta_lbA, /**< Step direction of lower constraints' bounds. */ + const real_t* const delta_ubA, /**< Step direction of upper constraints' bounds. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yAC, /**< Dual homotopy step direction of active constraints' multiplier. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int* BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus* BC_status, /**< Output: Status of blocking constraint. */ + BooleanType* BC_isBound /**< Output: Indicates if blocking constraint is a bound. */ + ); + +/** Updates the active set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED */ +returnValue QProblem_changeActiveSet( QProblem* _THIS, + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound /**< Indicates if blocking constraint is a bound. */ + ); + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblem_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new, /**< Final upper variable bounds. */ + const real_t* const lbA_new, /**< Final lower constraint bounds. */ + const real_t* const ubA_new /**< Final upper constraint bounds. */ + ); + + +/** Ramping Strategy to avoid ties. Modifies homotopy start without + * changing current active set. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRamping( QProblem* _THIS ); + + +/** ... */ +returnValue QProblem_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far, /**< ... */ + const real_t* const lbA_new, /**< ... */ + real_t* const lbA_new_far, /**< ... */ + const real_t* const ubA_new, /**< ... */ + real_t* const ubA_new_far /**< ... */ + ); + +/** ... */ +returnValue QProblemBCPY_updateFarBounds( QProblem* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far /**< ... */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestC( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Constraints* const subjectTo, /**< Constraint object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + + +/** Drift correction at end of each active set iteration + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performDriftCorrection( QProblem* _THIS ); + + +/** Updates QP vectors, working sets and internal data structures in order to + start from an optimal solution corresponding to initial guesses of the working + set for bounds and constraints. + * \return SUCCESSFUL_RETURN \n + * RET_SETUP_AUXILIARYQP_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_setupAuxiliaryQP( QProblem* _THIS, + Bounds* const guessedBounds, /**< Initial guess for working set of bounds. */ + Constraints* const guessedConstraints /**< Initial guess for working set of constraints. */ + ); + +/** Determines if it is more efficient to refactorise the matrices when + * hotstarting or not (i.e. better to update the existing factorisations). + * \return BT_TRUE iff matrices shall be refactorised afresh + */ +BooleanType QProblem_shallRefactorise( QProblem* _THIS, + Bounds* const guessedBounds, /**< Guessed new working set of bounds. */ + Constraints* const guessedConstraints /**< Guessed new working set of constraints. */ + ); + +/** Setups internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + DenseMatrix *_A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + + +/** Sets up dense internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + real_t* const _A, /**< Constraint matrix. */ + const real_t* const _lb, /**< Lower bound vector (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bound vector (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + const real_t* const _lbA, /**< Lower constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const real_t* const _ubA /**< Upper constraints' bound vector. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_UNKNONW_BUG */ +returnValue QProblem_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const A_file, /**< Name of file where constraint matrix, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblem_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + const char* const lbA_file, /**< Name of file where lower constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no lower constraints' bounds exist, a NULL pointer can be passed. */ + const char* const ubA_file, /**< Name of file where upper constraints' bounds, of neighbouring QP to be solved, is stored. \n + If no upper constraints' bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new, /**< Output: Upper bounds of neighbouring QP to be solved */ + real_t* const lbA_new, /**< Output: Lower constraints' bounds of neighbouring QP to be solved */ + real_t* const ubA_new /**< Output: Upper constraints' bounds of neighbouring QP to be solved */ + ); + + +/** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblem_printIteration( QProblem* _THIS, + int iter, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status, /**< Status of blocking constraint. */ + BooleanType BC_isBound, /**< Indicates if blocking constraint is a bound. */ + real_t homotopyLength, /**< Current homotopy distance. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Sets constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setAM( QProblem* _THIS, + DenseMatrix *A_new /**< New constraint matrix. */ + ); + +/** Sets dense constraint matrix of the QP. \n + Note: Also internal vector Ax is recomputed! + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setA( QProblem* _THIS, + real_t* const A_new /**< New dense constraint matrix (with correct dimension!). */ + ); + + +/** Sets constraints' lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, + const real_t* const lbA_new /**< New constraints' lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower constraints' bound vector (with correct dimension!). */ + ); + +/** Sets constraints' upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, + const real_t* const ubA_new /**< New constraints' upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper constraints' bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper constraints' bound vector (with correct dimension!). */ + ); + + +/** Decides if lower bounds are smaller than upper bounds + * + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE */ +returnValue QProblem_areBoundsConsistent( QProblem* _THIS, + const real_t* const lb, /**< Vector of lower bounds*/ + const real_t* const ub, /**< Vector of upper bounds*/ + const real_t* const lbA, /**< Vector of lower constraints*/ + const real_t* const ubA /**< Vector of upper constraints*/ + ); + + +/** Drops the blocking bound/constraint that led to infeasibility, or finds another + * bound/constraint to drop according to drop priorities. + * \return SUCCESSFUL_RETURN \n + */ +returnValue QProblem_dropInfeasibles ( QProblem* _THIS, + int BC_number, /**< Number of the bound or constraint to be added */ + SubjectToStatus BC_status, /**< New status of the bound or constraint to be added */ + BooleanType BC_isBound, /**< Whether a bound or a constraint is to be added */ + real_t *xiB, + real_t *xiC + ); + + +/** If Hessian type has been set by the user, nothing is done. + * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or + * HST_POSDEF (default), respectively. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_INDEFINITE */ +returnValue QProblem_determineHessianType( QProblem* _THIS ); + +/** Computes the Cholesky decomposition of the (simply projected) Hessian + * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple + * projection matrix! + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemBCPY_computeCholesky( QProblem* _THIS ); + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_obtainAuxiliaryWorkingSet( QProblem* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Output: Working set for auxiliary QP. */ + ); + + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveR( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that _THIS function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblem_backsolveRrem( QProblem* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemBCPY_determineDataShift( QProblem* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bB_isZero /**< Output: Indicates if active bounds are to be shifted. */ + ); + + +/** Sets up internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_setupQPdataM( QProblem* _THIS, + DenseMatrix *_H, /**< Hessian matrix.*/ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdata( QProblem* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemBCPY_setupQPdataFromFile( QProblem* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemBCPY_loadQPvectorsFromFile( QProblem* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ + ); + + +/** Sets internal infeasibility flag and throws given error in case the far bound + * strategy is not enabled (as QP might actually not be infeasible in _THIS case). + * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_ENSURELI_FAILED_CYCLING \n + RET_ENSURELI_FAILED_NOINDEX */ +returnValue QProblem_setInfeasibilityFlag( QProblem* _THIS, + returnValue returnvalue, /**< Returnvalue to be tunneled. */ + BooleanType doThrowError /**< Flag forcing to throw an error. */ + ); + + +/** Determines if next QP iteration can be performed within given CPU time limit. + * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n + BT_FALSE: Sufficient CPU time for next QP iteration. */ +BooleanType QProblem_isCPUtimeLimitExceeded( QProblem* _THIS, + const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ + real_t starttime, /**< Start time of current QP solution. */ + int nWSR /**< Number of working set recalculations performed so far. */ + ); + + +/** Regularise Hessian matrix by adding a scaled identity matrix to it. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_ALREADY_REGULARISED */ +returnValue QProblem_regulariseHessian( QProblem* _THIS ); + + +/** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setHM( QProblem* _THIS, + DenseMatrix* H_new /**< New Hessian matrix. */ + ); + +/** Sets dense Hessian matrix of the QP. + * If a null pointer is passed and + * a) hessianType is HST_IDENTITY, nothing is done, + * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblem_setH( QProblem* _THIS, + real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ + ); + +/** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setG( QProblem* _THIS, + const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + +/** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setLB( QProblem* _THIS, + const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + +/** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblem_setUB( QProblem* _THIS, + const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblemBCPY_getRelativeHomotopyLength( QProblem* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new /**< Final upper variable bounds. */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblem_performRatioTestB( QProblem* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + +/** Checks whether given ratio is blocking, i.e. limits the maximum step length + * along the homotopy path to a value lower than given one. + * \return SUCCESSFUL_RETURN */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, /**< Numerator for performing the ratio test. */ + real_t den, /**< Denominator for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t /**< Input: Current maximum step length along the homotopy path, + * Output: Updated maximum possible step length along the homotopy path. */ + ); + + +/** ... + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpDataIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + +/** ... +* \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue QProblem_writeQpWorkspaceIntoMatFile( QProblem* _THIS, + const char* const filename /**< Mat file name. */ + ); + + +/* + * g e t B o u n d s + */ +static inline returnValue QProblem_getBounds( QProblem* _THIS, Bounds* _bounds ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + _bounds = _THIS->bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +static inline int QProblem_getNV( QProblem* _THIS ) +{ + return Bounds_getNV( _THIS->bounds ); +} + + +/* + * g e t N F R + */ +static inline int QProblem_getNFR( QProblem* _THIS ) +{ + return Bounds_getNFR( _THIS->bounds ); +} + + +/* + * g e t N F X + */ +static inline int QProblem_getNFX( QProblem* _THIS ) +{ + return Bounds_getNFX( _THIS->bounds ); +} + + +/* + * g e t N F V + */ +static inline int QProblem_getNFV( QProblem* _THIS ) +{ + return Bounds_getNFV( _THIS->bounds ); +} + + +/* + * g e t S t a t u s + */ +static inline QProblemStatus QProblem_getStatus( QProblem* _THIS ) +{ + return _THIS->status; +} + + +/* + * i s I n i t i a l i s e d + */ +static inline BooleanType QProblem_isInitialised( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +static inline BooleanType QProblem_isSolved( QProblem* _THIS ) +{ + if ( _THIS->status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +static inline BooleanType QProblem_isInfeasible( QProblem* _THIS ) +{ + return _THIS->infeasible; +} + + +/* + * i s U n b o u n d e d + */ +static inline BooleanType QProblem_isUnbounded( QProblem* _THIS ) +{ + return _THIS->unbounded; +} + + +/* + * g e t H e s s i a n T y p e + */ +static inline HessianType QProblem_getHessianType( QProblem* _THIS ) +{ + return _THIS->hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +static inline returnValue QProblem_setHessianType( QProblem* _THIS, HessianType _hessianType ) +{ + _THIS->hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + +/* + * u s i n g R e g u l a r i s a t i o n + */ +static inline BooleanType QProblem_usingRegularisation( QProblem* _THIS ) +{ + if ( _THIS->regVal > QPOASES_ZERO ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t O p t i o n s + */ +static inline Options QProblem_getOptions( QProblem* _THIS ) +{ + return _THIS->options; +} + + +/* + * s e t O p t i o n s + */ +static inline returnValue QProblem_setOptions( QProblem* _THIS, + Options _options + ) +{ + OptionsCPY( &_options,&(_THIS->options) ); + Options_ensureConsistency( &(_THIS->options) ); + + QProblem_setPrintLevel( _THIS,_THIS->options.printLevel ); + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t P r i n t L e v e l + */ +static inline PrintLevel QProblem_getPrintLevel( QProblem* _THIS ) +{ + return _THIS->options.printLevel; +} + + +/* + * g e t C o u n t + */ +static inline unsigned int QProblem_getCount( QProblem* _THIS ) +{ + return _THIS->count; +} + + +/* + * r e s e t C o u n t e r + */ +static inline returnValue QProblem_resetCounter( QProblem* _THIS ) +{ + _THIS->count = 0; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t H + */ +static inline returnValue QProblem_setHM( QProblem* _THIS, DenseMatrix* H_new ) +{ + if ( H_new == 0 ) + return QProblem_setH( _THIS,(real_t*)0 ); + else + return QProblem_setH( _THIS,DenseMatrix_getVal(H_new) ); +} + + +/* + * s e t H + */ +static inline returnValue QProblem_setH( QProblem* _THIS, real_t* const H_new ) +{ + /* if null pointer is passed, Hessian is set to zero matrix + * (or stays identity matrix) */ + if ( H_new == 0 ) + { + if ( _THIS->hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + _THIS->hessianType = HST_ZERO; + + _THIS->H = 0; + } + else + { + DenseMatrixCON( _THIS->H,QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),H_new ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t G + */ +static inline returnValue QProblem_setG( QProblem* _THIS, const real_t* const g_new ) +{ + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( g_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLB( QProblem* _THIS, const real_t* const lb_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lb_new != 0 ) + { + memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); + } + else + { + /* if no lower bounds are specified, set them to -infinity */ + for( i=0; ilb[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblem_setLBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUB( QProblem* _THIS, const real_t* const ub_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ub_new != 0 ) + { + memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); + } + else + { + /* if no upper bounds are specified, set them to infinity */ + for( i=0; iub[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B + */ +static inline returnValue QProblem_setUBn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + + +/* + * i s B l o c k i n g + */ +static inline BooleanType QProblem_isBlocking( QProblem* _THIS, + real_t num, + real_t den, + real_t epsNum, + real_t epsDen, + real_t* t + ) +{ + if ( ( den >= epsDen ) && ( num >= epsNum ) ) + { + if ( num < (*t)*den ) + return BT_TRUE; + } + + return BT_FALSE; +} + + + +/* + * g e t C o n s t r a i n t s + */ +static inline returnValue QProblem_getConstraints( QProblem* _THIS, Constraints* _constraints ) +{ + int nV = QProblem_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + ConstraintsCPY( _THIS->constraints,_constraints ); + + return SUCCESSFUL_RETURN; +} + + + +/* + * g e t N C + */ +static inline int QProblem_getNC( QProblem* _THIS ) +{ + return Constraints_getNC( _THIS->constraints ); +} + + +/* + * g e t N E C + */ +static inline int QProblem_getNEC( QProblem* _THIS ) +{ + return Constraints_getNEC( _THIS->constraints ); +} + + +/* + * g e t N A C + */ +static inline int QProblem_getNAC( QProblem* _THIS ) +{ + return Constraints_getNAC( _THIS->constraints ); +} + + +/* + * g e t N I A C + */ +static inline int QProblem_getNIAC( QProblem* _THIS ) +{ + return Constraints_getNIAC( _THIS->constraints ); +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t A + */ +static inline returnValue QProblem_setAM( QProblem* _THIS, DenseMatrix *A_new ) +{ + if ( A_new == 0 ) + return QProblem_setA( _THIS,(real_t*)0 ); + else + return QProblem_setA( _THIS,DenseMatrix_getVal(A_new) ); +} + + +/* + * s e t A + */ +static inline returnValue QProblem_setA( QProblem* _THIS, real_t* const A_new ) +{ + int j; + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( A_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + DenseMatrixCON( _THIS->A,QProblem_getNC( _THIS ),QProblem_getNV( _THIS ),QProblem_getNV( _THIS ),A_new ); + + DenseMatrix_times( _THIS->A,1, 1.0, _THIS->x, nV, 0.0, _THIS->Ax, nC); + + for( j=0; jAx_u[j] = _THIS->ubA[j] - _THIS->Ax[j]; + _THIS->Ax_l[j] = _THIS->Ax[j] - _THIS->lbA[j]; + + /* (ckirches) disable constraints with empty rows */ + if ( qpOASES_isZero( DenseMatrix_getRowNorm( _THIS->A,j,2 ),QPOASES_ZERO ) == BT_TRUE ) + Constraints_setType( _THIS->constraints,j,ST_DISABLED ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBA( QProblem* _THIS, const real_t* const lbA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lbA_new != 0 ) + { + memcpy( _THIS->lbA,lbA_new,nC*sizeof(real_t) ); + } + else + { + /* if no lower constraints' bounds are specified, set them to -infinity */ + for( i=0; ilbA[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B A + */ +static inline returnValue QProblem_setLBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->lbA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBA( QProblem* _THIS, const real_t* const ubA_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblem_getNV( _THIS ); + unsigned int nC = (unsigned int)QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ubA_new != 0 ) + { + memcpy( _THIS->ubA,ubA_new,nC*sizeof(real_t) ); + } + else + { + /* if no upper constraints' bounds are specified, set them to infinity */ + for( i=0; iubA[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B A + */ +static inline returnValue QProblem_setUBAn( QProblem* _THIS, int number, real_t value ) +{ + int nV = QProblem_getNV( _THIS ); + int nC = QProblem_getNC( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nC ) ) + { + _THIS->ubA[number] = value; + return SUCCESSFUL_RETURN; + } + else + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_QPROBLEM_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/QProblemB.h b/third_party/acados/include/qpOASES_e/QProblemB.h new file mode 100644 index 0000000..ee5157d --- /dev/null +++ b/third_party/acados/include/qpOASES_e/QProblemB.h @@ -0,0 +1,1641 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/QProblemB.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of the QProblemB class which is able to use the newly + * developed online active set strategy for parametric quadratic programming + * for problems with (simple) bounds only. + */ + + + +#ifndef QPOASES_QPROBLEMB_H +#define QPOASES_QPROBLEMB_H + + +#include +#include +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + Bounds *emptyBounds; + Bounds *auxiliaryBounds; + + real_t *ub_new_far; + real_t *lb_new_far; + + real_t *g_new; + real_t *lb_new; + real_t *ub_new; + + real_t *g_new2; + real_t *lb_new2; + real_t *ub_new2; + + real_t *Hx; + + real_t *_H; + + real_t *g_original; + real_t *lb_original; + real_t *ub_original; + + real_t *delta_xFR; + real_t *delta_xFX; + real_t *delta_yFX; + real_t *delta_g; + real_t *delta_lb; + real_t *delta_ub; + + real_t *gMod; + + real_t *num; + real_t *den; + + real_t *rhs; + real_t *r; +} QProblemB_ws; + +int QProblemB_ws_calculateMemorySize( unsigned int nV ); + +char *QProblemB_ws_assignMemory( unsigned int nV, QProblemB_ws **mem, void *raw_memory ); + +QProblemB_ws *QProblemB_ws_createMemory( unsigned int nV ); + + +/** + * \brief Implements the online active set strategy for box-constrained QPs. + * + * Class for setting up and solving quadratic programs with bounds (= box constraints) only. + * The main feature is the possibility to use the newly developed online active set strategy + * for parametric quadratic programming. + * + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + */ +typedef struct +{ + QProblemB_ws *ws; + Bounds *bounds; /**< Data structure for problem's bounds. */ + Flipper *flipper; /**< Struct for making a temporary copy of the matrix factorisations. */ + + DenseMatrix* H; /**< Hessian matrix pointer. */ + + Options options; /**< Struct containing all user-defined options for solving QPs. */ + TabularOutput tabularOutput; /**< Struct storing information for tabular output (printLevel == PL_TABULAR). */ + + real_t *g; /**< Gradient. */ + real_t *lb; /**< Lower bound vector (on variables). */ + real_t *ub; /**< Upper bound vector (on variables). */ + + real_t *R; /**< Cholesky factor of H (i.e. H = R^T*R). */ + + real_t *x; /**< Primal solution vector. */ + real_t *y; /**< Dual solution vector. */ + + real_t *delta_xFR_TMP; /**< Temporary for determineStepDirection */ + + real_t tau; /**< Last homotopy step length. */ + real_t regVal; /**< Holds the offset used to regularise Hessian matrix (zero by default). */ + + real_t ramp0; /**< Start value for Ramping Strategy. */ + real_t ramp1; /**< Final value for Ramping Strategy. */ + + QProblemStatus status; /**< Current status of the solution process. */ + HessianType hessianType; /**< Type of Hessian matrix. */ + + BooleanType haveCholesky; /**< Flag indicating whether Cholesky decomposition has already been setup. */ + BooleanType infeasible; /**< QP infeasible? */ + BooleanType unbounded; /**< QP unbounded? */ + + int rampOffset; /**< Offset index for Ramping. */ + unsigned int count; /**< Counts the number of hotstart function calls (internal usage only!). */ +} QProblemB; + +int QProblemB_calculateMemorySize( unsigned int nV ); + +char *QProblemB_assignMemory( unsigned int nV, QProblemB **mem, void *raw_memory ); + +QProblemB *QProblemB_createMemory( unsigned int nV ); + + +/** Constructor which takes the QP dimension and Hessian type + * information. If the Hessian is the zero (i.e. HST_ZERO) or the + * identity matrix (i.e. HST_IDENTITY), respectively, no memory + * is allocated for it and a NULL pointer can be passed for it + * to the init() functions. */ +void QProblemBCON( QProblemB* _THIS, + int _nV, /**< Number of variables. */ + HessianType _hessianType /**< Type of Hessian matrix. */ + ); + +void QProblemBCPY( QProblemB* FROM, + QProblemB* TO + ); + + +/** Clears all data structures of QProblemB except for QP data. + * \return SUCCESSFUL_RETURN \n + RET_RESET_FAILED */ +returnValue QProblemB_reset( QProblemB* _THIS ); + + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initM( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_init( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it + * using at most nWSR iterations. + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblemB_initF( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation (if pointer passed). */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initMW( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a simply bounded QP problem with given QP data and tries to solve it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_initW( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub, /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const real_t* const _R /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. + The Cholesky factor must be stored in a real_t array of size nV*nV + in row-major format. Note: Only used if xOpt/yOpt and gB are NULL! \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + +/** Initialises a simply bounded QP problem with given QP data to be read from files and solves it + * using at most nWSR iterations. Depending on the parameter constellation it: \n + * 1. 0, 0, 0 : starts with xOpt = 0, yOpt = 0 and gB empty (or all implicit equality bounds), \n + * 2. xOpt, 0, 0 : starts with xOpt, yOpt = 0 and obtain gB by "clipping", \n + * 3. 0, yOpt, 0 : starts with xOpt = 0, yOpt and obtain gB from yOpt != 0, \n + * 4. 0, 0, gB: starts with xOpt = 0, yOpt = 0 and gB, \n + * 5. xOpt, yOpt, 0 : starts with xOpt, yOpt and obtain gB from yOpt != 0, \n + * 6. xOpt, 0, gB: starts with xOpt, yOpt = 0 and gB, \n + * 7. xOpt, yOpt, gB: starts with xOpt, yOpt and gB (assume them to be consistent!) + * + * Note: This function internally calls solveInitialQP for initialisation! + * + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED \n + RET_UNABLE_TO_READ_FILE */ +returnValue QProblemB_initFW( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix is stored. \n + If Hessian matrix is trivial, a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient vector is stored. */ + const char* const lb_file, /**< Name of file where lower bound vector. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bound vector. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations when using initial homotopy. \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP initialisation. \n + Output: CPU time spent for QP initialisation. */ + const real_t* const xOpt, /**< Optimal primal solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old primal solution is kept!) */ + const real_t* const yOpt, /**< Optimal dual solution vector. A NULL pointer can be passed. \n + (If a null pointer is passed, the old dual solution is kept!) */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, all bounds are assumed inactive!) */ + const char* const R_file /**< Name of the file where a pre-computed (upper triangular) Cholesky factor + of the Hessian matrix is stored. \n + (If a null pointer is passed, Cholesky decomposition is computed internally!) */ + ); + + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_hotstart( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. QP solution is started from previous solution. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_hotstartF( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QP sequence using the online active set strategy. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_hotstartW( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set is kept!) */ + ); + +/** Solves an initialised QP sequence using the online active set strategy, + * where QP data is read from files. + * By default, QP solution is started from previous solution. If a guess + * for the working set is provided, an initialised homotopy is performed. + * + * Note: This function internally calls solveQP/solveRegularisedQP + * for solving an initialised QP! + * + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_hotstartFW( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + Bounds* const guessedBounds /**< Optimal working set of bounds for solution (xOpt,yOpt). \n + (If a null pointer is passed, the previous working set is kept!) */ + ); + + +/** Writes a vector with the state of the working set + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSet( QProblemB* _THIS, + real_t* workingSet /** Output: array containing state of the working set. */ + ); + +/** Writes a vector with the state of the working set of bounds + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSetBounds( QProblemB* _THIS, + real_t* workingSetB /** Output: array containing state of the working set of bounds. */ + ); + +/** Writes a vector with the state of the working set of constraints + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue QProblemB_getWorkingSetConstraints( QProblemB* _THIS, + real_t* workingSetC /** Output: array containing state of the working set of constraints. */ + ); + + +/** Returns current bounds object of the QP (deep copy). + * \return SUCCESSFUL_RETURN \n + RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_getBounds( QProblemB* _THIS, + Bounds* _bounds /** Output: Bounds object. */ + ); + + +/** Returns the number of variables. + * \return Number of variables. */ +static inline int QProblemB_getNV( QProblemB* _THIS ); + +/** Returns the number of free variables. + * \return Number of free variables. */ +static inline int QProblemB_getNFR( QProblemB* _THIS ); + +/** Returns the number of fixed variables. + * \return Number of fixed variables. */ +static inline int QProblemB_getNFX( QProblemB* _THIS ); + +/** Returns the number of implicitly fixed variables. + * \return Number of implicitly fixed variables. */ +static inline int QProblemB_getNFV( QProblemB* _THIS ); + +/** Returns the dimension of null space. + * \return Dimension of null space. */ +int QProblemB_getNZ( QProblemB* _THIS ); + + +/** Returns the optimal objective function value. + * \return finite value: Optimal objective function value (QP was solved) \n + +infinity: QP was not yet solved */ +real_t QProblemB_getObjVal( QProblemB* _THIS ); + +/** Returns the objective function value at an arbitrary point x. + * \return Objective function value at point x */ +real_t QProblemB_getObjValX( QProblemB* _THIS, + const real_t* const _x /**< Point at which the objective function shall be evaluated. */ + ); + +/** Returns the primal solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblemB_getPrimalSolution( QProblemB* _THIS, + real_t* const xOpt /**< Output: Primal solution vector (if QP has been solved). */ + ); + +/** Returns the dual solution vector. + * \return SUCCESSFUL_RETURN \n + RET_QP_NOT_SOLVED */ +returnValue QProblemB_getDualSolution( QProblemB* _THIS, + real_t* const yOpt /**< Output: Dual solution vector (if QP has been solved). */ + ); + + +/** Returns status of the solution process. + * \return Status of solution process. */ +static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS ); + + +/** Returns if the QProblem object is initialised. + * \return BT_TRUE: QProblemB initialised \n + BT_FALSE: QProblemB not initialised */ +static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS ); + +/** Returns if the QP has been solved. + * \return BT_TRUE: QProblemB solved \n + BT_FALSE: QProblemB not solved */ +static inline BooleanType QProblemB_isSolved( QProblemB* _THIS ); + +/** Returns if the QP is infeasible. + * \return BT_TRUE: QP infeasible \n + BT_FALSE: QP feasible (or not known to be infeasible!) */ +static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS ); + +/** Returns if the QP is unbounded. + * \return BT_TRUE: QP unbounded \n + BT_FALSE: QP unbounded (or not known to be unbounded!) */ +static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS ); + + +/** Returns Hessian type flag (type is not determined due to _THIS call!). + * \return Hessian type. */ +static inline HessianType QProblemB_getHessianType( QProblemB* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setHessianType( QProblemB* _THIS, + HessianType _hessianType /**< New Hessian type. */ + ); + +/** Returns if the QP has been internally regularised. + * \return BT_TRUE: Hessian is internally regularised for QP solution \n + BT_FALSE: No internal Hessian regularisation is used for QP solution */ +static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS ); + +/** Returns current options struct. + * \return Current options struct. */ +static inline Options QProblemB_getOptions( QProblemB* _THIS ); + +/** Overrides current options with given ones. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setOptions( QProblemB* _THIS, + Options _options /**< New options. */ + ); + +/** Returns the print level. + * \return Print level. */ +static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS ); + +/** Changes the print level. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setPrintLevel( QProblemB* _THIS, + PrintLevel _printlevel /**< New print level. */ + ); + +/** Returns the current number of QP problems solved. + * \return Number of QP problems solved. */ +static inline unsigned int QProblemB_getCount( QProblemB* _THIS ); + +/** Resets QP problem counter (to zero). + * \return SUCCESSFUL_RETURN. */ +static inline returnValue QProblemB_resetCounter( QProblemB* _THIS ); + + +/** Prints concise list of properties of the current QP. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printProperties( QProblemB* _THIS ); + +/** Prints a list of all options and their current values. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printOptions( QProblemB* _THIS ); + + +/** If Hessian type has been set by the user, nothing is done. + * Otherwise the Hessian type is set to HST_IDENTITY, HST_ZERO, or + * HST_POSDEF (default), respectively. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_INDEFINITE */ +returnValue QProblemB_determineHessianType( QProblemB* _THIS ); + +/** Determines type of existing constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblemB_setupSubjectToType( QProblemB* _THIS ); + +/** Determines type of new constraints and bounds (i.e. implicitly fixed, unbounded etc.). + * \return SUCCESSFUL_RETURN \n + RET_SETUPSUBJECTTOTYPE_FAILED */ +returnValue QProblemB_setupSubjectToTypeNew( QProblemB* _THIS, + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new /**< New upper bounds. */ + ); + +/** Computes the Cholesky decomposition of the (simply projected) Hessian + * (i.e. R^T*R = Z^T*H*Z). It only works in the case where Z is a simple + * projection matrix! + * Note: If Hessian turns out not to be positive definite, the Hessian type + * is set to HST_SEMIDEF accordingly. + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemB_computeCholesky( QProblemB* _THIS ); + +/** Computes initial Cholesky decomposition of the projected Hessian making + * use of the function setupCholeskyDecomposition() or setupCholeskyDecompositionProjected(). + * \return SUCCESSFUL_RETURN \n + * RET_HESSIAN_NOT_SPD \n + * RET_INDEXLIST_CORRUPTED */ +returnValue QProblemB_setupInitialCholesky( QProblemB* _THIS ); + + +/** Obtains the desired working set for the auxiliary initial QP in + * accordance with the user specifications + * \return SUCCESSFUL_RETURN \n + RET_OBTAINING_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_obtainAuxiliaryWorkingSet( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + const real_t* const yOpt, /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are assumed to be zero. */ + Bounds* const guessedBounds, /**< Guessed working set for solution (xOpt,yOpt). */ + Bounds* auxiliaryBounds /**< Input: Allocated bound object. \n + * Ouput: Working set for auxiliary QP. */ + ); + +/** Decides if lower bounds are smaller than upper bounds + * + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE */ +returnValue QProblemB_areBoundsConsistent( QProblemB* _THIS, + const real_t* const lb, /**< Vector of lower bounds*/ + const real_t* const ub /**< Vector of upper bounds*/ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblemB_backsolveR( QProblemB* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + real_t* const a /**< Output: Solution vector */ + ); + +/** Solves the system Ra = b or R^Ta = b where R is an upper triangular matrix. \n + * Special variant for the case that _THIS function is called from within "removeBound()". + * \return SUCCESSFUL_RETURN \n + RET_DIV_BY_ZERO */ +returnValue QProblemB_backsolveRrem( QProblemB* _THIS, + const real_t* const b, /**< Right hand side vector. */ + BooleanType transposed, /**< Indicates if the transposed system shall be solved. */ + BooleanType removingBound, /**< Indicates if function is called from "removeBound()". */ + real_t* const a /**< Output: Solution vector */ + ); + + +/** Determines step direction of the shift of the QP data. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_determineDataShift( QProblemB* _THIS, + const real_t* const g_new, /**< New gradient vector. */ + const real_t* const lb_new, /**< New lower bounds. */ + const real_t* const ub_new, /**< New upper bounds. */ + real_t* const delta_g, /**< Output: Step direction of gradient vector. */ + real_t* const delta_lb, /**< Output: Step direction of lower bounds. */ + real_t* const delta_ub, /**< Output: Step direction of upper bounds. */ + BooleanType* Delta_bB_isZero/**< Output: Indicates if active bounds are to be shifted. */ + ); + + +/** Sets up internal QP data. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_setupQPdataM( QProblemB* _THIS, + DenseMatrix *_H, /**< Hessian matrix.*/ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data. If the current Hessian is trivial + * (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemB_setupQPdata( QProblemB* _THIS, + real_t* const _H, /**< Hessian matrix. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const real_t* const _g, /**< Gradient vector. */ + const real_t* const _lb, /**< Lower bounds (on variables). \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const _ub /**< Upper bounds (on variables). \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Sets up internal QP data by loading it from files. If the current Hessian + * is trivial (i.e. HST_ZERO or HST_IDENTITY) but a non-trivial one is given, + * memory for Hessian is allocated and it is set to the given one. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS \n + RET_NO_HESSIAN_SPECIFIED */ +returnValue QProblemB_setupQPdataFromFile( QProblemB* _THIS, + const char* const H_file, /**< Name of file where Hessian matrix, of neighbouring QP to be solved, is stored. \n + If Hessian matrix is trivial,a NULL pointer can be passed. */ + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + ); + +/** Loads new QP vectors from files (internal members are not affected!). + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE \n + RET_INVALID_ARGUMENTS */ +returnValue QProblemB_loadQPvectorsFromFile( QProblemB* _THIS, + const char* const g_file, /**< Name of file where gradient, of neighbouring QP to be solved, is stored. */ + const char* const lb_file, /**< Name of file where lower bounds, of neighbouring QP to be solved, is stored. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const char* const ub_file, /**< Name of file where upper bounds, of neighbouring QP to be solved, is stored. \n + If no upper bounds exist, a NULL pointer can be passed. */ + real_t* const g_new, /**< Output: Gradient of neighbouring QP to be solved. */ + real_t* const lb_new, /**< Output: Lower bounds of neighbouring QP to be solved */ + real_t* const ub_new /**< Output: Upper bounds of neighbouring QP to be solved */ + ); + + +/** Sets internal infeasibility flag and throws given error in case the far bound + * strategy is not enabled (as QP might actually not be infeasible in _THIS case). + * \return RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_ENSURELI_FAILED_CYCLING \n + RET_ENSURELI_FAILED_NOINDEX */ +returnValue QProblemB_setInfeasibilityFlag( QProblemB* _THIS, + returnValue returnvalue, /**< Returnvalue to be tunneled. */ + BooleanType doThrowError /**< Flag forcing to throw an error. */ + ); + + +/** Determines if next QP iteration can be performed within given CPU time limit. + * \return BT_TRUE: CPU time limit is exceeded, stop QP solution. \n + BT_FALSE: Sufficient CPU time for next QP iteration. */ +BooleanType QProblemB_isCPUtimeLimitExceeded( QProblemB* _THIS, + const real_t* const cputime, /**< Maximum CPU time allowed for QP solution. */ + real_t starttime, /**< Start time of current QP solution. */ + int nWSR /**< Number of working set recalculations performed so far. */ + ); + + +/** Regularise Hessian matrix by adding a scaled identity matrix to it. + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_ALREADY_REGULARISED */ +returnValue QProblemB_regulariseHessian( QProblemB* _THIS ); + + +/** Sets Hessian matrix of the QP. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setHM( QProblemB* _THIS, + DenseMatrix* H_new /**< New Hessian matrix. */ + ); + +/** Sets dense Hessian matrix of the QP. + * If a null pointer is passed and + * a) hessianType is HST_IDENTITY, nothing is done, + * b) hessianType is not HST_IDENTITY, Hessian matrix is set to zero. + * \return SUCCESSFUL_RETURN */ +static inline returnValue QProblemB_setH( QProblemB* _THIS, + real_t* const H_new /**< New dense Hessian matrix (with correct dimension!). */ + ); + +/** Changes gradient vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +static inline returnValue QProblemB_setG( QProblemB* _THIS, + const real_t* const g_new /**< New gradient vector (with correct dimension!). */ + ); + +/** Changes lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_setLB( QProblemB* _THIS, + const real_t* const lb_new /**< New lower bound vector (with correct dimension!). */ + ); + +/** Changes single entry of lower bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblemB_setLBn( QProblemB* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of lower bound vector. */ + ); + +/** Changes upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP */ +static inline returnValue QProblemB_setUB( QProblemB* _THIS, + const real_t* const ub_new /**< New upper bound vector (with correct dimension!). */ + ); + +/** Changes single entry of upper bound vector of the QP. + * \return SUCCESSFUL_RETURN \n + * RET_QPOBJECT_NOT_SETUP \n + * RET_INDEX_OUT_OF_BOUNDS */ +static inline returnValue QProblemB_setUBn( QProblemB* _THIS, + int number, /**< Number of entry to be changed. */ + real_t value /**< New value for entry of upper bound vector. */ + ); + + +/** Computes parameters for the Givens matrix G for which [x,y]*G = [z,0] + * \return SUCCESSFUL_RETURN */ +static inline void QProblemB_computeGivens( real_t xold, /**< Matrix entry to be normalised. */ + real_t yold, /**< Matrix entry to be annihilated. */ + real_t* xnew, /**< Output: Normalised matrix entry. */ + real_t* ynew, /**< Output: Annihilated matrix entry. */ + real_t* c, /**< Output: Cosine entry of Givens matrix. */ + real_t* s /**< Output: Sine entry of Givens matrix. */ + ); + +/** Applies Givens matrix determined by c and s (cf. computeGivens). + * \return SUCCESSFUL_RETURN */ +static inline void QProblemB_applyGivens( real_t c, /**< Cosine entry of Givens matrix. */ + real_t s, /**< Sine entry of Givens matrix. */ + real_t nu, /**< Further factor: s/(1+c). */ + real_t xold, /**< Matrix entry to be transformed corresponding to + * the normalised entry of the original matrix. */ + real_t yold, /**< Matrix entry to be transformed corresponding to + * the annihilated entry of the original matrix. */ + real_t* xnew, /**< Output: Transformed matrix entry corresponding to + * the normalised entry of the original matrix. */ + real_t* ynew /**< Output: Transformed matrix entry corresponding to + * the annihilated entry of the original matrix. */ + ); + + + +/** Compute relative length of homotopy in data space for termination + * criterion. + * \return Relative length in data space. */ +real_t QProblemB_getRelativeHomotopyLength( QProblemB* _THIS, + const real_t* const g_new, /**< Final gradient. */ + const real_t* const lb_new, /**< Final lower variable bounds. */ + const real_t* const ub_new /**< Final upper variable bounds. */ + ); + +/** Ramping Strategy to avoid ties. Modifies homotopy start without + * changing current active set. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performRamping( QProblemB* _THIS ); + + +/** ... */ +returnValue QProblemB_updateFarBounds( QProblemB* _THIS, + real_t curFarBound, /**< ... */ + int nRamp, /**< ... */ + const real_t* const lb_new, /**< ... */ + real_t* const lb_new_far, /**< ... */ + const real_t* const ub_new, /**< ... */ + real_t* const ub_new_far /**< ... */ + ); + + + +/** Performs robustified ratio test yield the maximum possible step length + * along the homotopy path. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performRatioTestB( QProblemB* _THIS, + int nIdx, /**< Number of ratios to be checked. */ + const int* const idxList, /**< Array containing the indices of all ratios to be checked. */ + Bounds* const subjectTo, /**< Bound object corresponding to ratios to be checked. */ + const real_t* const num, /**< Array containing all numerators for performing the ratio test. */ + const real_t* const den, /**< Array containing all denominators for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t, /**< Output: Maximum possible step length along the homotopy path. */ + int* BC_idx /**< Output: Index of blocking constraint. */ + ); + +/** Checks whether given ratio is blocking, i.e. limits the maximum step length + * along the homotopy path to a value lower than given one. + * \return SUCCESSFUL_RETURN */ +static inline BooleanType QProblemB_isBlocking( QProblemB* _THIS, + real_t num, /**< Numerator for performing the ratio test. */ + real_t den, /**< Denominator for performing the ratio test. */ + real_t epsNum, /**< Numerator tolerance. */ + real_t epsDen, /**< Denominator tolerance. */ + real_t* t /**< Input: Current maximum step length along the homotopy path, + * Output: Updated maximum possible step length along the homotopy path. */ + ); + + +/** Solves a QProblemB whose QP data is assumed to be stored in the member variables. + * A guess for its primal/dual optimal solution vectors and the corresponding + * optimal working set can be provided. + * Note: This function is internally called by all init functions! + * \return SUCCESSFUL_RETURN \n + RET_INIT_FAILED \n + RET_INIT_FAILED_CHOLESKY \n + RET_INIT_FAILED_HOTSTART \n + RET_INIT_FAILED_INFEASIBILITY \n + RET_INIT_FAILED_UNBOUNDEDNESS \n + RET_MAX_NWSR_REACHED */ +returnValue QProblemB_solveInitialQP( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector.*/ + const real_t* const yOpt, /**< Optimal dual solution vector. */ + Bounds* const guessedBounds, /**< Optimal working set of bounds for solution (xOpt,yOpt). */ + const real_t* const _R, /**< Pre-computed (upper triangular) Cholesky factor of Hessian matrix. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + * Output: Number of performed working set recalculations. */ + real_t* const cputime /**< Input: Maximum CPU time allowed for QP solution. \n + * Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + ); + +/** Solves an initialised QProblemB using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_solveQP( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveQP() calls. This number is + always zero, except for successive calls from solveRegularisedQP() + or when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Solves an initialised QProblemB using online active set strategy. + * Note: This function is internally called by all hotstart functions! + * \return SUCCESSFUL_RETURN \n + RET_MAX_NWSR_REACHED \n + RET_HOTSTART_FAILED_AS_QP_NOT_INITIALISED \n + RET_HOTSTART_FAILED \n + RET_SHIFT_DETERMINATION_FAILED \n + RET_STEPDIRECTION_DETERMINATION_FAILED \n + RET_STEPLENGTH_DETERMINATION_FAILED \n + RET_HOMOTOPY_STEP_FAILED \n + RET_HOTSTART_STOPPED_INFEASIBILITY \n + RET_HOTSTART_STOPPED_UNBOUNDEDNESS */ +returnValue QProblemB_solveRegularisedQP( QProblemB* _THIS, + const real_t* const g_new, /**< Gradient of neighbouring QP to be solved. */ + const real_t* const lb_new, /**< Lower bounds of neighbouring QP to be solved. \n + If no lower bounds exist, a NULL pointer can be passed. */ + const real_t* const ub_new, /**< Upper bounds of neighbouring QP to be solved. \n + If no upper bounds exist, a NULL pointer can be passed. */ + int* nWSR, /**< Input: Maximum number of working set recalculations; \n + Output: Number of performed working set recalculations. */ + real_t* const cputime, /**< Input: Maximum CPU time allowed for QP solution. \n + Output: CPU time spent for QP solution (or to perform nWSR iterations). */ + int nWSRperformed, /**< Number of working set recalculations already performed to solve + this QP within previous solveRegularisedQP() calls. This number is + always zero, except for successive calls when using the far bound strategy. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + +/** Sets up bound data structure according to auxiliaryBounds. + * (If the working set shall be setup afresh, make sure that + * bounds data structure has been resetted!) + * \return SUCCESSFUL_RETURN \n + RET_SETUP_WORKINGSET_FAILED \n + RET_INVALID_ARGUMENTS \n + RET_UNKNOWN_BUG */ +returnValue QProblemB_setupAuxiliaryWorkingSet( QProblemB* _THIS, + Bounds* const auxiliaryBounds, /**< Working set for auxiliary QP. */ + BooleanType setupAfresh /**< Flag indicating if given working set shall be + * setup afresh or by updating the current one. */ + ); + +/** Sets up the optimal primal/dual solution of the auxiliary initial QP. + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setupAuxiliaryQPsolution( QProblemB* _THIS, + const real_t* const xOpt, /**< Optimal primal solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + const real_t* const yOpt /**< Optimal dual solution vector. + * If a NULL pointer is passed, all entries are set to zero. */ + ); + +/** Sets up gradient of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been (ialised!). + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_setupAuxiliaryQPgradient( QProblemB* _THIS ); + +/** Sets up bounds of the auxiliary initial QP for given + * optimal primal/dual solution and given initial working set + * (assumes that members X, Y and BOUNDS have already been initialised!). + * \return SUCCESSFUL_RETURN \n + RET_UNKNOWN_BUG */ +returnValue QProblemB_setupAuxiliaryQPbounds( QProblemB* _THIS, + BooleanType useRelaxation /**< Flag indicating if inactive bounds shall be relaxed. */ + ); + + +/** Updates QP vectors, working sets and internal data structures in order to + start from an optimal solution corresponding to initial guesses of the working + set for bounds + * \return SUCCESSFUL_RETURN \n + * RET_SETUP_AUXILIARYQP_FAILED */ +returnValue QProblemB_setupAuxiliaryQP( QProblemB* _THIS, + Bounds* const guessedBounds /**< Initial guess for working set of bounds. */ + ); + +/** Determines step direction of the homotopy path. + * \return SUCCESSFUL_RETURN \n + RET_STEPDIRECTION_FAILED_CHOLESKY */ +returnValue QProblemB_determineStepDirection( QProblemB* _THIS, + const real_t* const delta_g, /**< Step direction of gradient vector. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + BooleanType Delta_bB_isZero, /**< Indicates if active bounds are to be shifted. */ + real_t* const delta_xFX, /**< Output: Primal homotopy step direction of fixed variables. */ + real_t* const delta_xFR, /**< Output: Primal homotopy step direction of free variables. */ + real_t* const delta_yFX /**< Output: Dual homotopy step direction of fixed variables' multiplier. */ + ); + +/** Determines the maximum possible step length along the homotopy path + * and performs _THIS step (without changing working set). + * \return SUCCESSFUL_RETURN \n + * RET_QP_INFEASIBLE \n + */ +returnValue QProblemB_performStep( QProblemB* _THIS, + const real_t* const delta_g, /**< Step direction of gradient. */ + const real_t* const delta_lb, /**< Step direction of lower bounds. */ + const real_t* const delta_ub, /**< Step direction of upper bounds. */ + const real_t* const delta_xFX, /**< Primal homotopy step direction of fixed variables. */ + const real_t* const delta_xFR, /**< Primal homotopy step direction of free variables. */ + const real_t* const delta_yFX, /**< Dual homotopy step direction of fixed variables' multiplier. */ + int* BC_idx, /**< Output: Index of blocking constraint. */ + SubjectToStatus* BC_status /**< Output: Status of blocking constraint. */ + ); + +/** Updates active set. + * \return SUCCESSFUL_RETURN \n + RET_REMOVE_FROM_ACTIVESET_FAILED \n + RET_ADD_TO_ACTIVESET_FAILED */ +returnValue QProblemB_changeActiveSet( QProblemB* _THIS, + int BC_idx, /**< Index of blocking constraint. */ + SubjectToStatus BC_status /**< Status of blocking constraint. */ + ); + +/** Drift correction at end of each active set iteration + * \return SUCCESSFUL_RETURN */ +returnValue QProblemB_performDriftCorrection( QProblemB* _THIS ); + +/** Determines if it is more efficient to refactorise the matrices when + * hotstarting or not (i.e. better to update the existing factorisations). + * \return BT_TRUE iff matrices shall be refactorised afresh + */ +BooleanType QProblemB_shallRefactorise( QProblemB* _THIS, + Bounds* const guessedBounds /**< Guessed new working set. */ + ); + + +/** Adds a bound to active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_ADDBOUND_FAILED */ +returnValue QProblemB_addBound( QProblemB* _THIS, + int number, /**< Number of bound to be added to active set. */ + SubjectToStatus B_status, /**< Status of new active bound. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + +/** Removes a bounds from active set (specialised version for the case where no constraints exist). + * \return SUCCESSFUL_RETURN \n + RET_HESSIAN_NOT_SPD \n + RET_REMOVEBOUND_FAILED */ +returnValue QProblemB_removeBound( QProblemB* _THIS, + int number, /**< Number of bound to be removed from active set. */ + BooleanType updateCholesky /**< Flag indicating if Cholesky decomposition shall be updated. */ + ); + + +/** Prints concise information on the current iteration. + * \return SUCCESSFUL_RETURN \n */ +returnValue QProblemB_printIteration( QProblemB* _THIS, + int iter, /**< Number of current iteration. */ + int BC_idx, /**< Index of blocking bound. */ + SubjectToStatus BC_status, /**< Status of blocking bound. */ + real_t homotopyLength, /**< Current homotopy distance. */ + BooleanType isFirstCall /**< Indicating whether this is the first call for current QP. */ + ); + + + +/* + * g e t B o u n d s + */ +static inline returnValue QProblemB_getBounds( QProblemB* _THIS, Bounds* _bounds ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + _bounds = _THIS->bounds; + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t N V + */ +static inline int QProblemB_getNV( QProblemB* _THIS ) +{ + return Bounds_getNV( _THIS->bounds ); +} + + +/* + * g e t N F R + */ +static inline int QProblemB_getNFR( QProblemB* _THIS ) +{ + return Bounds_getNFR( _THIS->bounds ); +} + + +/* + * g e t N F X + */ +static inline int QProblemB_getNFX( QProblemB* _THIS ) +{ + return Bounds_getNFX( _THIS->bounds ); +} + + +/* + * g e t N F V + */ +static inline int QProblemB_getNFV( QProblemB* _THIS ) +{ + return Bounds_getNFV( _THIS->bounds ); +} + + +/* + * g e t S t a t u s + */ +static inline QProblemStatus QProblemB_getStatus( QProblemB* _THIS ) +{ + return _THIS->status; +} + + +/* + * i s I n i t i a l i s e d + */ +static inline BooleanType QProblemB_isInitialised( QProblemB* _THIS ) +{ + if ( _THIS->status == QPS_NOTINITIALISED ) + return BT_FALSE; + else + return BT_TRUE; +} + + +/* + * i s S o l v e d + */ +static inline BooleanType QProblemB_isSolved( QProblemB* _THIS ) +{ + if ( _THIS->status == QPS_SOLVED ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s I n f e a s i b l e + */ +static inline BooleanType QProblemB_isInfeasible( QProblemB* _THIS ) +{ + return _THIS->infeasible; +} + + +/* + * i s U n b o u n d e d + */ +static inline BooleanType QProblemB_isUnbounded( QProblemB* _THIS ) +{ + return _THIS->unbounded; +} + + +/* + * g e t H e s s i a n T y p e + */ +static inline HessianType QProblemB_getHessianType( QProblemB* _THIS ) +{ + return _THIS->hessianType; +} + + +/* + * s e t H e s s i a n T y p e + */ +static inline returnValue QProblemB_setHessianType( QProblemB* _THIS, HessianType _hessianType ) +{ + _THIS->hessianType = _hessianType; + return SUCCESSFUL_RETURN; +} + + +/* + * u s i n g R e g u l a r i s a t i o n + */ +static inline BooleanType QProblemB_usingRegularisation( QProblemB* _THIS ) +{ + if ( _THIS->regVal > QPOASES_ZERO ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t O p t i o n s + */ +static inline Options QProblemB_getOptions( QProblemB* _THIS ) +{ + return _THIS->options; +} + + +/* + * s e t O p t i o n s + */ +static inline returnValue QProblemB_setOptions( QProblemB* _THIS, + Options _options + ) +{ + OptionsCPY( &_options,&(_THIS->options) ); + Options_ensureConsistency( &(_THIS->options) ); + + QProblemB_setPrintLevel( _THIS,_THIS->options.printLevel ); + + return SUCCESSFUL_RETURN; +} + + +/* + * g e t P r i n t L e v e l + */ +static inline PrintLevel QProblemB_getPrintLevel( QProblemB* _THIS ) +{ + return _THIS->options.printLevel; +} + + + +/* + * g e t C o u n t + */ +static inline unsigned int QProblemB_getCount( QProblemB* _THIS ) +{ + return _THIS->count; +} + + +/* + * r e s e t C o u n t e r + */ +static inline returnValue QProblemB_resetCounter( QProblemB* _THIS ) +{ + _THIS->count = 0; + return SUCCESSFUL_RETURN; +} + + + +/***************************************************************************** + * P R O T E C T E D * + *****************************************************************************/ + + +/* + * s e t H + */ +static inline returnValue QProblemB_setHM( QProblemB* _THIS, DenseMatrix* H_new ) +{ + if ( H_new == 0 ) + return QProblemB_setH( _THIS,(real_t*)0 ); + else + return QProblemB_setH( _THIS,DenseMatrix_getVal(H_new) ); +} + + +/* + * s e t H + */ +static inline returnValue QProblemB_setH( QProblemB* _THIS, real_t* const H_new ) +{ + /* if null pointer is passed, Hessian is set to zero matrix + * (or stays identity matrix) */ + if ( H_new == 0 ) + { + if ( _THIS->hessianType == HST_IDENTITY ) + return SUCCESSFUL_RETURN; + + _THIS->hessianType = HST_ZERO; + + _THIS->H = 0; + } + else + { + DenseMatrixCON( _THIS->H,QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),QProblemB_getNV( _THIS ),H_new ); + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t G + */ +static inline returnValue QProblemB_setG( QProblemB* _THIS, const real_t* const g_new ) +{ + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( g_new == 0 ) + return THROWERROR( RET_INVALID_ARGUMENTS ); + + memcpy( _THIS->g,g_new,nV*sizeof(real_t) ); + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblemB_setLB( QProblemB* _THIS, const real_t* const lb_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( lb_new != 0 ) + { + memcpy( _THIS->lb,lb_new,nV*sizeof(real_t) ); + } + else + { + /* if no lower bounds are specified, set them to -infinity */ + for( i=0; ilb[i] = -QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t L B + */ +static inline returnValue QProblemB_setLBn( QProblemB* _THIS, int number, real_t value ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->lb[number] = value; + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * s e t U B + */ +static inline returnValue QProblemB_setUB( QProblemB* _THIS, const real_t* const ub_new ) +{ + unsigned int i; + unsigned int nV = (unsigned int)QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ub_new != 0 ) + { + memcpy( _THIS->ub,ub_new,nV*sizeof(real_t) ); + } + else + { + /* if no upper bounds are specified, set them to infinity */ + for( i=0; iub[i] = QPOASES_INFTY; + } + + return SUCCESSFUL_RETURN; +} + + +/* + * s e t U B + */ +static inline returnValue QProblemB_setUBn( QProblemB* _THIS, int number, real_t value ) +{ + int nV = QProblemB_getNV( _THIS ); + + if ( nV == 0 ) + return THROWERROR( RET_QPOBJECT_NOT_SETUP ); + + if ( ( number >= 0 ) && ( number < nV ) ) + { + _THIS->ub[number] = value; + + return SUCCESSFUL_RETURN; + } + else + { + return THROWERROR( RET_INDEX_OUT_OF_BOUNDS ); + } +} + + +/* + * c o m p u t e G i v e n s + */ +static inline void QProblemB_computeGivens( real_t xold, real_t yold, + real_t* xnew, real_t* ynew, real_t* c, real_t* s + ) +{ + real_t t, mu; + + if ( fabs( yold ) <= QPOASES_ZERO ) + { + *c = 1.0; + *s = 0.0; + + *xnew = xold; + *ynew = yold; + } + else + { + mu = fabs( xold ); + if ( fabs( yold ) > mu ) + mu = fabs( yold ); + + t = mu * sqrt( (xold/mu)*(xold/mu) + (yold/mu)*(yold/mu) ); + + if ( xold < 0.0 ) + t = -t; + + *c = xold/t; + *s = yold/t; + *xnew = t; + *ynew = 0.0; + } + + return; +} + + +/* + * a p p l y G i v e n s + */ +static inline void QProblemB_applyGivens( real_t c, real_t s, real_t nu, real_t xold, real_t yold, + real_t* xnew, real_t* ynew + ) +{ + #ifdef __USE_THREE_MULTS_GIVENS__ + + /* Givens plane rotation requiring only three multiplications, + * cf. Hammarling, S.: A note on modifications to the givens plane rotation. + * J. Inst. Maths Applics, 13:215-218, 1974. */ + *xnew = xold*c + yold*s; + *ynew = (*xnew+xold)*nu - yold; + + #else + + /* Usual Givens plane rotation requiring four multiplications. */ + *xnew = c*xold + s*yold; + *ynew = -s*xold + c*yold; + + #endif + + return; +} + + +/* + * i s B l o c k i n g + */ +static inline BooleanType QProblemB_isBlocking( QProblemB* _THIS, + real_t num, + real_t den, + real_t epsNum, + real_t epsDen, + real_t* t + ) +{ + if ( ( den >= epsDen ) && ( num >= epsNum ) ) + { + if ( num < (*t)*den ) + return BT_TRUE; + } + + return BT_FALSE; +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_QPROBLEMB_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Types.h b/third_party/acados/include/qpOASES_e/Types.h new file mode 100644 index 0000000..fc042ae --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Types.h @@ -0,0 +1,310 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Types.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of all non-built-in types (except for classes). + */ + + +#ifndef QPOASES_TYPES_H +#define QPOASES_TYPES_H + +#ifdef USE_ACADOS_TYPES +#include "acados/utils/types.h" +#endif + +/* If your compiler does not support the snprintf() function, + * uncomment the following line and try to compile again. */ +/* #define __NO_SNPRINTF__ */ + + +/* Uncomment the following line for setting the __DSPACE__ flag. */ +/* #define __DSPACE__ */ + +/* Uncomment the following line for setting the __XPCTARGET__ flag. */ +/* #define __XPCTARGET__ */ + + +/* Uncomment the following line for setting the __NO_FMATH__ flag. */ +/* #define __NO_FMATH__ */ + +/* Uncomment the following line to enable debug information. */ +/* #define __DEBUG__ */ + +/* Uncomment the following line to enable suppress any kind of console output. */ +/* #define __SUPPRESSANYOUTPUT__ */ + + +/** Forces to always include all implicitly fixed bounds and all equality constraints + * into the initial working set when setting up an auxiliary QP. */ +#define __ALWAYS_INITIALISE_WITH_ALL_EQUALITIES__ + +/* Uncomment the following line to activate the use of an alternative Givens + * plane rotation requiring only three multiplications. */ +/* #define __USE_THREE_MULTS_GIVENS__ */ + +/* Uncomment the following line to activate the use of single precision arithmetic. */ +/* #define __USE_SINGLE_PRECISION__ */ + +/* The inline keyword is skipped by default as it is not part of the C90 standard. + * However, by uncommenting the following line, use of the inline keyword can be enforced. */ +/* #define __USE_INLINE__ */ + + +/* Work-around for Borland BCC 5.5 compiler. */ +#ifdef __BORLANDC__ +#if __BORLANDC__ < 0x0561 + #define __STDC__ 1 +#endif +#endif + + +/* Work-around for Microsoft compilers. */ +#ifdef _MSC_VER + #define __NO_SNPRINTF__ + #pragma warning( disable : 4061 4100 4250 4514 4996 ) +#endif + + +/* Apply pre-processor settings when using qpOASES within auto-generated code. */ +#ifdef __CODE_GENERATION__ + #define __NO_COPYRIGHT__ + #define __EXTERNAL_DIMENSIONS__ +#endif /* __CODE_GENERATION__ */ + + +/* Avoid using static variables declaration within functions. */ +#ifdef __NO_STATIC__ + #define myStatic +#else + #define myStatic static +#endif /* __NO_STATIC__ */ + + +/* Skip inline keyword if not specified otherwise. */ +#ifndef __USE_INLINE__ + #define inline +#endif + + +/* Avoid any printing on embedded platforms. */ +#if defined(__DSPACE__) || defined(__XPCTARGET__) + #define __SUPPRESSANYOUTPUT__ + #define __NO_SNPRINTF__ +#endif + + +#ifdef __NO_SNPRINTF__ + #if (!defined(_MSC_VER)) || defined(__DSPACE__) || defined(__XPCTARGET__) + /* If snprintf is not available, provide an empty implementation... */ + int snprintf( char* s, size_t n, const char* format, ... ); + #else + /* ... or substitute snprintf by _snprintf for Microsoft compilers. */ + #define snprintf _snprintf + #endif +#endif /* __NO_SNPRINTF__ */ + + +/** Macro for switching on/off the beginning of the qpOASES namespace definition. */ +#define BEGIN_NAMESPACE_QPOASES + +/** Macro for switching on/off the end of the qpOASES namespace definition. */ +#define END_NAMESPACE_QPOASES + +/** Macro for switching on/off the use of the qpOASES namespace. */ +#define USING_NAMESPACE_QPOASES + +/** Macro for switching on/off references to the qpOASES namespace. */ +#define REFER_NAMESPACE_QPOASES /*::*/ + + +/** Macro for accessing the Cholesky factor R. */ +#define RR( I,J ) _THIS->R[(I)+nV*(J)] + +/** Macro for accessing the orthonormal matrix Q of the QT factorisation. */ +#define QQ( I,J ) _THIS->Q[(I)+nV*(J)] + +/** Macro for accessing the triangular matrix T of the QT factorisation. */ +#define TT( I,J ) _THIS->T[(I)*nVC_min+(J)] + + + +BEGIN_NAMESPACE_QPOASES + + +/** Defines real_t for facilitating switching between double and float. */ + +#ifndef USE_ACADOS_TYPES +#ifndef __CODE_GENERATION__ + + #ifdef __USE_SINGLE_PRECISION__ + typedef float real_t; + #else + typedef double real_t; + #endif /* __USE_SINGLE_PRECISION__ */ + +#endif /* __CODE_GENERATION__ */ +#endif /* USE_ACADOS_TYPES */ + +/** Summarises all possible logical values. */ +typedef enum +{ + BT_FALSE = 0, /**< Logical value for "false". */ + BT_TRUE /**< Logical value for "true". */ +} BooleanType; + + +/** Summarises all possible print levels. Print levels are used to describe + * the desired amount of output during runtime of qpOASES. */ +typedef enum +{ + PL_DEBUG_ITER = -2, /**< Full tabular debugging output. */ + PL_TABULAR, /**< Tabular output. */ + PL_NONE, /**< No output. */ + PL_LOW, /**< Print error messages only. */ + PL_MEDIUM, /**< Print error and warning messages as well as concise info messages. */ + PL_HIGH /**< Print all messages with full details. */ +} PrintLevel; + + +/** Defines visibility status of a message. */ +typedef enum +{ + VS_HIDDEN, /**< Message not visible. */ + VS_VISIBLE /**< Message visible. */ +} VisibilityStatus; + + +/** Summarises all possible states of the (S)QProblem(B) object during the +solution process of a QP sequence. */ +typedef enum +{ + QPS_NOTINITIALISED, /**< QProblem object is freshly instantiated or reset. */ + QPS_PREPARINGAUXILIARYQP, /**< An auxiliary problem is currently setup, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_AUXILIARYQPSOLVED, /**< An auxilary problem was solved, either at the very beginning + * via an initial homotopy or after changing the QP matrices. */ + QPS_PERFORMINGHOMOTOPY, /**< A homotopy according to the main idea of the online active + * set strategy is performed. */ + QPS_HOMOTOPYQPSOLVED, /**< An intermediate QP along the homotopy path was solved. */ + QPS_SOLVED /**< The solution of the actual QP was found. */ +} QProblemStatus; + + +/** Summarises all possible types of the QP's Hessian matrix. */ +typedef enum +{ + HST_ZERO, /**< Hessian is zero matrix (i.e. LP formulation). */ + HST_IDENTITY, /**< Hessian is identity matrix. */ + HST_POSDEF, /**< Hessian is (strictly) positive definite. */ + HST_POSDEF_NULLSPACE, /**< Hessian is positive definite on null space of active bounds/constraints. */ + HST_SEMIDEF, /**< Hessian is positive semi-definite. */ + HST_INDEF, /**< Hessian is indefinite. */ + HST_UNKNOWN /**< Hessian type is unknown. */ +} HessianType; + + +/** Summarises all possible types of bounds and constraints. */ +typedef enum +{ + ST_UNBOUNDED, /**< Bound/constraint is unbounded. */ + ST_BOUNDED, /**< Bound/constraint is bounded but not fixed. */ + ST_EQUALITY, /**< Bound/constraint is fixed (implicit equality bound/constraint). */ + ST_DISABLED, /**< Bound/constraint is disabled (i.e. ignored when solving QP). */ + ST_UNKNOWN /**< Type of bound/constraint unknown. */ +} SubjectToType; + + +/** Summarises all possible states of bounds and constraints. */ +typedef enum +{ + ST_LOWER = -1, /**< Bound/constraint is at its lower bound. */ + ST_INACTIVE, /**< Bound/constraint is inactive. */ + ST_UPPER, /**< Bound/constraint is at its upper bound. */ + ST_INFEASIBLE_LOWER, /**< (to be documented) */ + ST_INFEASIBLE_UPPER, /**< (to be documented) */ + ST_UNDEFINED /**< Status of bound/constraint undefined. */ +} SubjectToStatus; + + +/** + * \brief Stores internal information for tabular (debugging) output. + * + * Struct storing internal information for tabular (debugging) output + * when using the (S)QProblem(B) objects. + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + int idxAddB; /**< Index of bound that has been added to working set. */ + int idxRemB; /**< Index of bound that has been removed from working set. */ + int idxAddC; /**< Index of constraint that has been added to working set. */ + int idxRemC; /**< Index of constraint that has been removed from working set. */ + int excAddB; /**< Flag indicating whether a bound has been added to working set to keep a regular projected Hessian. */ + int excRemB; /**< Flag indicating whether a bound has been removed from working set to keep a regular projected Hessian. */ + int excAddC; /**< Flag indicating whether a constraint has been added to working set to keep a regular projected Hessian. */ + int excRemC; /**< Flag indicating whether a constraint has been removed from working set to keep a regular projected Hessian. */ +} TabularOutput; + +/** + * \brief Struct containing the variable header for mat file. + * + * Struct storing the header of a variable to be stored in + * Matlab's binary format (using the outdated Level 4 variant + * for simplictiy). + * + * Note, this code snippet has been inspired from the document + * "Matlab(R) MAT-file Format, R2013b" by MathWorks + * + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2013-2015 + */ +typedef struct +{ + long numericFormat; /**< Flag indicating numerical format. */ + long nRows; /**< Number of rows. */ + long nCols; /**< Number of rows. */ + long imaginaryPart; /**< (to be documented) */ + long nCharName; /**< Number of character in name. */ +} MatMatrixHeader; + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_TYPES_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/UnitTesting.h b/third_party/acados/include/qpOASES_e/UnitTesting.h new file mode 100644 index 0000000..dbff201 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/UnitTesting.h @@ -0,0 +1,79 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/UnitTesting.h + * \author Hans Joachim Ferreau + * \version 3.1embedded + * \date 2014-2015 + * + * Definition of auxiliary functions/macros for unit testing. + */ + + +#ifndef QPOASES_UNIT_TESTING_H +#define QPOASES_UNIT_TESTING_H + + +#ifndef TEST_TOL_FACTOR +#define TEST_TOL_FACTOR 1 +#endif + + +/** Return value for tests that passed. */ +#define TEST_PASSED 0 + +/** Return value for tests that failed. */ +#define TEST_FAILED 1 + +/** Return value for tests that could not run due to missing external data. */ +#define TEST_DATA_NOT_FOUND 99 + + +/** Macro verifying that two numerical values are equal in order to pass unit test. */ +#define QPOASES_TEST_FOR_EQUAL( x,y ) if ( REFER_NAMESPACE_QPOASES isEqual( (x),(y) ) == BT_FALSE ) { return TEST_FAILED; } + +/** Macro verifying that two numerical values are close to each other in order to pass unit test. */ +#define QPOASES_TEST_FOR_NEAR( x,y ) if ( REFER_NAMESPACE_QPOASES getAbs((x)-(y)) / REFER_NAMESPACE_QPOASES getMax( 1.0,REFER_NAMESPACE_QPOASES getAbs(x) ) >= 1e-10 ) { return TEST_FAILED; } + +/** Macro verifying that first quantity is lower or equal than second one in order to pass unit test. */ +#define QPOASES_TEST_FOR_TOL( x,tol ) if ( (x) > (tol)*(TEST_TOL_FACTOR) ) { return TEST_FAILED; } + +/** Macro verifying that a logical expression holds in order to pass unit test. */ +#define QPOASES_TEST_FOR_TRUE( x ) if ( (x) == 0 ) { return TEST_FAILED; } + + + +BEGIN_NAMESPACE_QPOASES + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_UNIT_TESTING_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/Utils.h b/third_party/acados/include/qpOASES_e/Utils.h new file mode 100644 index 0000000..75e45a5 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/Utils.h @@ -0,0 +1,500 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/Utils.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of some utilities for working with the different QProblem classes. + */ + + +#ifndef QPOASES_UTILS_H +#define QPOASES_UTILS_H + +#include + + +BEGIN_NAMESPACE_QPOASES + + +/** Prints a vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printV( const real_t* const v, /**< Vector to be printed. */ + int n /**< Length of vector. */ + ); + +/** Prints a permuted vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printPV( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const int* const V_idx /**< Pemutation vector. */ + ); + +/** Prints a named vector. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNV( const real_t* const v, /**< Vector to be printed. */ + int n, /**< Length of vector. */ + const char* name /** Name of vector. */ + ); + +/** Prints a matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol /**< Column number of matrix. */ + ); + +/** Prints a permuted matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printPM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol , /**< Column number of matrix. */ + const int* const ROW_idx, /**< Row pemutation vector. */ + const int* const COL_idx /**< Column pemutation vector. */ + ); + +/** Prints a named matrix. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNM( const real_t* const M, /**< Matrix to be printed. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* name /** Name of matrix. */ + ); + +/** Prints an index array. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printI( const int* const _index, /**< Index array to be printed. */ + int n /**< Length of index array. */ + ); + +/** Prints a named index array. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printNI( const int* const _index, /**< Index array to be printed. */ + int n, /**< Length of index array. */ + const char* name /**< Name of index array. */ + ); + + +/** Prints a string to desired output target (useful also for MATLAB output!). + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_myPrintf( const char* s /**< String to be written. */ + ); + + +/** Prints qpOASES copyright notice. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_printCopyrightNotice( ); + + +/** Reads a real_t matrix from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileM( real_t* data, /**< Matrix to be read from file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads a real_t vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileV( real_t* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + +/** Reads an integer (column) vector from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE \n + RET_UNABLE_TO_READ_FILE */ +returnValue qpOASES_readFromFileI( int* data, /**< Vector to be read from file. */ + int n, /**< Length of vector. */ + const char* datafilename /**< Data file name. */ + ); + + +/** Writes a real_t matrix into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileM( const real_t* const data, /**< Matrix to be written into file. */ + int nrow, /**< Row number of matrix. */ + int ncol, /**< Column number of matrix. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes a real_t vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileV( const real_t* const data, /**< Vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if data shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes an integer (column) vector into a file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_OPEN_FILE */ +returnValue qpOASES_writeIntoFileI( const int* const integer, /**< Integer vector to be written into file. */ + int n, /**< Length of vector. */ + const char* datafilename, /**< Data file name. */ + BooleanType append /**< Indicates if integer shall be appended if the file already exists (otherwise it is overwritten). */ + ); + +/** Writes a real_t matrix/vector into a Matlab binary file. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS + RET_UNABLE_TO_WRITE_FILE */ +returnValue qpOASES_writeIntoMatFile( FILE* const matFile, /**< Pointer to Matlab binary file. */ + const real_t* const data, /**< Data to be written into file. */ + int nRows, /**< Row number of matrix. */ + int nCols, /**< Column number of matrix. */ + const char* name /**< Matlab name of matrix/vector to be stored. */ + ); + +/** Writes in integer matrix/vector into a Matlab binary file. + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS + RET_UNABLE_TO_WRITE_FILE */ +returnValue qpOASES_writeIntoMatFileI( FILE* const matFile, /**< Pointer to Matlab binary file. */ + const int* const data, /**< Data to be written into file. */ + int nRows, /**< Row number of matrix. */ + int nCols, /**< Column number of matrix. */ + const char* name /**< Matlab name of matrix/vector to be stored. */ + ); + + +/** Returns the current system time. + * \return current system time */ +real_t qpOASES_getCPUtime( ); + + +/** Returns the N-norm of a vector. + * \return >= 0.0: successful */ +real_t qpOASES_getNorm( const real_t* const v, /**< Vector. */ + int n, /**< Vector's dimension. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + +/** Tests whether two real-valued arguments are (numerically) equal. + * \return BT_TRUE: arguments differ not more than TOL \n + BT_FALSE: arguments differ more than TOL */ +static inline BooleanType qpOASES_isEqual( real_t x, /**< First real number. */ + real_t y, /**< Second real number. */ + real_t TOL /**< Tolerance for comparison. */ + ); + + +/** Tests whether a real-valued argument is (numerically) zero. + * \return BT_TRUE: argument differs from 0.0 not more than TOL \n + BT_FALSE: argument differs from 0.0 more than TOL */ +static inline BooleanType qpOASES_isZero( real_t x, /**< Real number. */ + real_t TOL /**< Tolerance for comparison. */ + ); + + +/** Returns sign of a real-valued argument. + * \return 1.0: argument is non-negative \n + -1.0: argument is negative */ +static inline real_t qpOASES_getSign( real_t arg /**< real-valued argument whose sign is to be determined. */ + ); + + +/** Returns maximum of two integers. + * \return Maximum of two integers */ +static inline int qpOASES_getMaxI( int x, /**< First integer. */ + int y /**< Second integer. */ + ); + + +/** Returns minimum of two integers. + * \return Minimum of two integers */ +static inline int qpOASES_getMinI( int x, /**< First integer. */ + int y /**< Second integer. */ + ); + + +/** Returns maximum of two reals. + * \return Maximum of two reals */ +static inline real_t qpOASES_getMax( real_t x, /**< First real number. */ + real_t y /**< Second real number. */ + ); + + +/** Returns minimum of two reals. + * \return Minimum of two reals */ +static inline real_t qpOASES_getMin( real_t x, /**< First real number. */ + real_t y /**< Second real number. */ + ); + + +/** Returns the absolute value of a real_t-valued argument. + * \return Absolute value of a real_t-valued argument */ +static inline real_t qpOASES_getAbs( real_t x /**< real_t-valued argument. */ + ); + +/** Returns the square-root of a real number. + * \return Square-root of a real number */ +static inline real_t qpOASES_getSqrt( real_t x /**< Non-negative real number. */ + ); + + +/** Computes the maximum violation of the KKT optimality conditions + * of given iterate for given QP data. */ +returnValue qpOASES_getKktViolation( int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + const real_t* const H, /**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */ + const real_t* const g, /**< Gradient vector. */ + const real_t* const A, /**< Constraint matrix. */ + const real_t* const lb, /**< Lower bound vector (on variables). */ + const real_t* const ub, /**< Upper bound vector (on variables). */ + const real_t* const lbA, /**< Lower constraints' bound vector. */ + const real_t* const ubA, /**< Upper constraints' bound vector. */ + const real_t* const x, /**< Primal trial vector. */ + const real_t* const y, /**< Dual trial vector. */ + real_t* const _stat, /**< Output: maximum value of stationarity condition residual. */ + real_t* const feas, /**< Output: maximum value of primal feasibility violation. */ + real_t* const cmpl /**< Output: maximum value of complementarity residual. */ + ); + +/** Computes the maximum violation of the KKT optimality conditions + * of given iterate for given QP data. */ +returnValue qpOASES_getKktViolationSB( int nV, /**< Number of variables. */ + const real_t* const H, /**< Hessian matrix (may be NULL if Hessian is zero or identity matrix). */ + const real_t* const g, /**< Gradient vector. */ + const real_t* const lb, /**< Lower bound vector (on variables). */ + const real_t* const ub, /**< Upper bound vector (on variables). */ + const real_t* const x, /**< Primal trial vector. */ + const real_t* const y, /**< Dual trial vector. */ + real_t* const _stat, /**< Output: maximum value of stationarity condition residual. */ + real_t* const feas, /**< Output: maximum value of primal feasibility violation. */ + real_t* const cmpl /**< Output: maximum value of complementarity residual. */ + ); + + +/** Writes a value of BooleanType into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertBooleanTypeToString( BooleanType value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + +/** Writes a value of SubjectToStatus into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertSubjectToStatusToString( SubjectToStatus value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + +/** Writes a value of PrintLevel into a string. + * \return SUCCESSFUL_RETURN */ +returnValue qpOASES_convertPrintLevelToString( PrintLevel value, /**< Value to be written. */ + char* const string /**< Input: String of sufficient size, \n + Output: String containing value. */ + ); + + +/** Converts a returnValue from an QProblem(B) object into a more + * simple status flag. + * + * \return 0: QP problem solved + * 1: QP could not be solved within given number of iterations + * -1: QP could not be solved due to an internal error + * -2: QP is infeasible (and thus could not be solved) + * -3: QP is unbounded (and thus could not be solved) + */ +int qpOASES_getSimpleStatus( returnValue returnvalue, /**< ReturnValue to be analysed. */ + BooleanType doPrintStatus /**< Flag indicating whether simple status shall be printed to screen. */ + ); + +/** Normalises QP constraints. + * \return SUCCESSFUL_RETURN \n + * RET_INVALID_ARGUMENTS */ +returnValue qpOASES_normaliseConstraints( int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + real_t* A, /**< Input: Constraint matrix, \n + Output: Normalised constraint matrix. */ + real_t* lbA, /**< Input: Constraints' lower bound vector, \n + Output: Normalised constraints' lower bound vector. */ + real_t* ubA, /**< Input: Constraints' upper bound vector, \n + Output: Normalised constraints' upper bound vector. */ + int type /**< Norm type, 1: one-norm, 2: Euclidean norm. */ + ); + + +#ifdef __DEBUG__ +/** Writes matrix with given dimension into specified file. */ +void gdb_printmat( const char *fname, /**< File name. */ + real_t *M, /**< Matrix to be written. */ + int n, /**< Number of rows. */ + int m, /**< Number of columns. */ + int ldim /**< Leading dimension. */ + ); +#endif /* __DEBUG__ */ + + +#if defined(__DSPACE__) || defined(__XPCTARGET__) +void __cxa_pure_virtual( void ); +#endif /* __DSPACE__ || __XPCTARGET__*/ + + + +/* + * i s E q u a l + */ +static inline BooleanType qpOASES_isEqual( real_t x, + real_t y, + real_t TOL + ) +{ + if ( qpOASES_getAbs(x-y) <= TOL ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * i s Z e r o + */ +static inline BooleanType qpOASES_isZero( real_t x, + real_t TOL + ) +{ + if ( qpOASES_getAbs(x) <= TOL ) + return BT_TRUE; + else + return BT_FALSE; +} + + +/* + * g e t S i g n + */ +static inline real_t qpOASES_getSign( real_t arg + ) +{ + if ( arg >= 0.0 ) + return 1.0; + else + return -1.0; +} + + + +/* + * g e t M a x + */ +static inline int qpOASES_getMaxI( int x, + int y + ) +{ + return (yx) ? x : y; +} + + +/* + * g e t M a x + */ +static inline real_t qpOASES_getMax( real_t x, + real_t y + ) +{ + #ifdef __NO_FMATH__ + return (yx) ? x : y; + #else + return (y>x) ? x : y; + /*return fmin(x,y); seems to be slower */ + #endif +} + + +/* + * g e t A b s + */ +static inline real_t qpOASES_getAbs( real_t x + ) +{ + #ifdef __NO_FMATH__ + return (x>=0.0) ? x : -x; + #else + return fabs(x); + #endif +} + +/* + * g e t S q r t + */ +static inline real_t qpOASES_getSqrt( real_t x + ) +{ + #ifdef __NO_FMATH__ + return sqrt(x); /* put your custom sqrt-replacement here */ + #else + return sqrt(x); + #endif +} + + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_UTILS_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/include/qpOASES_e/extras/OQPinterface.h b/third_party/acados/include/qpOASES_e/extras/OQPinterface.h new file mode 100644 index 0000000..da59ea9 --- /dev/null +++ b/third_party/acados/include/qpOASES_e/extras/OQPinterface.h @@ -0,0 +1,227 @@ +/* + * This file is part of qpOASES. + * + * qpOASES -- An Implementation of the Online Active Set Strategy. + * Copyright (C) 2007-2015 by Hans Joachim Ferreau, Andreas Potschka, + * Christian Kirches et al. All rights reserved. + * + * qpOASES 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 2.1 of the License, or (at your option) any later version. + * + * qpOASES 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 qpOASES; if not, write to the Free Software + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA + * + */ + + +/** + * \file include/qpOASES_e/extras/OQPinterface.h + * \author Hans Joachim Ferreau, Andreas Potschka, Christian Kirches + * \version 3.1embedded + * \date 2007-2015 + * + * Declaration of an interface comprising several utility functions + * for solving test problems from the Online QP Benchmark Collection + * (This collection is no longer maintained, see + * http://www.qpOASES.org/onlineQP for a backup). + */ + + +#ifndef QPOASES_OQPINTERFACE_H +#define QPOASES_OQPINTERFACE_H + + +#include +#include + +#include +#include + + +BEGIN_NAMESPACE_QPOASES + +typedef struct { + QProblem *qp; + + DenseMatrix *H; + DenseMatrix *A; + + real_t *x; + real_t *y; +} OQPbenchmark_ws; + +int OQPbenchmark_ws_calculateMemorySize( unsigned int nV, unsigned int nC ); + +char *OQPbenchmark_ws_assignMemory( unsigned int nV, unsigned int nC, OQPbenchmark_ws **mem, void *raw_memory ); + +OQPbenchmark_ws *OQPbenchmark_ws_createMemory( unsigned int nV, unsigned int nC ); + +typedef struct { + QProblemB *qp; + + DenseMatrix *H; + + real_t *x; + real_t *y; +} OQPbenchmarkB_ws; + +int OQPbenchmarkB_ws_calculateMemorySize( unsigned int nV ); + +char *OQPbenchmarkB_ws_assignMemory( unsigned int nV, OQPbenchmarkB_ws **mem, void *raw_memory ); + +OQPbenchmarkB_ws *OQPbenchmarkB_ws_createMemory( unsigned int nV ); + +typedef struct { + OQPbenchmark_ws *qp_ws; + OQPbenchmarkB_ws *qpB_ws; + + real_t *H; + real_t *g; + real_t *A; + real_t *lb; + real_t *ub; + real_t *lbA; + real_t *ubA; +} OQPinterface_ws; + +int OQPinterface_ws_calculateMemorySize( unsigned int nV, unsigned int nC, unsigned int nQP ); + +char *OQPinterface_ws_assignMemory( unsigned int nV, unsigned int nC, unsigned int nQP, OQPinterface_ws **mem, void *raw_memory ); + +OQPinterface_ws *OQPinterface_ws_createMemory( unsigned int nV, unsigned int nC, unsigned int nQP ); + +/** Reads dimensions of an Online QP Benchmark problem from file. + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_READ_FILE \n + RET_FILEDATA_INCONSISTENT */ +returnValue readOQPdimensions( const char* path, /**< Full path of the data files (without trailing slash!). */ + int* nQP, /**< Output: Number of QPs. */ + int* nV, /**< Output: Number of variables. */ + int* nC, /**< Output: Number of constraints. */ + int* nEC /**< Output: Number of equality constraints. */ + ); + +/** Reads data of an Online QP Benchmark problem from file. + * This function allocates the required memory for all data; after successfully calling it, + * you have to free this memory yourself! + * \return SUCCESSFUL_RETURN \n + RET_INVALID_ARGUMENTS \n + RET_UNABLE_TO_READ_FILE \n + RET_FILEDATA_INCONSISTENT */ +returnValue readOQPdata( const char* path, /**< Full path of the data files (without trailing slash!). */ + int* nQP, /**< Output: Number of QPs. */ + int* nV, /**< Output: Number of variables. */ + int* nC, /**< Output: Number of constraints. */ + int* nEC, /**< Output: Number of equality constraints. */ + real_t* H, /**< Output: Hessian matrix. */ + real_t* g, /**< Output: Sequence of gradient vectors. */ + real_t* A, /**< Output: Constraint matrix. */ + real_t* lb, /**< Output: Sequence of lower bound vectors (on variables). */ + real_t* ub, /**< Output: Sequence of upper bound vectors (on variables). */ + real_t* lbA, /**< Output: Sequence of lower constraints' bound vectors. */ + real_t* ubA, /**< Output: Sequence of upper constraints' bound vectors. */ + real_t* xOpt, /**< Output: Sequence of primal solution vectors + * (not read if a null pointer is passed). */ + real_t* yOpt, /**< Output: Sequence of dual solution vectors + * (not read if a null pointer is passed). */ + real_t* objOpt /**< Output: Sequence of optimal objective function values + * (not read if a null pointer is passed). */ + ); + + +/** Solves an Online QP Benchmark problem as specified by the arguments. + * The maximum deviations from the given optimal solution as well as the + * maximum CPU time to solve each QP are determined. + * \return SUCCESSFUL_RETURN \n + RET_BENCHMARK_ABORTED */ +returnValue solveOQPbenchmark( int nQP, /**< Number of QPs. */ + int nV, /**< Number of variables. */ + int nC, /**< Number of constraints. */ + int nEC, /**< Number of equality constraints. */ + real_t* _H, /**< Hessian matrix. */ + const real_t* const g, /**< Sequence of gradient vectors. */ + real_t* _A, /**< Constraint matrix. */ + const real_t* const lb, /**< Sequence of lower bound vectors (on variables). */ + const real_t* const ub, /**< Sequence of upper bound vectors (on variables). */ + const real_t* const lbA, /**< Sequence of lower constraints' bound vectors. */ + const real_t* const ubA, /**< Sequence of upper constraints' bound vectors. */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPbenchmark_ws *work /**< Workspace. */ + ); + +/** Solves an Online QP Benchmark problem (without constraints) as specified + * by the arguments. The maximum deviations from the given optimal solution + * as well as the maximum CPU time to solve each QP are determined. + * \return SUCCESSFUL_RETURN \n + RET_BENCHMARK_ABORTED */ +returnValue solveOQPbenchmarkB( int nQP, /**< Number of QPs. */ + int nV, /**< Number of variables. */ + real_t* _H, /**< Hessian matrix. */ + const real_t* const g, /**< Sequence of gradient vectors. */ + const real_t* const lb, /**< Sequence of lower bound vectors (on variables). */ + const real_t* const ub, /**< Sequence of upper bound vectors (on variables). */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPbenchmarkB_ws *work /**< Workspace. */ + ); + + +/** Runs an Online QP Benchmark problem and determines the maximum + * violation of the KKT optimality conditions as well as the + * maximum and average number of iterations and CPU time to solve + * each QP. + * + * \return SUCCESSFUL_RETURN \n + RET_UNABLE_TO_READ_BENCHMARK \n + RET_BENCHMARK_ABORTED */ +returnValue runOQPbenchmark( const char* path, /**< Full path of the benchmark files (without trailing slash!). */ + BooleanType isSparse, /**< Shall convert matrices to sparse format before solution? */ + BooleanType useHotstarts, /**< Shall QP solution be hotstarted? */ + const Options* options, /**< QP solver options to be used while solving benchmark problems. */ + int maxAllowedNWSR, /**< Maximum number of working set recalculations to be performed. */ + real_t* maxNWSR, /**< Output: Maximum number of performed working set recalculations. */ + real_t* avgNWSR, /**< Output: Average number of performed working set recalculations. */ + real_t* maxCPUtime, /**< Output: Maximum CPU time required for solving each QP. */ + real_t* avgCPUtime, /**< Output: Average CPU time required for solving each QP. */ + real_t* maxStationarity, /**< Output: Maximum residual of stationarity condition. */ + real_t* maxFeasibility, /**< Output: Maximum residual of primal feasibility condition. */ + real_t* maxComplementarity, /**< Output: Maximum residual of complementarity condition. */ + OQPinterface_ws* work /**< Workspace. */ + ); + +END_NAMESPACE_QPOASES + + +#endif /* QPOASES_OQPINTERFACE_H */ + + +/* + * end of file + */ diff --git a/third_party/acados/larch64/lib/libqpOASES_e.so b/third_party/acados/larch64/lib/libqpOASES_e.so index f96ec1a..1404862 120000 --- a/third_party/acados/larch64/lib/libqpOASES_e.so +++ b/third_party/acados/larch64/lib/libqpOASES_e.so @@ -1 +1 @@ -ELF \ No newline at end of file +libqpOASES_e.so.3.1 \ No newline at end of file diff --git a/third_party/acados/x86_64/t_renderer b/third_party/acados/x86_64/t_renderer old mode 100644 new mode 100755 diff --git a/third_party/bootstrap/.gitignore b/third_party/bootstrap/.gitignore new file mode 100644 index 0000000..ac06c0c --- /dev/null +++ b/third_party/bootstrap/.gitignore @@ -0,0 +1 @@ +/icons/ diff --git a/third_party/catch2/include/catch2/catch.hpp b/third_party/catch2/include/catch2/catch.hpp new file mode 100644 index 0000000..d2a1242 --- /dev/null +++ b/third_party/catch2/include/catch2/catch.hpp @@ -0,0 +1,17970 @@ +/* + * Catch v2.13.9 + * Generated: 2022-04-12 22:37:23.260201 + * ---------------------------------------------------------- + * This file has been merged from multiple headers. Please don't edit it directly + * Copyright (c) 2022 Two Blue Cubes Ltd. All rights reserved. + * + * Distributed under the Boost Software License, Version 1.0. (See accompanying + * file LICENSE_1_0.txt or copy at http://www.boost.org/LICENSE_1_0.txt) + */ +#ifndef TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +#define TWOBLUECUBES_SINGLE_INCLUDE_CATCH_HPP_INCLUDED +// start catch.hpp + + +#define CATCH_VERSION_MAJOR 2 +#define CATCH_VERSION_MINOR 13 +#define CATCH_VERSION_PATCH 9 + +#ifdef __clang__ +# pragma clang system_header +#elif defined __GNUC__ +# pragma GCC system_header +#endif + +// start catch_suppress_warnings.h + +#ifdef __clang__ +# ifdef __ICC // icpc defines the __clang__ macro +# pragma warning(push) +# pragma warning(disable: 161 1682) +# else // __ICC +# pragma clang diagnostic push +# pragma clang diagnostic ignored "-Wpadded" +# pragma clang diagnostic ignored "-Wswitch-enum" +# pragma clang diagnostic ignored "-Wcovered-switch-default" +# endif +#elif defined __GNUC__ + // Because REQUIREs trigger GCC's -Wparentheses, and because still + // supported version of g++ have only buggy support for _Pragmas, + // Wparentheses have to be suppressed globally. +# pragma GCC diagnostic ignored "-Wparentheses" // See #674 for details + +# pragma GCC diagnostic push +# pragma GCC diagnostic ignored "-Wunused-variable" +# pragma GCC diagnostic ignored "-Wpadded" +#endif +// end catch_suppress_warnings.h +#if defined(CATCH_CONFIG_MAIN) || defined(CATCH_CONFIG_RUNNER) +# define CATCH_IMPL +# define CATCH_CONFIG_ALL_PARTS +#endif + +// In the impl file, we want to have access to all parts of the headers +// Can also be used to sanely support PCHs +#if defined(CATCH_CONFIG_ALL_PARTS) +# define CATCH_CONFIG_EXTERNAL_INTERFACES +# if defined(CATCH_CONFIG_DISABLE_MATCHERS) +# undef CATCH_CONFIG_DISABLE_MATCHERS +# endif +# if !defined(CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER) +# define CATCH_CONFIG_ENABLE_CHRONO_STRINGMAKER +# endif +#endif + +#if !defined(CATCH_CONFIG_IMPL_ONLY) +// start catch_platform.h + +// See e.g.: +// https://opensource.apple.com/source/CarbonHeaders/CarbonHeaders-18.1/TargetConditionals.h.auto.html +#ifdef __APPLE__ +# include +# if (defined(TARGET_OS_OSX) && TARGET_OS_OSX == 1) || \ + (defined(TARGET_OS_MAC) && TARGET_OS_MAC == 1) +# define CATCH_PLATFORM_MAC +# elif (defined(TARGET_OS_IPHONE) && TARGET_OS_IPHONE == 1) +# define CATCH_PLATFORM_IPHONE +# endif + +#elif defined(linux) || defined(__linux) || defined(__linux__) +# define CATCH_PLATFORM_LINUX + +#elif defined(WIN32) || defined(__WIN32__) || defined(_WIN32) || defined(_MSC_VER) || defined(__MINGW32__) +# define CATCH_PLATFORM_WINDOWS +#endif + +// end catch_platform.h + +#ifdef CATCH_IMPL +# ifndef CLARA_CONFIG_MAIN +# define CLARA_CONFIG_MAIN_NOT_DEFINED +# define CLARA_CONFIG_MAIN +# endif +#endif + +// start catch_user_interfaces.h + +namespace Catch { + unsigned int rngSeed(); +} + +// end catch_user_interfaces.h +// start catch_tag_alias_autoregistrar.h + +// start catch_common.h + +// start catch_compiler_capabilities.h + +// Detect a number of compiler features - by compiler +// The following features are defined: +// +// CATCH_CONFIG_COUNTER : is the __COUNTER__ macro supported? +// CATCH_CONFIG_WINDOWS_SEH : is Windows SEH supported? +// CATCH_CONFIG_POSIX_SIGNALS : are POSIX signals supported? +// CATCH_CONFIG_DISABLE_EXCEPTIONS : Are exceptions enabled? +// **************** +// Note to maintainers: if new toggles are added please document them +// in configuration.md, too +// **************** + +// In general each macro has a _NO_ form +// (e.g. CATCH_CONFIG_NO_POSIX_SIGNALS) which disables the feature. +// Many features, at point of detection, define an _INTERNAL_ macro, so they +// can be combined, en-mass, with the _NO_ forms later. + +#ifdef __cplusplus + +# if (__cplusplus >= 201402L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201402L) +# define CATCH_CPP14_OR_GREATER +# endif + +# if (__cplusplus >= 201703L) || (defined(_MSVC_LANG) && _MSVC_LANG >= 201703L) +# define CATCH_CPP17_OR_GREATER +# endif + +#endif + +// Only GCC compiler should be used in this block, so other compilers trying to +// mask themselves as GCC should be ignored. +#if defined(__GNUC__) && !defined(__clang__) && !defined(__ICC) && !defined(__CUDACC__) && !defined(__LCC__) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "GCC diagnostic pop" ) + +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) + +#endif + +#if defined(__clang__) + +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic push" ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION _Pragma( "clang diagnostic pop" ) + +// As of this writing, IBM XL's implementation of __builtin_constant_p has a bug +// which results in calls to destructors being emitted for each temporary, +// without a matching initialization. In practice, this can result in something +// like `std::string::~string` being called on an uninitialized value. +// +// For example, this code will likely segfault under IBM XL: +// ``` +// REQUIRE(std::string("12") + "34" == "1234") +// ``` +// +// Therefore, `CATCH_INTERNAL_IGNORE_BUT_WARN` is not implemented. +# if !defined(__ibmxl__) && !defined(__CUDACC__) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) (void)__builtin_constant_p(__VA_ARGS__) /* NOLINT(cppcoreguidelines-pro-type-vararg, hicpp-vararg) */ +# endif + +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wexit-time-destructors\"" ) \ + _Pragma( "clang diagnostic ignored \"-Wglobal-constructors\"") + +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wparentheses\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-variable\"" ) + +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wgnu-zero-variadic-macro-arguments\"" ) + +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS \ + _Pragma( "clang diagnostic ignored \"-Wunused-template\"" ) + +#endif // __clang__ + +//////////////////////////////////////////////////////////////////////////////// +// Assume that non-Windows platforms support posix signals by default +#if !defined(CATCH_PLATFORM_WINDOWS) + #define CATCH_INTERNAL_CONFIG_POSIX_SIGNALS +#endif + +//////////////////////////////////////////////////////////////////////////////// +// We know some environments not to support full POSIX signals +#if defined(__CYGWIN__) || defined(__QNX__) || defined(__EMSCRIPTEN__) || defined(__DJGPP__) + #define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +#endif + +#ifdef __OS400__ +# define CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS +# define CATCH_CONFIG_COLOUR_NONE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Android somehow still does not support std::to_string +#if defined(__ANDROID__) +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING +# define CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Not all Windows environments support SEH properly +#if defined(__MINGW32__) +# define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH +#endif + +//////////////////////////////////////////////////////////////////////////////// +// PS4 +#if defined(__ORBIS__) +# define CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE +#endif + +//////////////////////////////////////////////////////////////////////////////// +// Cygwin +#ifdef __CYGWIN__ + +// Required for some versions of Cygwin to declare gettimeofday +// see: http://stackoverflow.com/questions/36901803/gettimeofday-not-declared-in-this-scope-cygwin +# define _BSD_SOURCE +// some versions of cygwin (most) do not support std::to_string. Use the libstd check. +// https://gcc.gnu.org/onlinedocs/gcc-4.8.2/libstdc++/api/a01053_source.html line 2812-2813 +# if !((__cplusplus >= 201103L) && defined(_GLIBCXX_USE_C99) \ + && !defined(_GLIBCXX_HAVE_BROKEN_VSWPRINTF)) + +# define CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING + +# endif +#endif // __CYGWIN__ + +//////////////////////////////////////////////////////////////////////////////// +// Visual C++ +#if defined(_MSC_VER) + +// Universal Windows platform does not support SEH +// Or console colours (or console at all...) +# if defined(WINAPI_FAMILY) && (WINAPI_FAMILY == WINAPI_FAMILY_APP) +# define CATCH_CONFIG_COLOUR_NONE +# else +# define CATCH_INTERNAL_CONFIG_WINDOWS_SEH +# endif + +# if !defined(__clang__) // Handle Clang masquerading for msvc + +// MSVC traditional preprocessor needs some workaround for __VA_ARGS__ +// _MSVC_TRADITIONAL == 0 means new conformant preprocessor +// _MSVC_TRADITIONAL == 1 means old traditional non-conformant preprocessor +# if !defined(_MSVC_TRADITIONAL) || (defined(_MSVC_TRADITIONAL) && _MSVC_TRADITIONAL) +# define CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +# endif // MSVC_TRADITIONAL + +// Only do this if we're not using clang on Windows, which uses `diagnostic push` & `diagnostic pop` +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION __pragma( warning(push) ) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION __pragma( warning(pop) ) +# endif // __clang__ + +#endif // _MSC_VER + +#if defined(_REENTRANT) || defined(_MSC_VER) +// Enable async processing, as -pthread is specified or no additional linking is required +# define CATCH_INTERNAL_CONFIG_USE_ASYNC +#endif // _MSC_VER + +//////////////////////////////////////////////////////////////////////////////// +// Check if we are compiled with -fno-exceptions or equivalent +#if defined(__EXCEPTIONS) || defined(__cpp_exceptions) || defined(_CPPUNWIND) +# define CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED +#endif + +//////////////////////////////////////////////////////////////////////////////// +// DJGPP +#ifdef __DJGPP__ +# define CATCH_INTERNAL_CONFIG_NO_WCHAR +#endif // __DJGPP__ + +//////////////////////////////////////////////////////////////////////////////// +// Embarcadero C++Build +#if defined(__BORLANDC__) + #define CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// Use of __COUNTER__ is suppressed during code analysis in +// CLion/AppCode 2017.2.x and former, because __COUNTER__ is not properly +// handled by it. +// Otherwise all supported compilers support COUNTER macro, +// but user still might want to turn it off +#if ( !defined(__JETBRAINS_IDE__) || __JETBRAINS_IDE__ >= 20170300L ) + #define CATCH_INTERNAL_CONFIG_COUNTER +#endif + +//////////////////////////////////////////////////////////////////////////////// + +// RTX is a special version of Windows that is real time. +// This means that it is detected as Windows, but does not provide +// the same set of capabilities as real Windows does. +#if defined(UNDER_RTSS) || defined(RTX64_BUILD) + #define CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH + #define CATCH_INTERNAL_CONFIG_NO_ASYNC + #define CATCH_CONFIG_COLOUR_NONE +#endif + +#if !defined(_GLIBCXX_USE_C99_MATH_TR1) +#define CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Various stdlib support checks that require __has_include +#if defined(__has_include) + // Check if string_view is available and usable + #if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW + #endif + + // Check if optional is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # define CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if byte is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # include + # if defined(__cpp_lib_byte) && (__cpp_lib_byte > 0) + # define CATCH_INTERNAL_CONFIG_CPP17_BYTE + # endif + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) + + // Check if variant is available and usable + # if __has_include() && defined(CATCH_CPP17_OR_GREATER) + # if defined(__clang__) && (__clang_major__ < 8) + // work around clang bug with libstdc++ https://bugs.llvm.org/show_bug.cgi?id=31852 + // fix should be in clang 8, workaround in libstdc++ 8.2 + # include + # if defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # define CATCH_CONFIG_NO_CPP17_VARIANT + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__GLIBCXX__) && defined(_GLIBCXX_RELEASE) && (_GLIBCXX_RELEASE < 9) + # else + # define CATCH_INTERNAL_CONFIG_CPP17_VARIANT + # endif // defined(__clang__) && (__clang_major__ < 8) + # endif // __has_include() && defined(CATCH_CPP17_OR_GREATER) +#endif // defined(__has_include) + +#if defined(CATCH_INTERNAL_CONFIG_COUNTER) && !defined(CATCH_CONFIG_NO_COUNTER) && !defined(CATCH_CONFIG_COUNTER) +# define CATCH_CONFIG_COUNTER +#endif +#if defined(CATCH_INTERNAL_CONFIG_WINDOWS_SEH) && !defined(CATCH_CONFIG_NO_WINDOWS_SEH) && !defined(CATCH_CONFIG_WINDOWS_SEH) && !defined(CATCH_INTERNAL_CONFIG_NO_WINDOWS_SEH) +# define CATCH_CONFIG_WINDOWS_SEH +#endif +// This is set by default, because we assume that unix compilers are posix-signal-compatible by default. +#if defined(CATCH_INTERNAL_CONFIG_POSIX_SIGNALS) && !defined(CATCH_INTERNAL_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_NO_POSIX_SIGNALS) && !defined(CATCH_CONFIG_POSIX_SIGNALS) +# define CATCH_CONFIG_POSIX_SIGNALS +#endif +// This is set by default, because we assume that compilers with no wchar_t support are just rare exceptions. +#if !defined(CATCH_INTERNAL_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_NO_WCHAR) && !defined(CATCH_CONFIG_WCHAR) +# define CATCH_CONFIG_WCHAR +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_NO_CPP11_TO_STRING) && !defined(CATCH_CONFIG_CPP11_TO_STRING) +# define CATCH_CONFIG_CPP11_TO_STRING +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_NO_CPP17_OPTIONAL) && !defined(CATCH_CONFIG_CPP17_OPTIONAL) +# define CATCH_CONFIG_CPP17_OPTIONAL +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_NO_CPP17_STRING_VIEW) && !defined(CATCH_CONFIG_CPP17_STRING_VIEW) +# define CATCH_CONFIG_CPP17_STRING_VIEW +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_VARIANT) && !defined(CATCH_CONFIG_NO_CPP17_VARIANT) && !defined(CATCH_CONFIG_CPP17_VARIANT) +# define CATCH_CONFIG_CPP17_VARIANT +#endif + +#if defined(CATCH_INTERNAL_CONFIG_CPP17_BYTE) && !defined(CATCH_CONFIG_NO_CPP17_BYTE) && !defined(CATCH_CONFIG_CPP17_BYTE) +# define CATCH_CONFIG_CPP17_BYTE +#endif + +#if defined(CATCH_CONFIG_EXPERIMENTAL_REDIRECT) +# define CATCH_INTERNAL_CONFIG_NEW_CAPTURE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_NEW_CAPTURE) && !defined(CATCH_INTERNAL_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NO_NEW_CAPTURE) && !defined(CATCH_CONFIG_NEW_CAPTURE) +# define CATCH_CONFIG_NEW_CAPTURE +#endif + +#if !defined(CATCH_INTERNAL_CONFIG_EXCEPTIONS_ENABLED) && !defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +# define CATCH_CONFIG_DISABLE_EXCEPTIONS +#endif + +#if defined(CATCH_INTERNAL_CONFIG_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_NO_POLYFILL_ISNAN) && !defined(CATCH_CONFIG_POLYFILL_ISNAN) +# define CATCH_CONFIG_POLYFILL_ISNAN +#endif + +#if defined(CATCH_INTERNAL_CONFIG_USE_ASYNC) && !defined(CATCH_INTERNAL_CONFIG_NO_ASYNC) && !defined(CATCH_CONFIG_NO_USE_ASYNC) && !defined(CATCH_CONFIG_USE_ASYNC) +# define CATCH_CONFIG_USE_ASYNC +#endif + +#if defined(CATCH_INTERNAL_CONFIG_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_NO_ANDROID_LOGWRITE) && !defined(CATCH_CONFIG_ANDROID_LOGWRITE) +# define CATCH_CONFIG_ANDROID_LOGWRITE +#endif + +#if defined(CATCH_INTERNAL_CONFIG_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_NO_GLOBAL_NEXTAFTER) && !defined(CATCH_CONFIG_GLOBAL_NEXTAFTER) +# define CATCH_CONFIG_GLOBAL_NEXTAFTER +#endif + +// Even if we do not think the compiler has that warning, we still have +// to provide a macro that can be used by the code. +#if !defined(CATCH_INTERNAL_START_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_START_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION) +# define CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_PARENTHESES_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_WARNINGS +#endif +#if !defined(CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_ZERO_VARIADIC_WARNINGS +#endif + +// The goal of this macro is to avoid evaluation of the arguments, but +// still have the compiler warn on problems inside... +#if !defined(CATCH_INTERNAL_IGNORE_BUT_WARN) +# define CATCH_INTERNAL_IGNORE_BUT_WARN(...) +#endif + +#if defined(__APPLE__) && defined(__apple_build_version__) && (__clang_major__ < 10) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#elif defined(__clang__) && (__clang_major__ < 5) +# undef CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if !defined(CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS) +# define CATCH_INTERNAL_SUPPRESS_UNUSED_TEMPLATE_WARNINGS +#endif + +#if defined(CATCH_CONFIG_DISABLE_EXCEPTIONS) +#define CATCH_TRY if ((true)) +#define CATCH_CATCH_ALL if ((false)) +#define CATCH_CATCH_ANON(type) if ((false)) +#else +#define CATCH_TRY try +#define CATCH_CATCH_ALL catch (...) +#define CATCH_CATCH_ANON(type) catch (type) +#endif + +#if defined(CATCH_INTERNAL_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_NO_TRADITIONAL_MSVC_PREPROCESSOR) && !defined(CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR) +#define CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#endif + +// end catch_compiler_capabilities.h +#define INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) name##line +#define INTERNAL_CATCH_UNIQUE_NAME_LINE( name, line ) INTERNAL_CATCH_UNIQUE_NAME_LINE2( name, line ) +#ifdef CATCH_CONFIG_COUNTER +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __COUNTER__ ) +#else +# define INTERNAL_CATCH_UNIQUE_NAME( name ) INTERNAL_CATCH_UNIQUE_NAME_LINE( name, __LINE__ ) +#endif + +#include +#include +#include + +// We need a dummy global operator<< so we can bring it into Catch namespace later +struct Catch_global_namespace_dummy {}; +std::ostream& operator<<(std::ostream&, Catch_global_namespace_dummy); + +namespace Catch { + + struct CaseSensitive { enum Choice { + Yes, + No + }; }; + + class NonCopyable { + NonCopyable( NonCopyable const& ) = delete; + NonCopyable( NonCopyable && ) = delete; + NonCopyable& operator = ( NonCopyable const& ) = delete; + NonCopyable& operator = ( NonCopyable && ) = delete; + + protected: + NonCopyable(); + virtual ~NonCopyable(); + }; + + struct SourceLineInfo { + + SourceLineInfo() = delete; + SourceLineInfo( char const* _file, std::size_t _line ) noexcept + : file( _file ), + line( _line ) + {} + + SourceLineInfo( SourceLineInfo const& other ) = default; + SourceLineInfo& operator = ( SourceLineInfo const& ) = default; + SourceLineInfo( SourceLineInfo&& ) noexcept = default; + SourceLineInfo& operator = ( SourceLineInfo&& ) noexcept = default; + + bool empty() const noexcept { return file[0] == '\0'; } + bool operator == ( SourceLineInfo const& other ) const noexcept; + bool operator < ( SourceLineInfo const& other ) const noexcept; + + char const* file; + std::size_t line; + }; + + std::ostream& operator << ( std::ostream& os, SourceLineInfo const& info ); + + // Bring in operator<< from global namespace into Catch namespace + // This is necessary because the overload of operator<< above makes + // lookup stop at namespace Catch + using ::operator<<; + + // Use this in variadic streaming macros to allow + // >> +StreamEndStop + // as well as + // >> stuff +StreamEndStop + struct StreamEndStop { + std::string operator+() const; + }; + template + T const& operator + ( T const& value, StreamEndStop ) { + return value; + } +} + +#define CATCH_INTERNAL_LINEINFO \ + ::Catch::SourceLineInfo( __FILE__, static_cast( __LINE__ ) ) + +// end catch_common.h +namespace Catch { + + struct RegistrarForTagAliases { + RegistrarForTagAliases( char const* alias, char const* tag, SourceLineInfo const& lineInfo ); + }; + +} // end namespace Catch + +#define CATCH_REGISTER_TAG_ALIAS( alias, spec ) \ + CATCH_INTERNAL_START_WARNINGS_SUPPRESSION \ + CATCH_INTERNAL_SUPPRESS_GLOBALS_WARNINGS \ + namespace{ Catch::RegistrarForTagAliases INTERNAL_CATCH_UNIQUE_NAME( AutoRegisterTagAlias )( alias, spec, CATCH_INTERNAL_LINEINFO ); } \ + CATCH_INTERNAL_STOP_WARNINGS_SUPPRESSION + +// end catch_tag_alias_autoregistrar.h +// start catch_test_registry.h + +// start catch_interfaces_testcase.h + +#include + +namespace Catch { + + class TestSpec; + + struct ITestInvoker { + virtual void invoke () const = 0; + virtual ~ITestInvoker(); + }; + + class TestCase; + struct IConfig; + + struct ITestCaseRegistry { + virtual ~ITestCaseRegistry(); + virtual std::vector const& getAllTests() const = 0; + virtual std::vector const& getAllTestsSorted( IConfig const& config ) const = 0; + }; + + bool isThrowSafe( TestCase const& testCase, IConfig const& config ); + bool matchTest( TestCase const& testCase, TestSpec const& testSpec, IConfig const& config ); + std::vector filterTests( std::vector const& testCases, TestSpec const& testSpec, IConfig const& config ); + std::vector const& getAllTestCasesSorted( IConfig const& config ); + +} + +// end catch_interfaces_testcase.h +// start catch_stringref.h + +#include +#include +#include +#include + +namespace Catch { + + /// A non-owning string class (similar to the forthcoming std::string_view) + /// Note that, because a StringRef may be a substring of another string, + /// it may not be null terminated. + class StringRef { + public: + using size_type = std::size_t; + using const_iterator = const char*; + + private: + static constexpr char const* const s_empty = ""; + + char const* m_start = s_empty; + size_type m_size = 0; + + public: // construction + constexpr StringRef() noexcept = default; + + StringRef( char const* rawChars ) noexcept; + + constexpr StringRef( char const* rawChars, size_type size ) noexcept + : m_start( rawChars ), + m_size( size ) + {} + + StringRef( std::string const& stdString ) noexcept + : m_start( stdString.c_str() ), + m_size( stdString.size() ) + {} + + explicit operator std::string() const { + return std::string(m_start, m_size); + } + + public: // operators + auto operator == ( StringRef const& other ) const noexcept -> bool; + auto operator != (StringRef const& other) const noexcept -> bool { + return !(*this == other); + } + + auto operator[] ( size_type index ) const noexcept -> char { + assert(index < m_size); + return m_start[index]; + } + + public: // named queries + constexpr auto empty() const noexcept -> bool { + return m_size == 0; + } + constexpr auto size() const noexcept -> size_type { + return m_size; + } + + // Returns the current start pointer. If the StringRef is not + // null-terminated, throws std::domain_exception + auto c_str() const -> char const*; + + public: // substrings and searches + // Returns a substring of [start, start + length). + // If start + length > size(), then the substring is [start, size()). + // If start > size(), then the substring is empty. + auto substr( size_type start, size_type length ) const noexcept -> StringRef; + + // Returns the current start pointer. May not be null-terminated. + auto data() const noexcept -> char const*; + + constexpr auto isNullTerminated() const noexcept -> bool { + return m_start[m_size] == '\0'; + } + + public: // iterators + constexpr const_iterator begin() const { return m_start; } + constexpr const_iterator end() const { return m_start + m_size; } + }; + + auto operator += ( std::string& lhs, StringRef const& sr ) -> std::string&; + auto operator << ( std::ostream& os, StringRef const& sr ) -> std::ostream&; + + constexpr auto operator "" _sr( char const* rawChars, std::size_t size ) noexcept -> StringRef { + return StringRef( rawChars, size ); + } +} // namespace Catch + +constexpr auto operator "" _catch_sr( char const* rawChars, std::size_t size ) noexcept -> Catch::StringRef { + return Catch::StringRef( rawChars, size ); +} + +// end catch_stringref.h +// start catch_preprocessor.hpp + + +#define CATCH_RECURSION_LEVEL0(...) __VA_ARGS__ +#define CATCH_RECURSION_LEVEL1(...) CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(CATCH_RECURSION_LEVEL0(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL2(...) CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(CATCH_RECURSION_LEVEL1(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL3(...) CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(CATCH_RECURSION_LEVEL2(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL4(...) CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(CATCH_RECURSION_LEVEL3(__VA_ARGS__))) +#define CATCH_RECURSION_LEVEL5(...) CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(CATCH_RECURSION_LEVEL4(__VA_ARGS__))) + +#ifdef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_EXPAND_VARGS(...) __VA_ARGS__ +// MSVC needs more evaluations +#define CATCH_RECURSION_LEVEL6(...) CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(CATCH_RECURSION_LEVEL5(__VA_ARGS__))) +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL6(CATCH_RECURSION_LEVEL6(__VA_ARGS__)) +#else +#define CATCH_RECURSE(...) CATCH_RECURSION_LEVEL5(__VA_ARGS__) +#endif + +#define CATCH_REC_END(...) +#define CATCH_REC_OUT + +#define CATCH_EMPTY() +#define CATCH_DEFER(id) id CATCH_EMPTY() + +#define CATCH_REC_GET_END2() 0, CATCH_REC_END +#define CATCH_REC_GET_END1(...) CATCH_REC_GET_END2 +#define CATCH_REC_GET_END(...) CATCH_REC_GET_END1 +#define CATCH_REC_NEXT0(test, next, ...) next CATCH_REC_OUT +#define CATCH_REC_NEXT1(test, next) CATCH_DEFER ( CATCH_REC_NEXT0 ) ( test, next, 0) +#define CATCH_REC_NEXT(test, next) CATCH_REC_NEXT1(CATCH_REC_GET_END test, next) + +#define CATCH_REC_LIST0(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1(f, x, peek, ...) , f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0) ) ( f, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2(f, x, peek, ...) f(x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1) ) ( f, peek, __VA_ARGS__ ) + +#define CATCH_REC_LIST0_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST1_UD(f, userdata, x, peek, ...) , f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST0_UD) ) ( f, userdata, peek, __VA_ARGS__ ) +#define CATCH_REC_LIST2_UD(f, userdata, x, peek, ...) f(userdata, x) CATCH_DEFER ( CATCH_REC_NEXT(peek, CATCH_REC_LIST1_UD) ) ( f, userdata, peek, __VA_ARGS__ ) + +// Applies the function macro `f` to each of the remaining parameters, inserts commas between the results, +// and passes userdata as the first parameter to each invocation, +// e.g. CATCH_REC_LIST_UD(f, x, a, b, c) evaluates to f(x, a), f(x, b), f(x, c) +#define CATCH_REC_LIST_UD(f, userdata, ...) CATCH_RECURSE(CATCH_REC_LIST2_UD(f, userdata, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define CATCH_REC_LIST(f, ...) CATCH_RECURSE(CATCH_REC_LIST2(f, __VA_ARGS__, ()()(), ()()(), ()()(), 0)) + +#define INTERNAL_CATCH_EXPAND1(param) INTERNAL_CATCH_EXPAND2(param) +#define INTERNAL_CATCH_EXPAND2(...) INTERNAL_CATCH_NO## __VA_ARGS__ +#define INTERNAL_CATCH_DEF(...) INTERNAL_CATCH_DEF __VA_ARGS__ +#define INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE(...) INTERNAL_CATCH_STRINGIZE2(__VA_ARGS__) +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_STRINGIZE2(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) +#else +// MSVC is adding extra space and needs another indirection to expand INTERNAL_CATCH_NOINTERNAL_CATCH_DEF +#define INTERNAL_CATCH_STRINGIZE2(...) INTERNAL_CATCH_STRINGIZE3(__VA_ARGS__) +#define INTERNAL_CATCH_STRINGIZE3(...) #__VA_ARGS__ +#define INTERNAL_CATCH_STRINGIZE_WITHOUT_PARENS(param) (INTERNAL_CATCH_STRINGIZE(INTERNAL_CATCH_REMOVE_PARENS(param)) + 1) +#endif + +#define INTERNAL_CATCH_MAKE_NAMESPACE2(...) ns_##__VA_ARGS__ +#define INTERNAL_CATCH_MAKE_NAMESPACE(name) INTERNAL_CATCH_MAKE_NAMESPACE2(name) + +#define INTERNAL_CATCH_REMOVE_PARENS(...) INTERNAL_CATCH_EXPAND1(INTERNAL_CATCH_DEF __VA_ARGS__) + +#ifndef CATCH_CONFIG_TRADITIONAL_MSVC_PREPROCESSOR +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) decltype(get_wrapper()) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__)) +#else +#define INTERNAL_CATCH_MAKE_TYPE_LIST2(...) INTERNAL_CATCH_EXPAND_VARGS(decltype(get_wrapper())) +#define INTERNAL_CATCH_MAKE_TYPE_LIST(...) INTERNAL_CATCH_EXPAND_VARGS(INTERNAL_CATCH_MAKE_TYPE_LIST2(INTERNAL_CATCH_REMOVE_PARENS(__VA_ARGS__))) +#endif + +#define INTERNAL_CATCH_MAKE_TYPE_LISTS_FROM_TYPES(...)\ + CATCH_REC_LIST(INTERNAL_CATCH_MAKE_TYPE_LIST,__VA_ARGS__) + +#define INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_0) INTERNAL_CATCH_REMOVE_PARENS(_0) +#define INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_0, _1) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_1_ARG(_1) +#define INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_0, _1, _2) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_2_ARG(_1, _2) +#define INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_0, _1, _2, _3) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_3_ARG(_1, _2, _3) +#define INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_0, _1, _2, _3, _4) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_4_ARG(_1, _2, _3, _4) +#define INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_0, _1, _2, _3, _4, _5) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_5_ARG(_1, _2, _3, _4, _5) +#define INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_0, _1, _2, _3, _4, _5, _6) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_6_ARG(_1, _2, _3, _4, _5, _6) +#define INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_0, _1, _2, _3, _4, _5, _6, _7) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_7_ARG(_1, _2, _3, _4, _5, _6, _7) +#define INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_8_ARG(_1, _2, _3, _4, _5, _6, _7, _8) +#define INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_9_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9) +#define INTERNAL_CATCH_REMOVE_PARENS_11_ARG(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10) INTERNAL_CATCH_REMOVE_PARENS(_0), INTERNAL_CATCH_REMOVE_PARENS_10_ARG(_1, _2, _3, _4, _5, _6, _7, _8, _9, _10) + +#define INTERNAL_CATCH_VA_NARGS_IMPL(_0, _1, _2, _3, _4, _5, _6, _7, _8, _9, _10, N, ...) N + +#define INTERNAL_CATCH_TYPE_GEN\ + template struct TypeList {};\ + template\ + constexpr auto get_wrapper() noexcept -> TypeList { return {}; }\ + template class...> struct TemplateTypeList{};\ + template class...Cs>\ + constexpr auto get_wrapper() noexcept -> TemplateTypeList { return {}; }\ + template\ + struct append;\ + template\ + struct rewrap;\ + template class, typename...>\ + struct create;\ + template class, typename>\ + struct convert;\ + \ + template \ + struct append { using type = T; };\ + template< template class L1, typename...E1, template class L2, typename...E2, typename...Rest>\ + struct append, L2, Rest...> { using type = typename append, Rest...>::type; };\ + template< template class L1, typename...E1, typename...Rest>\ + struct append, TypeList, Rest...> { using type = L1; };\ + \ + template< template class Container, template class List, typename...elems>\ + struct rewrap, List> { using type = TypeList>; };\ + template< template class Container, template class List, class...Elems, typename...Elements>\ + struct rewrap, List, Elements...> { using type = typename append>, typename rewrap, Elements...>::type>::type; };\ + \ + template