mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-08-21 08:03:42 +08:00
dragonpilot beta3
date: 2023-07-26T22:20:36 commit: c6d842c412052be1985b63d683c63be9dcb2b0eb
This commit is contained in:
dragonpilot
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Vendored
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
BIN
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,387 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
long long *l_ptr;
|
||||
int status = 0;
|
||||
|
||||
// create solver
|
||||
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
|
||||
|
||||
status = {{ model.name }}_acados_create(acados_ocp_capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_create() returned status %d.\n", status);
|
||||
}
|
||||
mexPrintf("{{ model.name }}_acados_create() -> success!\n");
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_plan_t *nlp_plan = {{ model.name }}_acados_get_nlp_plan(acados_ocp_capsule);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// mexPrintf("acados: got pointer to objectes!\n");
|
||||
|
||||
// field names of output struct
|
||||
#define FIELDS_OCP 9
|
||||
#define FIELDS_EXT_FUN 25
|
||||
#define MAX_FIELDS 25
|
||||
char *fieldnames[MAX_FIELDS];
|
||||
|
||||
for (int i = 0; i < MAX_FIELDS; i++)
|
||||
{
|
||||
fieldnames[i] = (char*) mxMalloc(50);
|
||||
}
|
||||
|
||||
memcpy(fieldnames[0],"config",sizeof("config"));
|
||||
memcpy(fieldnames[1],"dims",sizeof("dims"));
|
||||
memcpy(fieldnames[2],"opts",sizeof("opts"));
|
||||
memcpy(fieldnames[3],"in",sizeof("in"));
|
||||
memcpy(fieldnames[4],"out",sizeof("out"));
|
||||
memcpy(fieldnames[5],"solver",sizeof("solver"));
|
||||
memcpy(fieldnames[6],"sens_out",sizeof("sens_out"));
|
||||
memcpy(fieldnames[7],"plan",sizeof("plan"));
|
||||
memcpy(fieldnames[8],"capsule",sizeof("capsule"));
|
||||
|
||||
// create output struct - C_ocp
|
||||
plhs[0] = mxCreateStructMatrix(1, 1, 9, (const char **) fieldnames);
|
||||
|
||||
// MEX: config, dims, opts, in, out, solver, sens_out, plan
|
||||
// plan
|
||||
mxArray *plan_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(plan_mat);
|
||||
l_ptr[0] = (long long) nlp_plan;
|
||||
mxSetField(plhs[0], 0, "plan", plan_mat);
|
||||
|
||||
// config
|
||||
mxArray *config_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(config_mat);
|
||||
l_ptr[0] = (long long) nlp_config;
|
||||
mxSetField(plhs[0], 0, "config", config_mat);
|
||||
|
||||
// dims
|
||||
mxArray *dims_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(dims_mat);
|
||||
l_ptr[0] = (long long) nlp_dims;
|
||||
mxSetField(plhs[0], 0, "dims", dims_mat);
|
||||
|
||||
// opts
|
||||
mxArray *opts_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(opts_mat);
|
||||
l_ptr[0] = (long long) nlp_opts;
|
||||
mxSetField(plhs[0], 0, "opts", opts_mat);
|
||||
|
||||
// in
|
||||
mxArray *in_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(in_mat);
|
||||
l_ptr[0] = (long long) nlp_in;
|
||||
mxSetField(plhs[0], 0, "in", in_mat);
|
||||
|
||||
// out
|
||||
mxArray *out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(out_mat);
|
||||
l_ptr[0] = (long long) nlp_out;
|
||||
mxSetField(plhs[0], 0, "out", out_mat);
|
||||
|
||||
// solver
|
||||
mxArray *solver_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(solver_mat);
|
||||
l_ptr[0] = (long long) nlp_solver;
|
||||
mxSetField(plhs[0], 0, "solver", solver_mat);
|
||||
|
||||
// TODO: sens_out not actually implemented in templates..
|
||||
// sens_out
|
||||
mxArray *sens_out_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(sens_out_mat);
|
||||
l_ptr[0] = (long long) 1;
|
||||
mxSetField(plhs[0], 0, "sens_out", sens_out_mat);
|
||||
|
||||
// capsule
|
||||
mxArray *capsule_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(capsule_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule;
|
||||
mxSetField(plhs[0], 0, "capsule", capsule_mat);
|
||||
|
||||
/* store external function pointers */
|
||||
// dyn
|
||||
memcpy(fieldnames[0],"expl_ode_fun",sizeof("expl_ode_fun"));
|
||||
memcpy(fieldnames[1],"forw_vde",sizeof("forw_vde"));
|
||||
memcpy(fieldnames[2],"hess_vde",sizeof("hess_vde"));
|
||||
memcpy(fieldnames[3],"impl_dae_fun",sizeof("impl_dae_fun"));
|
||||
memcpy(fieldnames[4],"impl_dae_fun_jac_x_xdot_z",sizeof("impl_dae_fun_jac_x_xdot_z"));
|
||||
memcpy(fieldnames[5],"impl_dae_jac_x_xdot_u_z",sizeof("impl_dae_jac_x_xdot_u_z"));
|
||||
memcpy(fieldnames[6],"impl_dae_hess",sizeof("impl_dae_hess"));
|
||||
|
||||
memcpy(fieldnames[7],"gnsf_phi_fun",sizeof("gnsf_phi_fun"));
|
||||
memcpy(fieldnames[8],"gnsf_phi_fun_jac_y",sizeof("gnsf_phi_fun_jac_y"));
|
||||
memcpy(fieldnames[9],"gnsf_phi_jac_y_uhat",sizeof("gnsf_phi_jac_y_uhat"));
|
||||
memcpy(fieldnames[10],"gnsf_f_lo_jac_x1_x1dot_u_z",sizeof("gnsf_f_lo_jac_x1_x1dot_u_z"));
|
||||
memcpy(fieldnames[11],"gnsf_get_matrices_fun",sizeof("gnsf_get_matrices_fun"));
|
||||
|
||||
memcpy(fieldnames[12],"disc_phi_fun",sizeof("disc_phi_fun"));
|
||||
memcpy(fieldnames[13],"disc_phi_fun_jac",sizeof("disc_phi_fun_jac"));
|
||||
memcpy(fieldnames[14],"disc_phi_fun_jac_hess",sizeof("disc_phi_fun_jac_hess"));
|
||||
|
||||
// cost
|
||||
memcpy(fieldnames[15],"cost_y_fun",sizeof("cost_y_fun"));
|
||||
memcpy(fieldnames[16],"cost_y_fun_jac_ut_xt",sizeof("cost_y_fun_jac_ut_xt"));
|
||||
memcpy(fieldnames[17],"cost_y_hess",sizeof("cost_y_hess"));
|
||||
memcpy(fieldnames[18],"ext_cost_fun",sizeof("ext_cost_fun"));
|
||||
memcpy(fieldnames[19],"ext_cost_fun_jac",sizeof("ext_cost_fun_jac"));
|
||||
memcpy(fieldnames[20],"ext_cost_fun_jac_hess",sizeof("ext_cost_fun_jac_hess"));
|
||||
|
||||
// constraints
|
||||
memcpy(fieldnames[21],"phi_constraint",sizeof("phi_constraint"));
|
||||
memcpy(fieldnames[22],"nl_constr_h_fun_jac",sizeof("nl_constr_h_fun_jac"));
|
||||
memcpy(fieldnames[23],"nl_constr_h_fun",sizeof("nl_constr_h_fun"));
|
||||
memcpy(fieldnames[24],"nl_constr_h_fun_jac_hess",sizeof("nl_constr_h_fun_jac_hess"));
|
||||
|
||||
|
||||
// create output struct - C_ocp_ext_fun
|
||||
plhs[1] = mxCreateStructMatrix(1, 1, FIELDS_EXT_FUN, (const char **) fieldnames);
|
||||
|
||||
|
||||
for (int i = 0; i < FIELDS_EXT_FUN; i++)
|
||||
{
|
||||
mxFree( fieldnames[i] );
|
||||
}
|
||||
|
||||
/* dynamics */
|
||||
mxArray *expl_ode_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *forw_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *hess_vde_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_fun_jac_x_xdot_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_jac_x_xdot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *impl_dae_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
mxArray *gnsf_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_phi_fun_jac_y_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_phi_jac_y_uhat_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_f_lo_jac_x1_x1dot_u_z_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *gnsf_get_matrices_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
mxArray *disc_phi_fun_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *disc_phi_fun_jac_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
mxArray *disc_phi_fun_jac_hess_mat = mxCreateNumericMatrix(1, 1, mxINT64_CLASS, mxREAL);
|
||||
|
||||
{% if solver_options.integrator_type == "ERK" %}
|
||||
{# TODO: remove _casadi from these names.. #}
|
||||
l_ptr = mxGetData(forw_vde_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->forw_vde_casadi;
|
||||
l_ptr = mxGetData(expl_ode_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->expl_ode_fun;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(hess_vde_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->hess_vde_casadi;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "IRK" %}
|
||||
l_ptr = mxGetData(impl_dae_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun;
|
||||
l_ptr = mxGetData(impl_dae_fun_jac_x_xdot_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_fun_jac_x_xdot_z;
|
||||
l_ptr = mxGetData(impl_dae_jac_x_xdot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_jac_x_xdot_u_z;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(impl_dae_hess_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->impl_dae_hess;
|
||||
{%- endif %}
|
||||
{% elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
l_ptr = mxGetData(gnsf_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun;
|
||||
l_ptr = mxGetData(gnsf_phi_fun_jac_y_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_fun_jac_y;
|
||||
l_ptr = mxGetData(gnsf_phi_jac_y_uhat_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_phi_jac_y_uhat;
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
l_ptr = mxGetData(gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_f_lo_jac_x1_x1dot_u_z;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
l_ptr = mxGetData(gnsf_get_matrices_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->gnsf_get_matrices_fun;
|
||||
{% elif solver_options.integrator_type == "DISCRETE" %}
|
||||
l_ptr = mxGetData(disc_phi_fun_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun;
|
||||
l_ptr = mxGetData(disc_phi_fun_jac_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt;
|
||||
{% if solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr = mxGetData(disc_phi_fun_jac_hess_mat);
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->discr_dyn_phi_fun_jac_ut_xt_hess;
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "expl_ode_fun", expl_ode_fun_mat);
|
||||
mxSetField(plhs[1], 0, "forw_vde", forw_vde_mat);
|
||||
mxSetField(plhs[1], 0, "hess_vde", hess_vde_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_fun", gnsf_phi_fun_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_fun_jac_y", gnsf_phi_fun_jac_y_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_phi_jac_y_uhat", gnsf_phi_jac_y_uhat_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_f_lo_jac_x1_x1dot_u_z", gnsf_f_lo_jac_x1_x1dot_u_z_mat);
|
||||
mxSetField(plhs[1], 0, "gnsf_get_matrices_fun", gnsf_get_matrices_fun_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "impl_dae_fun", impl_dae_fun_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_fun_jac_x_xdot_z", impl_dae_fun_jac_x_xdot_z_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_jac_x_xdot_u_z", impl_dae_jac_x_xdot_u_z_mat);
|
||||
mxSetField(plhs[1], 0, "impl_dae_hess", impl_dae_hess_mat);
|
||||
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun", disc_phi_fun_mat);
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun_jac", disc_phi_fun_jac_mat);
|
||||
mxSetField(plhs[1], 0, "disc_phi_fun_jac_hess", disc_phi_fun_jac_hess_mat);
|
||||
/* constaints */
|
||||
mxArray *phi_constraint_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(phi_constraint_mat);
|
||||
{%- if constraints.constr_type == "BGP" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->phi_constraint;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGP" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->phi_e_constraint;
|
||||
{% endif %}
|
||||
mxSetField(plhs[1], 0, "phi_constraint", phi_constraint_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_jac_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac", nl_constr_h_fun_jac_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun", nl_constr_h_fun_mat);
|
||||
|
||||
mxArray *nl_constr_h_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(nl_constr_h_fun_jac_hess_mat);
|
||||
{% if constraints.constr_type == "BGH" and dims.nh > 0 and solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->nl_constr_h_fun_jac_hess;
|
||||
{% endif %}
|
||||
{% if constraints.constr_type_e == "BGH" and dims.nh_e > 0 and solver_options.hessian_approx == "EXACT" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->nl_constr_h_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "nl_constr_h_fun_jac_hess", nl_constr_h_fun_jac_hess_mat);
|
||||
|
||||
/* cost */
|
||||
mxArray *cost_y_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_fun_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_fun", cost_y_fun_mat);
|
||||
|
||||
mxArray *cost_y_fun_jac_ut_xt_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_fun_jac_ut_xt_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_fun_jac_ut_xt;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_fun_jac_ut_xt;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_fun_jac_ut_xt", cost_y_fun_jac_ut_xt_mat);
|
||||
|
||||
mxArray *cost_y_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(cost_y_hess_mat);
|
||||
{% if cost.cost_type == "NONLINEAR_LS" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->cost_y_hess;
|
||||
{% endif %}
|
||||
{% if cost.cost_type_e == "NONLINEAR_LS" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->cost_y_e_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "cost_y_hess", cost_y_hess_mat);
|
||||
|
||||
mxArray *ext_cost_fun_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun", ext_cost_fun_mat);
|
||||
|
||||
mxArray *ext_cost_fun_jac_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_jac_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun_jac", ext_cost_fun_jac_mat);
|
||||
|
||||
mxArray *ext_cost_fun_jac_hess_mat = mxCreateNumericMatrix(1, 2, mxINT64_CLASS, mxREAL);
|
||||
l_ptr = mxGetData(ext_cost_fun_jac_hess_mat);
|
||||
{% if cost.cost_type == "EXTERNAL" %}
|
||||
l_ptr[0] = (long long) acados_ocp_capsule->ext_cost_fun_jac_hess;
|
||||
{% endif -%}
|
||||
{% if cost.cost_type_e == "EXTERNAL" %}
|
||||
l_ptr[1] = (long long) &acados_ocp_capsule->ext_cost_e_fun_jac_hess;
|
||||
{%- endif %}
|
||||
mxSetField(plhs[1], 0, "ext_cost_fun_jac_hess", ext_cost_fun_jac_hess_mat);
|
||||
|
||||
|
||||
return;
|
||||
}
|
||||
@@ -0,0 +1,70 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
// system
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
// acados
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
int status = 0;
|
||||
long long *ptr;
|
||||
|
||||
// mexPrintf("\nin mex_acados_free\n");
|
||||
const mxArray *C_ocp = prhs[0];
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
|
||||
status = {{ model.name }}_acados_free(capsule);
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
status = {{ model.name }}_acados_free_capsule(capsule);
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free_capsule() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,570 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
#include "mex_macros.h"
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
|
||||
long long *ptr;
|
||||
int acados_size;
|
||||
mxArray *mex_field;
|
||||
char fun_name[20] = "ocp_set";
|
||||
char buffer [500]; // for error messages
|
||||
|
||||
/* RHS */
|
||||
int min_nrhs = 6;
|
||||
|
||||
char *ext_fun_type = mxArrayToString( prhs[0] );
|
||||
char *ext_fun_type_e = mxArrayToString( prhs[1] );
|
||||
|
||||
// C ocp
|
||||
const mxArray *C_ocp = prhs[2];
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
// plan
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
|
||||
ocp_nlp_plan_t *plan = (ocp_nlp_plan_t *) ptr[0];
|
||||
// config
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "config" ) );
|
||||
ocp_nlp_config *config = (ocp_nlp_config *) ptr[0];
|
||||
// dims
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "dims" ) );
|
||||
ocp_nlp_dims *dims = (ocp_nlp_dims *) ptr[0];
|
||||
// opts
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "opts" ) );
|
||||
void *opts = (void *) ptr[0];
|
||||
// in
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "in" ) );
|
||||
ocp_nlp_in *in = (ocp_nlp_in *) ptr[0];
|
||||
// out
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "out" ) );
|
||||
ocp_nlp_out *out = (ocp_nlp_out *) ptr[0];
|
||||
// solver
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "solver" ) );
|
||||
ocp_nlp_solver *solver = (ocp_nlp_solver *) ptr[0];
|
||||
|
||||
const mxArray *C_ext_fun_pointers = prhs[3];
|
||||
// field
|
||||
char *field = mxArrayToString( prhs[4] );
|
||||
// value
|
||||
double *value = mxGetPr( prhs[5] );
|
||||
|
||||
// for checks
|
||||
int matlab_size = (int) mxGetNumberOfElements( prhs[5] );
|
||||
int nrow = (int) mxGetM( prhs[5] );
|
||||
int ncol = (int) mxGetN( prhs[5] );
|
||||
|
||||
int N = dims->N;
|
||||
int nu = dims->nu[0];
|
||||
int nx = dims->nx[0];
|
||||
|
||||
// stage
|
||||
int s0, se;
|
||||
if (nrhs==min_nrhs)
|
||||
{
|
||||
s0 = 0;
|
||||
se = N;
|
||||
}
|
||||
else if (nrhs==min_nrhs+1)
|
||||
{
|
||||
s0 = mxGetScalar( prhs[6] );
|
||||
if (s0 > N)
|
||||
{
|
||||
sprintf(buffer, "ocp_set: N < specified stage = %d\n", s0);
|
||||
mexErrMsgTxt(buffer);
|
||||
}
|
||||
se = s0 + 1;
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buffer, "ocp_set: wrong nrhs: %d\n", nrhs);
|
||||
mexErrMsgTxt(buffer);
|
||||
}
|
||||
|
||||
/* Set value */
|
||||
// constraints
|
||||
if (!strcmp(field, "constr_x0"))
|
||||
{
|
||||
acados_size = nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_constraints_model_set(config, dims, in, 0, "lbx", value);
|
||||
ocp_nlp_constraints_model_set(config, dims, in, 0, "ubx", value);
|
||||
}
|
||||
else if (!strcmp(field, "constr_C"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_MAT(fun_name, "constr_C", nrow, ncol, ng, nx);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "C", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lbx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbx");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbx", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ubx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubx");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubx", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lbu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lbu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lbu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ubu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ubu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ubu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_D"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int ng = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_MAT(fun_name, "constr_D", nrow, ncol, ng, nu);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "D", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lg"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lg");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lg", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_ug"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "ug");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "ug", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_lh"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lh");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "lh", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "constr_uh"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "uh");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
|
||||
ocp_nlp_constraints_model_set(config, dims, in, ii, "uh", value);
|
||||
}
|
||||
}
|
||||
// cost:
|
||||
else if (!strcmp(field, "cost_y_ref"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "y_ref", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_y_ref_e"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, N, "y_ref");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, N, "y_ref", value);
|
||||
}
|
||||
else if (!strcmp(field, "cost_Vu"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
int nu = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "u");
|
||||
acados_size = ny * nu;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Vu", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Vx"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "y_ref");
|
||||
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "x");
|
||||
acados_size = ny * nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Vx", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_W"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
if ((plan->nlp_cost[ii] == LINEAR_LS) || (plan->nlp_cost[ii] == NONLINEAR_LS))
|
||||
{
|
||||
int ny = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "y_ref");
|
||||
acados_size = ny * ny;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "W", value);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_FOR_COST_STAGE(fun_name, field, plan->nlp_cost[ii], ii);
|
||||
}
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Z"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_Z");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Z", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Zl"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zl");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Zl", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_Zu"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "Zu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "Zu", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_z"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "cost_z");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "z", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_zl"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zl");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "zl", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "cost_zu"))
|
||||
{
|
||||
acados_size = ocp_nlp_dims_get_from_attr(config, dims, out, s0, "zu");
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
ocp_nlp_cost_model_set(config, dims, in, ii, "zu", value);
|
||||
}
|
||||
}
|
||||
// constraints TODO
|
||||
// // NOTE(oj): how is it with Jbx, Jbu, idxb can they be changed?!
|
||||
// else if (!strcmp(field, "constr_lbx"))
|
||||
// {
|
||||
// // bounds at 0 are a special case.
|
||||
// if (s0==0)
|
||||
// {
|
||||
// sprintf(buffer, "%s cannot set %s for stage 0", fun_name, field);
|
||||
// mexErrMsgTxt(buffer);
|
||||
// }
|
||||
// }
|
||||
// initializations
|
||||
else if (!strcmp(field, "init_x"))
|
||||
{
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = (N+1) * nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<=N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "x", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_u"))
|
||||
{
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = N*nu;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "u", value+ii*nu);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_z"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
int nz = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "z");
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = N*nz;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "z_guess", value+ii*nz);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_xdot"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == IRK)
|
||||
{
|
||||
int nx = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "x");
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = N*nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "xdot_guess", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_z", "irk")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_gnsf_phi"))
|
||||
{
|
||||
sim_solver_plan_t sim_plan = plan->sim_solver_plan[0];
|
||||
sim_solver_t type = sim_plan.sim_solver;
|
||||
if (type == GNSF)
|
||||
{
|
||||
int nout = ocp_nlp_dims_get_from_attr(config, dims, out, 0, "init_gnsf_phi");
|
||||
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = N*nout;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_set(config, solver, ii, "gnsf_phi_guess", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, "init_gnsf_phi", "irk_gnsf")
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_pi"))
|
||||
{
|
||||
if (nrhs!=min_nrhs)
|
||||
MEX_SETTER_NO_STAGE_SUPPORT(fun_name, field)
|
||||
|
||||
acados_size = N*nx;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
for (int ii=0; ii<N; ii++)
|
||||
{
|
||||
ocp_nlp_out_set(config, dims, out, ii, "pi", value+ii*nx);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_lam"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int nlam = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "lam");
|
||||
MEX_DIM_CHECK_VEC(fun_name, "lam", nrow*ncol, nlam);
|
||||
|
||||
ocp_nlp_out_set(config, dims, out, ii, "lam", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "init_t"))
|
||||
{
|
||||
for (int ii=s0; ii<se; ii++)
|
||||
{
|
||||
int nt = ocp_nlp_dims_get_from_attr(config, dims, out, ii, "t");
|
||||
MEX_DIM_CHECK_VEC(fun_name, "t", nrow*ncol, nt);
|
||||
|
||||
ocp_nlp_out_set(config, dims, out, ii, "t", value);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "p"))
|
||||
{
|
||||
if (nrhs==min_nrhs) // all stages
|
||||
{
|
||||
for (int ii=0; ii<=N; ii++)
|
||||
{
|
||||
{{ model.name }}_acados_update_params(capsule, ii, value, matlab_size);
|
||||
}
|
||||
}
|
||||
else if (nrhs==min_nrhs+1) // one stage
|
||||
{
|
||||
int stage = mxGetScalar( prhs[6] );
|
||||
{{ model.name }}_acados_update_params(capsule, stage, value, matlab_size);
|
||||
}
|
||||
}
|
||||
else if (!strcmp(field, "nlp_solver_max_iter"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int nlp_solver_max_iter = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "max_iter", &nlp_solver_max_iter);
|
||||
}
|
||||
else if (!strcmp(field, "rti_phase"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int rti_phase = (int) value[0];
|
||||
if (plan->nlp_solver == SQP && rti_phase != 0)
|
||||
{
|
||||
MEX_FIELD_ONLY_SUPPORTED_FOR_SOLVER(fun_name, field, "sqp_rti")
|
||||
}
|
||||
ocp_nlp_solver_opts_set(config, opts, "rti_phase", &rti_phase);
|
||||
}
|
||||
else if (!strcmp(field, "qp_warm_start"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int qp_warm_start = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "qp_warm_start", &qp_warm_start);
|
||||
}
|
||||
else if (!strcmp(field, "warm_start_first_qp"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int warm_start_first_qp = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "warm_start_first_qp", &warm_start_first_qp);
|
||||
}
|
||||
else if (!strcmp(field, "print_level"))
|
||||
{
|
||||
acados_size = 1;
|
||||
MEX_DIM_CHECK_VEC(fun_name, field, matlab_size, acados_size);
|
||||
int print_level = (int) value[0];
|
||||
ocp_nlp_solver_opts_set(config, opts, "print_level", &print_level);
|
||||
}
|
||||
else
|
||||
{
|
||||
MEX_FIELD_NOT_SUPPORTED_SUGGEST(fun_name, field, "p, constr_x0,\
|
||||
constr_lbx, constr_ubx, constr_C, constr_D, constr_lg, constr_ug, constr_lh, constr_uh\
|
||||
constr_lbu, constr_ubu, cost_y_ref[_e],\
|
||||
cost_Vu, cost_Vx, cost_Vz, cost_W, cost_Z, cost_Zl, cost_Zu, cost_z,\
|
||||
cost_zl, cost_zu, init_x, init_u, init_z, init_xdot, init_gnsf_phi,\
|
||||
init_pi, nlp_solver_max_iter, qp_warm_start, warm_start_first_qp, print_level");
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
// system
|
||||
#include <stdlib.h>
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
// acados
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
// C_ocp
|
||||
long long *ptr;
|
||||
const mxArray *C_ocp = prhs[0];
|
||||
|
||||
// capsule
|
||||
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
|
||||
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
|
||||
|
||||
// solve
|
||||
{{ model.name }}_acados_solve(capsule);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,508 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
{%- 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 %}
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
// acados
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
#include "acados/sim/sim_common.h"
|
||||
#include "acados/utils/external_function_generic.h"
|
||||
#include "acados/utils/print.h"
|
||||
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
|
||||
// ** solver data **
|
||||
|
||||
sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule()
|
||||
{
|
||||
void* capsule_mem = malloc(sizeof(sim_solver_capsule));
|
||||
sim_solver_capsule *capsule = (sim_solver_capsule *) capsule_mem;
|
||||
|
||||
return capsule;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule)
|
||||
{
|
||||
free(capsule);
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
|
||||
{
|
||||
// initialize
|
||||
const int nx = {{ model.name | upper }}_NX;
|
||||
const int nu = {{ model.name | upper }}_NU;
|
||||
const int nz = {{ model.name | upper }}_NZ;
|
||||
const int np = {{ model.name | upper }}_NP;
|
||||
bool tmp_bool;
|
||||
|
||||
{#// double Tsim = {{ solver_options.tf / dims.N }};#}
|
||||
double Tsim = {{ solver_options.Tsim }};
|
||||
|
||||
{% if solver_options.integrator_type == "IRK" %}
|
||||
capsule->sim_impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
// external functions (implicit model)
|
||||
capsule->sim_impl_dae_fun->casadi_fun = &{{ model.name }}_impl_dae_fun;
|
||||
capsule->sim_impl_dae_fun->casadi_work = &{{ model.name }}_impl_dae_fun_work;
|
||||
capsule->sim_impl_dae_fun->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in;
|
||||
capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
|
||||
capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
|
||||
capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
|
||||
external_function_param_casadi_create(capsule->sim_impl_dae_fun, np);
|
||||
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
|
||||
external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
|
||||
|
||||
// external_function_param_casadi impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_work = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_work;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
|
||||
external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
// external_function_param_casadi impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_impl_dae_hess->casadi_fun = &{{ model.name }}_impl_dae_hess;
|
||||
capsule->sim_impl_dae_hess->casadi_work = &{{ model.name }}_impl_dae_hess_work;
|
||||
capsule->sim_impl_dae_hess->casadi_sparsity_in = &{{ model.name }}_impl_dae_hess_sparsity_in;
|
||||
capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
|
||||
capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
|
||||
capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
|
||||
external_function_param_casadi_create(capsule->sim_impl_dae_hess, np);
|
||||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "ERK" %}
|
||||
// explicit ode
|
||||
capsule->sim_forw_vde_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_expl_ode_fun_casadi = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
capsule->sim_forw_vde_casadi->casadi_fun = &{{ model.name }}_expl_vde_forw;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_in = &{{ model.name }}_expl_vde_forw_n_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_n_out = &{{ model.name }}_expl_vde_forw_n_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
|
||||
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
|
||||
capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
|
||||
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
|
||||
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_n_out = &{{ model.name }}_expl_ode_fun_n_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
|
||||
capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
|
||||
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
// external_function_param_casadi impl_dae_jac_x_xdot_u_z;
|
||||
capsule->sim_expl_ode_hess->casadi_fun = &{{ model.name }}_expl_ode_hess;
|
||||
capsule->sim_expl_ode_hess->casadi_work = &{{ model.name }}_expl_ode_hess_work;
|
||||
capsule->sim_expl_ode_hess->casadi_sparsity_in = &{{ model.name }}_expl_ode_hess_sparsity_in;
|
||||
capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
|
||||
capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
|
||||
capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
|
||||
external_function_param_casadi_create(capsule->sim_expl_ode_hess, np);
|
||||
{%- endif %}
|
||||
|
||||
{% elif solver_options.integrator_type == "GNSF" -%}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_fun_jac_y = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
capsule->sim_gnsf_phi_jac_y_uhat = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
|
||||
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun->casadi_fun = &{{ model.name }}_gnsf_phi_fun;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_n_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_n_out;
|
||||
capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
|
||||
capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
|
||||
capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np);
|
||||
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_out = &{{ model.name }}_gnsf_phi_fun_jac_y_n_out;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
|
||||
capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np);
|
||||
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_out;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
|
||||
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
|
||||
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_n_out = &{{ model.name }}_gnsf_get_matrices_fun_n_out;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
|
||||
capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
|
||||
external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np);
|
||||
{% endif %}
|
||||
|
||||
// sim plan & config
|
||||
sim_solver_plan_t plan;
|
||||
plan.sim_solver = {{ solver_options.integrator_type }};
|
||||
|
||||
// create correct config based on plan
|
||||
sim_config * {{ model.name }}_sim_config = sim_config_create(plan);
|
||||
capsule->acados_sim_config = {{ model.name }}_sim_config;
|
||||
|
||||
// sim dims
|
||||
void *{{ model.name }}_sim_dims = sim_dims_create({{ model.name }}_sim_config);
|
||||
capsule->acados_sim_dims = {{ model.name }}_sim_dims;
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx", &nx);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nu", &nu);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz", &nz);
|
||||
{% if solver_options.integrator_type == "GNSF" %}
|
||||
int gnsf_nx1 = {{ dims.gnsf_nx1 }};
|
||||
int gnsf_nz1 = {{ dims.gnsf_nz1 }};
|
||||
int gnsf_nout = {{ dims.gnsf_nout }};
|
||||
int gnsf_ny = {{ dims.gnsf_ny }};
|
||||
int gnsf_nuhat = {{ dims.gnsf_nuhat }};
|
||||
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nx1", &gnsf_nx1);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nz1", &gnsf_nz1);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nout", &gnsf_nout);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "ny", &gnsf_ny);
|
||||
sim_dims_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims, "nuhat", &gnsf_nuhat);
|
||||
{% endif %}
|
||||
|
||||
// sim opts
|
||||
sim_opts *{{ model.name }}_sim_opts = sim_opts_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_opts = {{ model.name }}_sim_opts;
|
||||
int tmp_int = {{ solver_options.sim_method_newton_iter }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "newton_iter", &tmp_int);
|
||||
sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
|
||||
|
||||
{% if problem_class == "SIM" %}
|
||||
tmp_int = {{ solver_options.sim_method_num_stages }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = {{ solver_options.sim_method_num_steps }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
|
||||
|
||||
// options that are not available to AcadosOcpSolver
|
||||
// (in OCP they will be determined by other options, like exact_hessian)
|
||||
tmp_bool = {{ solver_options.sens_forw }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_forw", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_adj }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_adj", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_algebraic }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_algebraic", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.sens_hess }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "sens_hess", &tmp_bool);
|
||||
tmp_bool = {{ solver_options.output_z }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "output_z", &tmp_bool);
|
||||
|
||||
{% else %} {# num_stages and num_steps of first shooting interval are used #}
|
||||
tmp_int = {{ solver_options.sim_method_num_stages[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_stages", &tmp_int);
|
||||
tmp_int = {{ solver_options.sim_method_num_steps[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "num_steps", &tmp_int);
|
||||
tmp_bool = {{ solver_options.sim_method_jac_reuse[0] }};
|
||||
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool);
|
||||
{% endif %}
|
||||
|
||||
// sim in / out
|
||||
sim_in *{{ model.name }}_sim_in = sim_in_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_in = {{ model.name }}_sim_in;
|
||||
sim_out *{{ model.name }}_sim_out = sim_out_create({{ model.name }}_sim_config, {{ model.name }}_sim_dims);
|
||||
capsule->acados_sim_out = {{ model.name }}_sim_out;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "T", &Tsim);
|
||||
|
||||
// model functions
|
||||
{%- if solver_options.integrator_type == "IRK" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_fun", capsule->sim_impl_dae_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_fun_jac_x_xdot", capsule->sim_impl_dae_fun_jac_x_xdot_z);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_ode_jac_x_xdot_u", capsule->sim_impl_dae_jac_x_xdot_u_z);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"impl_dae_hess", capsule->sim_impl_dae_hess);
|
||||
{%- endif %}
|
||||
|
||||
{%- elif solver_options.integrator_type == "ERK" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_vde_for", capsule->sim_forw_vde_casadi);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_ode_fun", capsule->sim_expl_ode_fun_casadi);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"expl_ode_hess", capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun", capsule->sim_gnsf_phi_fun);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_fun_jac_y", capsule->sim_gnsf_phi_fun_jac_y);
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"phi_jac_y_uhat", capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"f_lo_jac_x1_x1dot_u_z", capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
{{ model.name }}_sim_config->model_set({{ model.name }}_sim_in->model,
|
||||
"gnsf_get_matrices_fun", capsule->sim_gnsf_get_matrices_fun);
|
||||
{%- endif %}
|
||||
|
||||
// sim solver
|
||||
sim_solver *{{ model.name }}_sim_solver = sim_solver_create({{ model.name }}_sim_config,
|
||||
{{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
|
||||
capsule->acados_sim_solver = {{ model.name }}_sim_solver;
|
||||
|
||||
{% if dims.np > 0 %}
|
||||
/* initialize parameter values */
|
||||
double* p = calloc(np, sizeof(double));
|
||||
{% for item in parameter_values %}
|
||||
{%- if item != 0 %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endif %}
|
||||
{%- endfor %}
|
||||
|
||||
{{ model.name }}_acados_sim_update_params(capsule, p, np);
|
||||
free(p);
|
||||
{% endif %}{# if dims.np #}
|
||||
|
||||
/* initialize input */
|
||||
// x
|
||||
double x0[{{ dims.nx }}];
|
||||
for (int ii = 0; ii < {{ dims.nx }}; ii++)
|
||||
x0[ii] = 0.0;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "x", x0);
|
||||
|
||||
|
||||
// u
|
||||
double u0[{{ dims.nu }}];
|
||||
for (int ii = 0; ii < {{ dims.nu }}; ii++)
|
||||
u0[ii] = 0.0;
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "u", u0);
|
||||
|
||||
// S_forw
|
||||
double S_forw[{{ dims.nx * (dims.nx + dims.nu) }}];
|
||||
for (int ii = 0; ii < {{ dims.nx * (dims.nx + dims.nu) }}; ii++)
|
||||
S_forw[ii] = 0.0;
|
||||
for (int ii = 0; ii < {{ dims.nx }}; ii++)
|
||||
S_forw[ii + ii * {{ dims.nx }} ] = 1.0;
|
||||
|
||||
|
||||
sim_in_set({{ model.name }}_sim_config, {{ model.name }}_sim_dims,
|
||||
{{ model.name }}_sim_in, "S_forw", S_forw);
|
||||
|
||||
int status = sim_precompute({{ model.name }}_sim_solver, {{ model.name }}_sim_in, {{ model.name }}_sim_out);
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_solve(sim_solver_capsule *capsule)
|
||||
{
|
||||
// integrate dynamics using acados sim_solver
|
||||
int status = sim_solve(capsule->acados_sim_solver,
|
||||
capsule->acados_sim_in, capsule->acados_sim_out);
|
||||
if (status != 0)
|
||||
printf("error in {{ model.name }}_acados_sim_solve()! Exiting.\n");
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
|
||||
{
|
||||
// free memory
|
||||
sim_solver_destroy(capsule->acados_sim_solver);
|
||||
sim_in_destroy(capsule->acados_sim_in);
|
||||
sim_out_destroy(capsule->acados_sim_out);
|
||||
sim_opts_destroy(capsule->acados_sim_opts);
|
||||
sim_dims_destroy(capsule->acados_sim_dims);
|
||||
sim_config_destroy(capsule->acados_sim_config);
|
||||
|
||||
// free external function
|
||||
{%- if solver_options.integrator_type == "IRK" %}
|
||||
external_function_param_casadi_free(capsule->sim_impl_dae_fun);
|
||||
external_function_param_casadi_free(capsule->sim_impl_dae_fun_jac_x_xdot_z);
|
||||
external_function_param_casadi_free(capsule->sim_impl_dae_jac_x_xdot_u_z);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
external_function_param_casadi_free(capsule->sim_impl_dae_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "ERK" %}
|
||||
external_function_param_casadi_free(capsule->sim_forw_vde_casadi);
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_fun_casadi);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
external_function_param_casadi_free(capsule->sim_expl_ode_hess);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_fun_jac_y);
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_phi_jac_y_uhat);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
external_function_param_casadi_free(capsule->sim_gnsf_get_matrices_fun);
|
||||
{% endif %}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
|
||||
{
|
||||
int status = 0;
|
||||
int casadi_np = {{ model.name | upper }}_NP;
|
||||
|
||||
if (casadi_np != np) {
|
||||
printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
|
||||
" External function has %i parameters. Exiting.\n", np, casadi_np);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
{%- if solver_options.integrator_type == "ERK" %}
|
||||
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
|
||||
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "IRK" %}
|
||||
capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
|
||||
capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
|
||||
capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
|
||||
{%- if hessian_approx == "EXACT" %}
|
||||
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
|
||||
{%- endif %}
|
||||
{%- elif solver_options.integrator_type == "GNSF" %}
|
||||
{% if model.gnsf.purely_linear != 1 %}
|
||||
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
|
||||
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
|
||||
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
|
||||
{% if model.gnsf.nontrivial_f_LO == 1 %}
|
||||
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
|
||||
{%- endif %}
|
||||
{%- endif %}
|
||||
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
|
||||
{% endif %}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
/* getters pointers to C objects*/
|
||||
sim_config * {{ model.name }}_acados_get_sim_config(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_config;
|
||||
};
|
||||
|
||||
sim_in * {{ model.name }}_acados_get_sim_in(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_in;
|
||||
};
|
||||
|
||||
sim_out * {{ model.name }}_acados_get_sim_out(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_out;
|
||||
};
|
||||
|
||||
void * {{ model.name }}_acados_get_sim_dims(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_dims;
|
||||
};
|
||||
|
||||
sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_opts;
|
||||
};
|
||||
|
||||
sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule)
|
||||
{
|
||||
return capsule->acados_sim_solver;
|
||||
};
|
||||
|
||||
+233
@@ -0,0 +1,233 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_sim_solver_sfunction_{{ model.name }}
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
#define SAMPLINGTIME {{ solver_options.Tsim }}
|
||||
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);
|
||||
|
||||
{# compute number of input ports #}
|
||||
{%- set n_inputs = 1 %} {# x0 #}
|
||||
{%- if dims.nu > 0 %} {# u0 -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif %}
|
||||
{%- if dims.np > 0 %} {# parameters #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif %}
|
||||
|
||||
// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
if ( !ssSetNumOutputPorts(S, 1) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
{%- set i_input = 0 %}
|
||||
// x0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx }});
|
||||
|
||||
{%- if dims.nu > 0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// u0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
// specify dimension information for the output ports
|
||||
ssSetOutputPortVectorDimension(S, 0, {{ dims.nx }} ); // xnext
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
{%- for i in range(end=n_inputs) %}
|
||||
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
|
||||
{%- endfor %}
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
|
||||
{{ model.name }}_acados_sim_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
sim_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
|
||||
// sim_opts * {{ model.name }}_acados_get_sim_opts(capsule);
|
||||
// sim_solver * {{ model.name }}_acados_get_sim_solver(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
{% set input_sizes = [dims.nx, dims.nu, dims.np] %}
|
||||
|
||||
// local buffer
|
||||
{%- set buffer_size = input_sizes | sort | last %}
|
||||
real_t buffer[{{ buffer_size }}];
|
||||
|
||||
|
||||
/* go through inputs */
|
||||
{%- set i_input = 0 %}
|
||||
// initial condition
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nx }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", buffer);
|
||||
|
||||
|
||||
// ssPrintf("\nin acados sim:\n");
|
||||
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x0[%d] = %f\n", i, buffer[i]);
|
||||
// ssPrintf("\n");
|
||||
|
||||
{% if dims.nu > 0 %}
|
||||
// control input - u
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nu }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "u", buffer);
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{% if dims.np > 0 %}
|
||||
// parameters
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.np }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
// update value of parameters
|
||||
{{ model.name }}_acados_sim_update_params(capsule, buffer, {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
|
||||
/* call solver */
|
||||
int acados_status = {{ model.name }}_acados_sim_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
real_t *out_x = ssGetOutputPortRealSignal(S, 0);
|
||||
|
||||
// get simulated state
|
||||
sim_out_get(acados_sim_config, acados_sim_dims, acados_sim_out,
|
||||
"xn", (void *) out_x);
|
||||
|
||||
// ssPrintf("\nacados sim solve: returned %d\n", acados_status);
|
||||
// for (int i = 0; i < {{ dims.nx }}; i++) ssPrintf("x_sim[%d] = %f\n", i, out_x[i]);
|
||||
// ssPrintf("\n");
|
||||
|
||||
}
|
||||
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
sim_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
{{ model.name }}_acados_sim_free(capsule);
|
||||
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,782 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
#define S_FUNCTION_NAME acados_solver_sfunction_{{ model.name }}
|
||||
#define S_FUNCTION_LEVEL 2
|
||||
|
||||
#define MDL_START
|
||||
|
||||
// acados
|
||||
// #include "acados/utils/print.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
|
||||
// example specific
|
||||
#include "{{ model.name }}_model/{{ model.name }}_model.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
#include "simstruc.h"
|
||||
|
||||
{% if simulink_opts.samplingtime == "t0" -%}
|
||||
#define SAMPLINGTIME {{ solver_options.time_steps[0] }}
|
||||
{%- elif simulink_opts.samplingtime == "-1" -%}
|
||||
#define SAMPLINGTIME -1
|
||||
{%- else -%}
|
||||
{{ throw(message = "simulink_opts.samplingtime must be '-1' or 't0', got val") }}
|
||||
{%- endif %}
|
||||
|
||||
static void mdlInitializeSizes (SimStruct *S)
|
||||
{
|
||||
// specify the number of continuous and discrete states
|
||||
ssSetNumContStates(S, 0);
|
||||
ssSetNumDiscStates(S, 0);
|
||||
|
||||
{%- for key, val in simulink_opts.inputs -%}
|
||||
{%- if val != 0 and val != 1 -%}
|
||||
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
|
||||
{%- endif -%}
|
||||
{%- endfor -%}
|
||||
|
||||
{#- compute number of input ports #}
|
||||
{%- set n_inputs = 0 -%}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 -%} {#- y_ref_0 -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref -%} {#- y_ref -#}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e -%} {#- y_ref_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- for key, val in simulink_opts.inputs -%}
|
||||
{%- if val != 0 and val != 1 -%}
|
||||
{{ throw(message = "simulink_opts.inputs must be 0 or 1, got val") }}
|
||||
{%- endif -%}
|
||||
{%- endfor -%}
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set n_inputs = n_inputs + 1 %}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set n_inputs = n_inputs + 1 %}
|
||||
{%- endif -%}
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
|
||||
{%- set n_inputs = n_inputs + 1 -%}
|
||||
{%- endif -%}
|
||||
|
||||
// specify the number of input ports
|
||||
if ( !ssSetNumInputPorts(S, {{ n_inputs }}) )
|
||||
return;
|
||||
|
||||
// specify the number of output ports
|
||||
{%- set_global n_outputs = 0 %}
|
||||
{%- for key, val in simulink_opts.outputs %}
|
||||
{%- if val == 1 %}
|
||||
{%- set_global n_outputs = n_outputs + val %}
|
||||
{%- elif val != 0 %}
|
||||
{{ throw(message = "simulink_opts.outputs must be 0 or 1, got val") }}
|
||||
{%- endif %}
|
||||
{%- endfor %}
|
||||
if ( !ssSetNumOutputPorts(S, {{ n_outputs }}) )
|
||||
return;
|
||||
|
||||
// specify dimension information for the input ports
|
||||
{%- set i_input = -1 %}{# note here i_input is 0-based #}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// parameters
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, ({{ dims.N }}+1) * {{ dims.np }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.ny }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// y_ref_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ (dims.N-1) * dims.nbx }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbx_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubx_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nbx_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lbu
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ubu
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.N*dims.nbu }});
|
||||
{%- endif -%}
|
||||
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lg
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// ug
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ng }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// lh
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// uh
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nh }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W_0
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_0 * dims.ny_0 }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny * dims.ny }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// cost_W_e
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.ny_e * dims.ny_e }});
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver -%} {#- reset_solver #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// reset_solver
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, 1);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init -%} {#- x_init #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// x_init
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nx * (dims.N+1) }});
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init -%} {#- u_init #}
|
||||
{%- set i_input = i_input + 1 %}
|
||||
// u_init
|
||||
ssSetInputPortVectorDimension(S, {{ i_input }}, {{ dims.nu * (dims.N) }});
|
||||
{%- endif -%}
|
||||
|
||||
/* specify dimension information for the OUTPUT ports */
|
||||
{%- set i_output = -1 %}{# note here i_output is 0-based #}
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nu * dims.N }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.xtraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx * (dims.N+1) }} );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, {{ dims.nx }} ); // state at shooting node 1
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
ssSetOutputPortVectorDimension(S, {{ i_output }}, 1 );
|
||||
{%- endif %}
|
||||
|
||||
// specify the direct feedthrough status
|
||||
// should be set to 1 for all inputs used in mdlOutputs
|
||||
{%- for i in range(end=n_inputs) %}
|
||||
ssSetInputPortDirectFeedThrough(S, {{ i }}, 1);
|
||||
{%- endfor %}
|
||||
|
||||
// one sample time
|
||||
ssSetNumSampleTimes(S, 1);
|
||||
}
|
||||
|
||||
|
||||
#if defined(MATLAB_MEX_FILE)
|
||||
|
||||
#define MDL_SET_INPUT_PORT_DIMENSION_INFO
|
||||
#define MDL_SET_OUTPUT_PORT_DIMENSION_INFO
|
||||
|
||||
static void mdlSetInputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetInputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
static void mdlSetOutputPortDimensionInfo(SimStruct *S, int_T port, const DimsInfo_T *dimsInfo)
|
||||
{
|
||||
if ( !ssSetOutputPortDimensionInfo(S, port, dimsInfo) )
|
||||
return;
|
||||
}
|
||||
|
||||
#endif /* MATLAB_MEX_FILE */
|
||||
|
||||
|
||||
static void mdlInitializeSampleTimes(SimStruct *S)
|
||||
{
|
||||
ssSetSampleTime(S, 0, SAMPLINGTIME);
|
||||
ssSetOffsetTime(S, 0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
static void mdlStart(SimStruct *S)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
|
||||
{{ model.name }}_acados_create(capsule);
|
||||
|
||||
ssSetUserData(S, (void*)capsule);
|
||||
}
|
||||
|
||||
|
||||
static void mdlOutputs(SimStruct *S, int_T tid)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(capsule);
|
||||
|
||||
InputRealPtrsType in_sign;
|
||||
|
||||
{%- set buffer_sizes = [dims.nbx_0, dims.np, dims.nbx, dims.nbu, dims.ng, dims.nh, dims.nx] -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %} {# y_ref_0 #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %} {# y_ref #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %} {# y_ref_e #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e)) %}
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_0 * dims.ny_0)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny * dims.ny)) %}
|
||||
{%- endif %}
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
{%- set buffer_sizes = buffer_sizes | concat(with=(dims.ny_e * dims.ny_e)) %}
|
||||
{%- endif %}
|
||||
|
||||
// local buffer
|
||||
{%- set buffer_size = buffer_sizes | sort | last %}
|
||||
real_t buffer[{{ buffer_size }}];
|
||||
|
||||
/* go through inputs */
|
||||
{%- set i_input = -1 %}
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.lbx_0 -%} {#- lbx_0 #}
|
||||
// lbx_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx_0 > 0 and simulink_opts.inputs.ubx_0 -%} {#- ubx_0 #}
|
||||
// ubx_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.nbx_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.np > 0 and simulink_opts.inputs.parameter_traj -%} {#- parameter_traj #}
|
||||
// parameters - stage-variant !!!
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
// update value of parameters
|
||||
for (int ii = 0; ii <= {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.np }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{dims.np}}+jj]);
|
||||
{{ model.name }}_acados_update_params(capsule, ii, buffer, {{ dims.np }});
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny_0 > 0 and simulink_opts.inputs.y_ref_0 %}
|
||||
// y_ref_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ny_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", (void *) buffer);
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny > 0 and dims.N > 1 and simulink_opts.inputs.y_ref %}
|
||||
// y_ref - for stages 1 to N-1
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int ii = 1; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.ny }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.ny }}+jj]);
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "yref", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{% if dims.ny_e > 0 and dims.N > 0 and simulink_opts.inputs.y_ref_e %}
|
||||
// y_ref_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ny_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "yref", (void *) buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.lbx -%} {#- lbx #}
|
||||
// lbx
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 1; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbx", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
{%- if dims.nbx > 0 and dims.N > 1 and simulink_opts.inputs.ubx -%} {#- ubx #}
|
||||
// ubx
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 1; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii-1)*{{ dims.nbx }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubx", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.lbx_e -%} {#- lbx_e #}
|
||||
// lbx_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nbx_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "lbx", buffer);
|
||||
{%- endif %}
|
||||
{%- if dims.nbx_e > 0 and dims.N > 0 and simulink_opts.inputs.ubx_e -%} {#- ubx_e #}
|
||||
// ubx_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nbx_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "ubx", buffer);
|
||||
{%- endif %}
|
||||
|
||||
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.lbu -%} {#- lbu #}
|
||||
// lbu
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lbu", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
{%- if dims.nbu > 0 and dims.N > 0 and simulink_opts.inputs.ubu -%} {#- ubu #}
|
||||
// ubu
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nbu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[ii*{{ dims.nbu }}+jj]);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ubu", (void *) buffer);
|
||||
}
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.lg -%} {#- lg #}
|
||||
// lg
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ng }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lg", buffer);
|
||||
{%- endif -%}
|
||||
{%- if dims.ng > 0 and simulink_opts.inputs.ug -%} {#- ug #}
|
||||
// ug
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.ng }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "ug", buffer);
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.lh -%} {#- lh #}
|
||||
// lh
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nh }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "lh", buffer);
|
||||
{%- endif -%}
|
||||
{%- if dims.nh > 0 and simulink_opts.inputs.uh -%} {#- uh #}
|
||||
// uh
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
|
||||
for (int i = 0; i < {{ dims.nh }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, ii, "uh", buffer);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if dims.ny_0 > 0 and simulink_opts.inputs.cost_W_0 %} {#- cost_W_0 #}
|
||||
// cost_W_0
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny_0 * dims.ny_0 }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny > 0 and simulink_opts.inputs.cost_W %} {#- cost_W #}
|
||||
// cost_W
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny * dims.ny }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
for (int ii = 1; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, ii, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.ny_e > 0 and simulink_opts.inputs.cost_W_e %} {#- cost_W_e #}
|
||||
// cost_W_e
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int i = 0; i < {{ dims.ny_e * dims.ny_e }}; i++)
|
||||
buffer[i] = (double)(*in_sign[i]);
|
||||
|
||||
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, {{ dims.N }}, "W", buffer);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.reset_solver %} {#- reset_solver #}
|
||||
// reset_solver
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
double reset = (double)(*in_sign[0]);
|
||||
if (reset)
|
||||
{
|
||||
{{ model.name }}_acados_reset(capsule);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.x_init %} {#- x_init #}
|
||||
// x_init
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nx }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nx }}+jj]);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "x", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.inputs.u_init %} {#- u_init #}
|
||||
// u_init
|
||||
{%- set i_input = i_input + 1 %}
|
||||
in_sign = ssGetInputPortRealSignalPtrs(S, {{ i_input }});
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
{
|
||||
for (int jj = 0; jj < {{ dims.nu }}; jj++)
|
||||
buffer[jj] = (double)(*in_sign[(ii)*{{ dims.nu }}+jj]);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, ii, "u", (void *) buffer);
|
||||
}
|
||||
{%- endif %}
|
||||
|
||||
/* call solver */
|
||||
int rti_phase = 0;
|
||||
ocp_nlp_solver_opts_set(nlp_config, capsule->nlp_opts, "rti_phase", &rti_phase);
|
||||
int acados_status = {{ model.name }}_acados_solve(capsule);
|
||||
|
||||
|
||||
/* set outputs */
|
||||
// assign pointers to output signals
|
||||
real_t *out_u0, *out_utraj, *out_xtraj, *out_status, *out_sqp_iter, *out_KKT_res, *out_x1, *out_cpu_time, *out_cpu_time_sim, *out_cpu_time_qp, *out_cpu_time_lin;
|
||||
int tmp_int;
|
||||
|
||||
{%- set i_output = -1 -%}{# note here i_output is 0-based #}
|
||||
{%- if dims.nu > 0 and simulink_opts.outputs.u0 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_u0 = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", (void *) out_u0);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.utraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_utraj = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
for (int ii = 0; ii < {{ dims.N }}; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
|
||||
"u", (void *) (out_utraj + ii * {{ dims.nu }}));
|
||||
{%- endif %}
|
||||
|
||||
{% if simulink_opts.outputs.xtraj == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
|
||||
out_xtraj = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
for (int ii = 0; ii < {{ dims.N + 1 }}; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii,
|
||||
"x", (void *) (out_xtraj + ii * {{ dims.nx }}));
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.solver_status == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_status = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
*out_status = (real_t) acados_status;
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.KKT_residual == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_KKT_res = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
*out_KKT_res = (real_t) nlp_out->inf_norm_res;
|
||||
{%- endif %}
|
||||
|
||||
{%- if dims.N > 0 and simulink_opts.outputs.x1 == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_x1 = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 1, "x", (void *) out_x1);
|
||||
{%- endif %}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
// get solution time
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_tot", (void *) out_cpu_time);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_sim == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_sim = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_sim", (void *) out_cpu_time_sim);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_qp == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_qp = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_qp", (void *) out_cpu_time_qp);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.CPU_time_lin == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_cpu_time_lin = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "time_lin", (void *) out_cpu_time_lin);
|
||||
{%- endif -%}
|
||||
|
||||
{%- if simulink_opts.outputs.sqp_iter == 1 %}
|
||||
{%- set i_output = i_output + 1 %}
|
||||
out_sqp_iter = ssGetOutputPortRealSignal(S, {{ i_output }});
|
||||
// get sqp iter
|
||||
ocp_nlp_get(nlp_config, capsule->nlp_solver, "sqp_iter", (void *) &tmp_int);
|
||||
*out_sqp_iter = (real_t) tmp_int;
|
||||
{%- endif %}
|
||||
|
||||
}
|
||||
|
||||
static void mdlTerminate(SimStruct *S)
|
||||
{
|
||||
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
|
||||
|
||||
{{ model.name }}_acados_free(capsule);
|
||||
{{ model.name }}_acados_free_capsule(capsule);
|
||||
}
|
||||
|
||||
|
||||
#ifdef MATLAB_MEX_FILE
|
||||
#include "simulink.c"
|
||||
#else
|
||||
#include "cg_sfun.h"
|
||||
#endif
|
||||
@@ -0,0 +1,216 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_c/external_function_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
|
||||
#define NX {{ model.name | upper }}_NX
|
||||
#define NZ {{ model.name | upper }}_NZ
|
||||
#define NU {{ model.name | upper }}_NU
|
||||
#define NP {{ model.name | upper }}_NP
|
||||
#define NBX {{ model.name | upper }}_NBX
|
||||
#define NBX0 {{ model.name | upper }}_NBX0
|
||||
#define NBU {{ model.name | upper }}_NBU
|
||||
#define NSBX {{ model.name | upper }}_NSBX
|
||||
#define NSBU {{ model.name | upper }}_NSBU
|
||||
#define NSH {{ model.name | upper }}_NSH
|
||||
#define NSG {{ model.name | upper }}_NSG
|
||||
#define NSPHI {{ model.name | upper }}_NSPHI
|
||||
#define NSHN {{ model.name | upper }}_NSHN
|
||||
#define NSGN {{ model.name | upper }}_NSGN
|
||||
#define NSPHIN {{ model.name | upper }}_NSPHIN
|
||||
#define NSBXN {{ model.name | upper }}_NSBXN
|
||||
#define NS {{ model.name | upper }}_NS
|
||||
#define NSN {{ model.name | upper }}_NSN
|
||||
#define NG {{ model.name | upper }}_NG
|
||||
#define NBXN {{ model.name | upper }}_NBXN
|
||||
#define NGN {{ model.name | upper }}_NGN
|
||||
#define NY0 {{ model.name | upper }}_NY0
|
||||
#define NY {{ model.name | upper }}_NY
|
||||
#define NYN {{ model.name | upper }}_NYN
|
||||
#define NH {{ model.name | upper }}_NH
|
||||
#define NPHI {{ model.name | upper }}_NPHI
|
||||
#define NHN {{ model.name | upper }}_NHN
|
||||
#define NPHIN {{ model.name | upper }}_NPHIN
|
||||
#define NR {{ model.name | upper }}_NR
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
|
||||
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
|
||||
// there is an opportunity to change the number of shooting intervals in C without new code generation
|
||||
int N = {{ model.name | upper }}_N;
|
||||
// allocate the array and fill it accordingly
|
||||
double* new_time_steps = NULL;
|
||||
int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(acados_ocp_capsule);
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(acados_ocp_capsule);
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(acados_ocp_capsule);
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out(acados_ocp_capsule);
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver(acados_ocp_capsule);
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
|
||||
|
||||
// initial condition
|
||||
int idxbx0[NBX0];
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
double lbx0[NBX0];
|
||||
double ubx0[NBX0];
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
|
||||
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[NX];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_init[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
// set parameters
|
||||
double p[NP];
|
||||
{%- for item in parameter_values %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endfor %}
|
||||
|
||||
for (int ii = 0; ii <= N; ii++)
|
||||
{
|
||||
{{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP);
|
||||
}
|
||||
{% endif %}{# if np > 0 #}
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 1;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[NX * (N+1)];
|
||||
double utraj[NU * N];
|
||||
|
||||
|
||||
// solve ocp in loop
|
||||
int rti_phase = 0;
|
||||
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize solution
|
||||
for (int i = 0; i < N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, N, "x", x_init);
|
||||
ocp_nlp_solver_opts_set(nlp_config, nlp_opts, "rti_phase", &rti_phase);
|
||||
status = {{ model.name }}_acados_solve(acados_ocp_capsule);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
|
||||
|
||||
printf("\n--- xtraj ---\n");
|
||||
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
|
||||
printf("\n--- utraj ---\n");
|
||||
d_print_exp_tran_mat( NU, N, utraj, NU );
|
||||
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
|
||||
|
||||
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
{
|
||||
printf("{{ model.name }}_acados_solve(): SUCCESS!\n");
|
||||
}
|
||||
else
|
||||
{
|
||||
printf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
{{ model.name }}_acados_print_stats(acados_ocp_capsule);
|
||||
|
||||
printf("\nSolver info:\n");
|
||||
printf(" SQP iterations %2d\n minimum time for %d solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, NTIMINGS, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_free(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_free() returned status %d. \n", status);
|
||||
}
|
||||
// free solver capsule
|
||||
status = {{ model.name }}_acados_free_capsule(acados_ocp_capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_free_capsule() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
@@ -0,0 +1,184 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/ocp_nlp_interface.h"
|
||||
#include "acados_solver_{{ model.name }}.h"
|
||||
// mex
|
||||
#include "mex.h"
|
||||
|
||||
/* auxilary mex */
|
||||
// prints a matrix in column-major format (exponential notation)
|
||||
void MEX_print_exp_mat(int m, int n, double *A, int lda)
|
||||
{
|
||||
for (int i=0; i<m; i++)
|
||||
{
|
||||
for (int j=0; j<n; j++)
|
||||
{
|
||||
mexPrintf("%e\t", A[i+lda*j]);
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
|
||||
// prints the transposed of a matrix in column-major format (exponential notation)
|
||||
void MEX_print_exp_tran_mat(int row, int col, double *A, int lda)
|
||||
{
|
||||
for (int j=0; j<col; j++)
|
||||
{
|
||||
for (int i=0; i<row; i++)
|
||||
{
|
||||
mexPrintf("%e\t", A[i+lda*j]);
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
mexPrintf("\n");
|
||||
}
|
||||
|
||||
|
||||
void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
|
||||
{
|
||||
|
||||
int status = 0;
|
||||
status = {{ model.name }}_acados_create();
|
||||
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
|
||||
// get pointers to nlp solver related objects
|
||||
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config();
|
||||
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims();
|
||||
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in();
|
||||
ocp_nlp_out *nlp_out = {{ model.name }}_acados_get_nlp_out();
|
||||
ocp_nlp_solver *nlp_solver = {{ model.name }}_acados_get_nlp_solver();
|
||||
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts();
|
||||
|
||||
// initial condition
|
||||
int idxbx0[{{ dims.nbx_0 }}];
|
||||
{% for i in range(end=dims.nbx_0) %}
|
||||
idxbx0[{{ i }}] = {{ constraints.idxbx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
double lbx0[{{ dims.nbx_0 }}];
|
||||
double ubx0[{{ dims.nbx_0 }}];
|
||||
{% for i in range(end=dims.nbx_0) %}
|
||||
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
|
||||
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
|
||||
{%- endfor %}
|
||||
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbx", idxbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", lbx0);
|
||||
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
|
||||
|
||||
// initialization for state values
|
||||
double x_init[{{ dims.nx }}];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_init[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// initial value for control input
|
||||
double u0[{{ dims.nu }}];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
// prepare evaluation
|
||||
int NTIMINGS = 10;
|
||||
double min_time = 1e12;
|
||||
double kkt_norm_inf;
|
||||
double elapsed_time;
|
||||
int sqp_iter;
|
||||
|
||||
double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
|
||||
double utraj[{{ dims.nu }} * ({{ dims.N }})];
|
||||
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < NTIMINGS; ii++)
|
||||
{
|
||||
// initialize primal solution
|
||||
for (int i = 0; i <= nlp_dims->N; i++)
|
||||
{
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "x", x_init);
|
||||
ocp_nlp_out_set(nlp_config, nlp_dims, nlp_out, i, "u", u0);
|
||||
}
|
||||
status = {{ model.name }}_acados_solve();
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "time_tot", &elapsed_time);
|
||||
min_time = MIN(elapsed_time, min_time);
|
||||
}
|
||||
|
||||
/* print solution and statistics */
|
||||
for (int ii = 0; ii <= nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
|
||||
for (int ii = 0; ii < nlp_dims->N; ii++)
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
|
||||
|
||||
mexPrintf("\n--- xtraj ---\n");
|
||||
MEX_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
|
||||
mexPrintf("\n--- utraj ---\n");
|
||||
MEX_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
|
||||
|
||||
mexPrintf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
|
||||
|
||||
if (status == ACADOS_SUCCESS)
|
||||
mexPrintf("{{ model.name }}_acados_solve(): SUCCESS!\n");
|
||||
else
|
||||
mexPrintf("{{ model.name }}_acados_solve() failed with status %d.\n", status);
|
||||
|
||||
// get solution
|
||||
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "kkt_norm_inf", &kkt_norm_inf);
|
||||
ocp_nlp_get(nlp_config, nlp_solver, "sqp_iter", &sqp_iter);
|
||||
|
||||
mexPrintf("\nSolver info:\n");
|
||||
mexPrintf(" SQP iterations %2d\n minimum time for 1 solve %f [ms]\n KKT %e\n",
|
||||
sqp_iter, min_time*1000, kkt_norm_inf);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_free();
|
||||
if (status)
|
||||
{
|
||||
mexPrintf("{{ model.name }}_acados_free() returned status %d.\n", status);
|
||||
}
|
||||
|
||||
return;
|
||||
}
|
||||
@@ -0,0 +1,135 @@
|
||||
/*
|
||||
* Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
|
||||
* Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
|
||||
* Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
|
||||
* Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
|
||||
*
|
||||
* 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.;
|
||||
*/
|
||||
|
||||
|
||||
// standard
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
// acados
|
||||
#include "acados/utils/print.h"
|
||||
#include "acados/utils/math.h"
|
||||
#include "acados_c/sim_interface.h"
|
||||
#include "acados_sim_solver_{{ model.name }}.h"
|
||||
|
||||
#define NX {{ model.name | upper }}_NX
|
||||
#define NZ {{ model.name | upper }}_NZ
|
||||
#define NU {{ model.name | upper }}_NU
|
||||
#define NP {{ model.name | upper }}_NP
|
||||
|
||||
|
||||
int main()
|
||||
{
|
||||
int status = 0;
|
||||
sim_solver_capsule *capsule = {{ model.name }}_acados_sim_solver_create_capsule();
|
||||
status = {{ model.name }}_acados_sim_create(capsule);
|
||||
|
||||
if (status)
|
||||
{
|
||||
printf("acados_create() returned status %d. Exiting.\n", status);
|
||||
exit(1);
|
||||
}
|
||||
|
||||
sim_config *acados_sim_config = {{ model.name }}_acados_get_sim_config(capsule);
|
||||
sim_in *acados_sim_in = {{ model.name }}_acados_get_sim_in(capsule);
|
||||
sim_out *acados_sim_out = {{ model.name }}_acados_get_sim_out(capsule);
|
||||
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
|
||||
|
||||
// initial condition
|
||||
double x_current[NX];
|
||||
{%- for i in range(end=dims.nx) %}
|
||||
x_current[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
{% if constraints.lbx_0 %}
|
||||
{%- for i in range(end=dims.nbx_0) %}
|
||||
x_current[{{ constraints.idxbx_0[i] }}] = {{ constraints.lbx_0[i] }};
|
||||
{%- endfor %}
|
||||
{% if dims.nbx_0 != dims.nx %}
|
||||
printf("main_sim: NOTE: initial state not fully defined via lbx_0, using 0.0 for indices that are not in idxbx_0.");
|
||||
{%- endif %}
|
||||
{% else %}
|
||||
printf("main_sim: initial state not defined, should be in lbx_0, using zero vector.");
|
||||
{%- endif %}
|
||||
|
||||
|
||||
// initial value for control input
|
||||
double u0[NU];
|
||||
{%- for i in range(end=dims.nu) %}
|
||||
u0[{{ i }}] = 0.0;
|
||||
{%- endfor %}
|
||||
|
||||
{%- if dims.np > 0 %}
|
||||
// set parameters
|
||||
double p[NP];
|
||||
{%- for item in parameter_values %}
|
||||
p[{{ loop.index0 }}] = {{ item }};
|
||||
{%- endfor %}
|
||||
|
||||
{{ model.name }}_acados_sim_update_params(capsule, p, NP);
|
||||
{% endif %}{# if np > 0 #}
|
||||
|
||||
int n_sim_steps = 3;
|
||||
// solve ocp in loop
|
||||
for (int ii = 0; ii < n_sim_steps; ii++)
|
||||
{
|
||||
sim_in_set(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_in, "x", x_current);
|
||||
status = {{ model.name }}_acados_sim_solve(capsule);
|
||||
|
||||
if (status != ACADOS_SUCCESS)
|
||||
{
|
||||
printf("acados_solve() failed with status %d.\n", status);
|
||||
}
|
||||
|
||||
sim_out_get(acados_sim_config, acados_sim_dims,
|
||||
acados_sim_out, "x", x_current);
|
||||
|
||||
printf("\nx_current, %d\n", ii);
|
||||
for (int jj = 0; jj < NX; jj++)
|
||||
{
|
||||
printf("%e\n", x_current[jj]);
|
||||
}
|
||||
}
|
||||
|
||||
printf("\nPerformed %d simulation steps with acados integrator successfully.\n\n", n_sim_steps);
|
||||
|
||||
// free solver
|
||||
status = {{ model.name }}_acados_sim_free(capsule);
|
||||
if (status) {
|
||||
printf("{{ model.name }}_acados_sim_free() returned status %d. \n", status);
|
||||
}
|
||||
|
||||
{{ model.name }}_acados_sim_solver_free_capsule(capsule);
|
||||
|
||||
return status;
|
||||
}
|
||||
Vendored
-1
@@ -1 +0,0 @@
|
||||
/data/openpilot/third_party/acados/larch64/lib
|
||||
Binary file not shown.
Binary file not shown.
Vendored
+218
@@ -0,0 +1,218 @@
|
||||
//
|
||||
// C++ standalone verion of fastcluster by Daniel Müllner
|
||||
//
|
||||
// Copyright: Christoph Dalitz, 2018
|
||||
// Daniel Müllner, 2011
|
||||
// License: BSD style license
|
||||
// (see the file LICENSE for details)
|
||||
//
|
||||
|
||||
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
|
||||
|
||||
extern "C" {
|
||||
#include "fastcluster.h"
|
||||
}
|
||||
|
||||
// Code by Daniel Müllner
|
||||
// workaround to make it usable as a standalone version (without R)
|
||||
bool fc_isnan(double x) { return false; }
|
||||
#include "fastcluster_dm.cpp"
|
||||
#include "fastcluster_R_dm.cpp"
|
||||
|
||||
extern "C" {
|
||||
//
|
||||
// Assigns cluster labels (0, ..., nclust-1) to the n points such
|
||||
// that the cluster result is split into nclust clusters.
|
||||
//
|
||||
// Input arguments:
|
||||
// n = number of observables
|
||||
// merge = clustering result in R format
|
||||
// nclust = number of clusters
|
||||
// Output arguments:
|
||||
// labels = allocated integer array of size n for result
|
||||
//
|
||||
void cutree_k(int n, const int* merge, int nclust, int* labels) {
|
||||
|
||||
int k,m1,m2,j,l;
|
||||
|
||||
if (nclust > n || nclust < 2) {
|
||||
for (j=0; j<n; j++) labels[j] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
// assign to each observable the number of its last merge step
|
||||
// beware: indices of observables in merge start at 1 (R convention)
|
||||
std::vector<int> last_merge(n, 0);
|
||||
for (k=1; k<=(n-nclust); k++) {
|
||||
// (m1,m2) = merge[k,]
|
||||
m1 = merge[k-1];
|
||||
m2 = merge[n-1+k-1];
|
||||
if (m1 < 0 && m2 < 0) { // both single observables
|
||||
last_merge[-m1-1] = last_merge[-m2-1] = k;
|
||||
}
|
||||
else if (m1 < 0 || m2 < 0) { // one is a cluster
|
||||
if(m1 < 0) { j = -m1; m1 = m2; } else j = -m2;
|
||||
// merging single observable and cluster
|
||||
for(l = 0; l < n; l++)
|
||||
if (last_merge[l] == m1)
|
||||
last_merge[l] = k;
|
||||
last_merge[j-1] = k;
|
||||
}
|
||||
else { // both cluster
|
||||
for(l=0; l < n; l++) {
|
||||
if( last_merge[l] == m1 || last_merge[l] == m2 )
|
||||
last_merge[l] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// assign cluster labels
|
||||
int label = 0;
|
||||
std::vector<int> z(n,-1);
|
||||
for (j=0; j<n; j++) {
|
||||
if (last_merge[j] == 0) { // still singleton
|
||||
labels[j] = label++;
|
||||
} else {
|
||||
if (z[last_merge[j]] < 0) {
|
||||
z[last_merge[j]] = label++;
|
||||
}
|
||||
labels[j] = z[last_merge[j]];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
//
|
||||
// Assigns cluster labels (0, ..., nclust-1) to the n points such
|
||||
// that the hierarchical clustering is stopped when cluster distance >= cdist
|
||||
//
|
||||
// Input arguments:
|
||||
// n = number of observables
|
||||
// merge = clustering result in R format
|
||||
// height = cluster distance at each merge step
|
||||
// cdist = cutoff cluster distance
|
||||
// Output arguments:
|
||||
// labels = allocated integer array of size n for result
|
||||
//
|
||||
void cutree_cdist(int n, const int* merge, double* height, double cdist, int* labels) {
|
||||
|
||||
int k;
|
||||
|
||||
for (k=0; k<(n-1); k++) {
|
||||
if (height[k] >= cdist) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
cutree_k(n, merge, n-k, labels);
|
||||
}
|
||||
|
||||
|
||||
//
|
||||
// Hierarchical clustering with one of Daniel Muellner's fast algorithms
|
||||
//
|
||||
// Input arguments:
|
||||
// n = number of observables
|
||||
// distmat = condensed distance matrix, i.e. an n*(n-1)/2 array representing
|
||||
// the upper triangle (without diagonal elements) of the distance
|
||||
// matrix, e.g. for n=4:
|
||||
// d00 d01 d02 d03
|
||||
// d10 d11 d12 d13 -> d01 d02 d03 d12 d13 d23
|
||||
// d20 d21 d22 d23
|
||||
// d30 d31 d32 d33
|
||||
// method = cluster metric (see enum method_code)
|
||||
// Output arguments:
|
||||
// merge = allocated (n-1)x2 matrix (2*(n-1) array) for storing result.
|
||||
// Result follows R hclust convention:
|
||||
// - observabe indices start with one
|
||||
// - merge[i][] contains the merged nodes in step i
|
||||
// - merge[i][j] is negative when the node is an atom
|
||||
// height = allocated (n-1) array with distances at each merge step
|
||||
// Return code:
|
||||
// 0 = ok
|
||||
// 1 = invalid method
|
||||
//
|
||||
int hclust_fast(int n, double* distmat, int method, int* merge, double* height) {
|
||||
|
||||
// call appropriate culstering function
|
||||
cluster_result Z2(n-1);
|
||||
if (method == HCLUST_METHOD_SINGLE) {
|
||||
// single link
|
||||
MST_linkage_core(n, distmat, Z2);
|
||||
}
|
||||
else if (method == HCLUST_METHOD_COMPLETE) {
|
||||
// complete link
|
||||
NN_chain_core<METHOD_METR_COMPLETE, t_float>(n, distmat, NULL, Z2);
|
||||
}
|
||||
else if (method == HCLUST_METHOD_AVERAGE) {
|
||||
// best average distance
|
||||
double* members = new double[n];
|
||||
for (int i=0; i<n; i++) members[i] = 1;
|
||||
NN_chain_core<METHOD_METR_AVERAGE, t_float>(n, distmat, members, Z2);
|
||||
delete[] members;
|
||||
}
|
||||
else if (method == HCLUST_METHOD_MEDIAN) {
|
||||
// best median distance (beware: O(n^3))
|
||||
generic_linkage<METHOD_METR_MEDIAN, t_float>(n, distmat, NULL, Z2);
|
||||
}
|
||||
else if (method == HCLUST_METHOD_CENTROID) {
|
||||
// best centroid distance (beware: O(n^3))
|
||||
double* members = new double[n];
|
||||
for (int i=0; i<n; i++) members[i] = 1;
|
||||
generic_linkage<METHOD_METR_CENTROID, t_float>(n, distmat, members, Z2);
|
||||
delete[] members;
|
||||
}
|
||||
else {
|
||||
return 1;
|
||||
}
|
||||
|
||||
int* order = new int[n];
|
||||
if (method == HCLUST_METHOD_MEDIAN || method == HCLUST_METHOD_CENTROID) {
|
||||
generate_R_dendrogram<true>(merge, height, order, Z2, n);
|
||||
} else {
|
||||
generate_R_dendrogram<false>(merge, height, order, Z2, n);
|
||||
}
|
||||
delete[] order; // only needed for visualization
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// Build condensed distance matrix
|
||||
// Input arguments:
|
||||
// n = number of observables
|
||||
// m = dimension of observable
|
||||
// Output arguments:
|
||||
// out = allocated integer array of size n * (n - 1) / 2 for result
|
||||
void hclust_pdist(int n, int m, double* pts, double* out) {
|
||||
int ii = 0;
|
||||
for (int i = 0; i < n; i++) {
|
||||
for (int j = i + 1; j < n; j++) {
|
||||
// Compute euclidian distance
|
||||
double d = 0;
|
||||
for (int k = 0; k < m; k ++) {
|
||||
double error = pts[i * m + k] - pts[j * m + k];
|
||||
d += (error * error);
|
||||
}
|
||||
out[ii] = d;//sqrt(d);
|
||||
ii++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void cluster_points_centroid(int n, int m, double* pts, double dist, int* idx) {
|
||||
double* pdist = new double[n * (n - 1) / 2];
|
||||
int* merge = new int[2 * (n - 1)];
|
||||
double* height = new double[n - 1];
|
||||
|
||||
hclust_pdist(n, m, pts, pdist);
|
||||
hclust_fast(n, pdist, HCLUST_METHOD_CENTROID, merge, height);
|
||||
cutree_cdist(n, merge, height, dist, idx);
|
||||
|
||||
delete[] pdist;
|
||||
delete[] merge;
|
||||
delete[] height;
|
||||
}
|
||||
}
|
||||
Vendored
BIN
Binary file not shown.
+115
@@ -0,0 +1,115 @@
|
||||
//
|
||||
// Excerpt from fastcluster_R.cpp
|
||||
//
|
||||
// Copyright: Daniel Müllner, 2011 <http://danifold.net>
|
||||
//
|
||||
|
||||
struct pos_node {
|
||||
t_index pos;
|
||||
int node;
|
||||
};
|
||||
|
||||
void order_nodes(const int N, const int * const merge, const t_index * const node_size, int * const order) {
|
||||
/* Parameters:
|
||||
N : number of data points
|
||||
merge : (N-1)×2 array which specifies the node indices which are
|
||||
merged in each step of the clustering procedure.
|
||||
Negative entries -1...-N point to singleton nodes, while
|
||||
positive entries 1...(N-1) point to nodes which are themselves
|
||||
parents of other nodes.
|
||||
node_size : array of node sizes - makes it easier
|
||||
order : output array of size N
|
||||
|
||||
Runtime: Θ(N)
|
||||
*/
|
||||
auto_array_ptr<pos_node> queue(N/2);
|
||||
|
||||
int parent;
|
||||
int child;
|
||||
t_index pos = 0;
|
||||
|
||||
queue[0].pos = 0;
|
||||
queue[0].node = N-2;
|
||||
t_index idx = 1;
|
||||
|
||||
do {
|
||||
--idx;
|
||||
pos = queue[idx].pos;
|
||||
parent = queue[idx].node;
|
||||
|
||||
// First child
|
||||
child = merge[parent];
|
||||
if (child<0) { // singleton node, write this into the 'order' array.
|
||||
order[pos] = -child;
|
||||
++pos;
|
||||
}
|
||||
else { /* compound node: put it on top of the queue and decompose it
|
||||
in a later iteration. */
|
||||
queue[idx].pos = pos;
|
||||
queue[idx].node = child-1; // convert index-1 based to index-0 based
|
||||
++idx;
|
||||
pos += node_size[child-1];
|
||||
}
|
||||
// Second child
|
||||
child = merge[parent+N-1];
|
||||
if (child<0) {
|
||||
order[pos] = -child;
|
||||
}
|
||||
else {
|
||||
queue[idx].pos = pos;
|
||||
queue[idx].node = child-1;
|
||||
++idx;
|
||||
}
|
||||
} while (idx>0);
|
||||
}
|
||||
|
||||
#define size_(r_) ( ((r_<N) ? 1 : node_size[r_-N]) )
|
||||
|
||||
template <const bool sorted>
|
||||
void generate_R_dendrogram(int * const merge, double * const height, int * const order, cluster_result & Z2, const int N) {
|
||||
// The array "nodes" is a union-find data structure for the cluster
|
||||
// identites (only needed for unsorted cluster_result input).
|
||||
union_find nodes(sorted ? 0 : N);
|
||||
if (!sorted) {
|
||||
std::stable_sort(Z2[0], Z2[N-1]);
|
||||
}
|
||||
|
||||
t_index node1, node2;
|
||||
auto_array_ptr<t_index> node_size(N-1);
|
||||
|
||||
for (t_index i=0; i<N-1; ++i) {
|
||||
// Get two data points whose clusters are merged in step i.
|
||||
// Find the cluster identifiers for these points.
|
||||
if (sorted) {
|
||||
node1 = Z2[i]->node1;
|
||||
node2 = Z2[i]->node2;
|
||||
}
|
||||
else {
|
||||
node1 = nodes.Find(Z2[i]->node1);
|
||||
node2 = nodes.Find(Z2[i]->node2);
|
||||
// Merge the nodes in the union-find data structure by making them
|
||||
// children of a new node.
|
||||
nodes.Union(node1, node2);
|
||||
}
|
||||
// Sort the nodes in the output array.
|
||||
if (node1>node2) {
|
||||
t_index tmp = node1;
|
||||
node1 = node2;
|
||||
node2 = tmp;
|
||||
}
|
||||
/* Conversion between labeling conventions.
|
||||
Input: singleton nodes 0,...,N-1
|
||||
compound nodes N,...,2N-2
|
||||
Output: singleton nodes -1,...,-N
|
||||
compound nodes 1,...,N
|
||||
*/
|
||||
merge[i] = (node1<N) ? -static_cast<int>(node1)-1
|
||||
: static_cast<int>(node1)-N+1;
|
||||
merge[i+N-1] = (node2<N) ? -static_cast<int>(node2)-1
|
||||
: static_cast<int>(node2)-N+1;
|
||||
height[i] = Z2[i]->dist;
|
||||
node_size[i] = size_(node1) + size_(node2);
|
||||
}
|
||||
|
||||
order_nodes(N, merge, node_size, order);
|
||||
}
|
||||
+1794
File diff suppressed because it is too large
Load Diff
Vendored
+35
@@ -0,0 +1,35 @@
|
||||
#include <cassert>
|
||||
|
||||
extern "C" {
|
||||
#include "fastcluster.h"
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, const char* argv[]) {
|
||||
const int n = 11;
|
||||
const int m = 3;
|
||||
double* pts = new double[n*m]{59.26000137, -9.35999966, -5.42500019,
|
||||
91.61999817, -0.31999999, -2.75,
|
||||
31.38000031, 0.40000001, -0.2,
|
||||
89.57999725, -8.07999992, -18.04999924,
|
||||
53.42000122, 0.63999999, -0.175,
|
||||
31.38000031, 0.47999999, -0.2,
|
||||
36.33999939, 0.16, -0.2,
|
||||
53.33999939, 0.95999998, -0.175,
|
||||
59.26000137, -9.76000023, -5.44999981,
|
||||
33.93999977, 0.40000001, -0.22499999,
|
||||
106.74000092, -5.76000023, -18.04999924};
|
||||
|
||||
int * idx = new int[n];
|
||||
int * correct_idx = new int[n]{0, 1, 2, 3, 4, 2, 5, 4, 0, 5, 6};
|
||||
|
||||
cluster_points_centroid(n, m, pts, 2.5 * 2.5, idx);
|
||||
|
||||
for (int i = 0; i < n; i++) {
|
||||
assert(idx[i] == correct_idx[i]);
|
||||
}
|
||||
|
||||
delete[] idx;
|
||||
delete[] correct_idx;
|
||||
delete[] pts;
|
||||
}
|
||||
Vendored
+784
@@ -0,0 +1,784 @@
|
||||
/* Copyright (c) 2013 Dropbox, Inc.
|
||||
*
|
||||
* Permission is hereby granted, free of charge, to any person obtaining a copy
|
||||
* of this software and associated documentation files (the "Software"), to deal
|
||||
* in the Software without restriction, including without limitation the rights
|
||||
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
||||
* copies of the Software, and to permit persons to whom the Software is
|
||||
* furnished to do so, subject to the following conditions:
|
||||
*
|
||||
* The above copyright notice and this permission notice shall be included in
|
||||
* all copies or substantial portions of the Software.
|
||||
*
|
||||
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
||||
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
||||
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
||||
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
|
||||
* THE SOFTWARE.
|
||||
*/
|
||||
|
||||
#include "json11.hpp"
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <cstdlib>
|
||||
#include <cstdio>
|
||||
#include <limits>
|
||||
|
||||
namespace json11 {
|
||||
|
||||
static const int max_depth = 200;
|
||||
|
||||
using std::string;
|
||||
using std::vector;
|
||||
using std::map;
|
||||
using std::make_shared;
|
||||
using std::initializer_list;
|
||||
using std::move;
|
||||
|
||||
/* Helper for representing null - just a do-nothing struct, plus comparison
|
||||
* operators so the helpers in JsonValue work. We can't use nullptr_t because
|
||||
* it may not be orderable.
|
||||
*/
|
||||
struct NullStruct {
|
||||
bool operator==(NullStruct) const { return true; }
|
||||
bool operator<(NullStruct) const { return false; }
|
||||
};
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Serialization
|
||||
*/
|
||||
|
||||
static void dump(NullStruct, string &out) {
|
||||
out += "null";
|
||||
}
|
||||
|
||||
static void dump(double value, string &out) {
|
||||
if (std::isfinite(value)) {
|
||||
char buf[32];
|
||||
snprintf(buf, sizeof buf, "%.17g", value);
|
||||
out += buf;
|
||||
} else {
|
||||
out += "null";
|
||||
}
|
||||
}
|
||||
|
||||
static void dump(int value, string &out) {
|
||||
char buf[32];
|
||||
snprintf(buf, sizeof buf, "%d", value);
|
||||
out += buf;
|
||||
}
|
||||
|
||||
static void dump(bool value, string &out) {
|
||||
out += value ? "true" : "false";
|
||||
}
|
||||
|
||||
static void dump(const string &value, string &out) {
|
||||
out += '"';
|
||||
for (size_t i = 0; i < value.length(); i++) {
|
||||
const char ch = value[i];
|
||||
if (ch == '\\') {
|
||||
out += "\\\\";
|
||||
} else if (ch == '"') {
|
||||
out += "\\\"";
|
||||
} else if (ch == '\b') {
|
||||
out += "\\b";
|
||||
} else if (ch == '\f') {
|
||||
out += "\\f";
|
||||
} else if (ch == '\n') {
|
||||
out += "\\n";
|
||||
} else if (ch == '\r') {
|
||||
out += "\\r";
|
||||
} else if (ch == '\t') {
|
||||
out += "\\t";
|
||||
} else if (static_cast<uint8_t>(ch) <= 0x1f) {
|
||||
char buf[8];
|
||||
snprintf(buf, sizeof buf, "\\u%04x", ch);
|
||||
out += buf;
|
||||
} else if (static_cast<uint8_t>(ch) == 0xe2 && static_cast<uint8_t>(value[i+1]) == 0x80
|
||||
&& static_cast<uint8_t>(value[i+2]) == 0xa8) {
|
||||
out += "\\u2028";
|
||||
i += 2;
|
||||
} else if (static_cast<uint8_t>(ch) == 0xe2 && static_cast<uint8_t>(value[i+1]) == 0x80
|
||||
&& static_cast<uint8_t>(value[i+2]) == 0xa9) {
|
||||
out += "\\u2029";
|
||||
i += 2;
|
||||
} else {
|
||||
out += ch;
|
||||
}
|
||||
}
|
||||
out += '"';
|
||||
}
|
||||
|
||||
static void dump(const Json::array &values, string &out) {
|
||||
bool first = true;
|
||||
out += "[";
|
||||
for (const auto &value : values) {
|
||||
if (!first)
|
||||
out += ", ";
|
||||
value.dump(out);
|
||||
first = false;
|
||||
}
|
||||
out += "]";
|
||||
}
|
||||
|
||||
static void dump(const Json::object &values, string &out) {
|
||||
bool first = true;
|
||||
out += "{";
|
||||
for (const auto &kv : values) {
|
||||
if (!first)
|
||||
out += ", ";
|
||||
dump(kv.first, out);
|
||||
out += ": ";
|
||||
kv.second.dump(out);
|
||||
first = false;
|
||||
}
|
||||
out += "}";
|
||||
}
|
||||
|
||||
void Json::dump(string &out) const {
|
||||
m_ptr->dump(out);
|
||||
}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Value wrappers
|
||||
*/
|
||||
|
||||
template <Json::Type tag, typename T>
|
||||
class Value : public JsonValue {
|
||||
protected:
|
||||
|
||||
// Constructors
|
||||
explicit Value(const T &value) : m_value(value) {}
|
||||
explicit Value(T &&value) : m_value(move(value)) {}
|
||||
|
||||
// Get type tag
|
||||
Json::Type type() const override {
|
||||
return tag;
|
||||
}
|
||||
|
||||
// Comparisons
|
||||
bool equals(const JsonValue * other) const override {
|
||||
return m_value == static_cast<const Value<tag, T> *>(other)->m_value;
|
||||
}
|
||||
bool less(const JsonValue * other) const override {
|
||||
return m_value < static_cast<const Value<tag, T> *>(other)->m_value;
|
||||
}
|
||||
|
||||
const T m_value;
|
||||
void dump(string &out) const override { json11::dump(m_value, out); }
|
||||
};
|
||||
|
||||
class JsonDouble final : public Value<Json::NUMBER, double> {
|
||||
double number_value() const override { return m_value; }
|
||||
int int_value() const override { return static_cast<int>(m_value); }
|
||||
bool equals(const JsonValue * other) const override { return m_value == other->number_value(); }
|
||||
bool less(const JsonValue * other) const override { return m_value < other->number_value(); }
|
||||
public:
|
||||
explicit JsonDouble(double value) : Value(value) {}
|
||||
};
|
||||
|
||||
class JsonInt final : public Value<Json::NUMBER, int> {
|
||||
double number_value() const override { return m_value; }
|
||||
int int_value() const override { return m_value; }
|
||||
bool equals(const JsonValue * other) const override { return m_value == other->number_value(); }
|
||||
bool less(const JsonValue * other) const override { return m_value < other->number_value(); }
|
||||
public:
|
||||
explicit JsonInt(int value) : Value(value) {}
|
||||
};
|
||||
|
||||
class JsonBoolean final : public Value<Json::BOOL, bool> {
|
||||
bool bool_value() const override { return m_value; }
|
||||
public:
|
||||
explicit JsonBoolean(bool value) : Value(value) {}
|
||||
};
|
||||
|
||||
class JsonString final : public Value<Json::STRING, string> {
|
||||
const string &string_value() const override { return m_value; }
|
||||
public:
|
||||
explicit JsonString(const string &value) : Value(value) {}
|
||||
explicit JsonString(string &&value) : Value(move(value)) {}
|
||||
};
|
||||
|
||||
class JsonArray final : public Value<Json::ARRAY, Json::array> {
|
||||
const Json::array &array_items() const override { return m_value; }
|
||||
const Json & operator[](size_t i) const override;
|
||||
public:
|
||||
explicit JsonArray(const Json::array &value) : Value(value) {}
|
||||
explicit JsonArray(Json::array &&value) : Value(move(value)) {}
|
||||
};
|
||||
|
||||
class JsonObject final : public Value<Json::OBJECT, Json::object> {
|
||||
const Json::object &object_items() const override { return m_value; }
|
||||
const Json & operator[](const string &key) const override;
|
||||
public:
|
||||
explicit JsonObject(const Json::object &value) : Value(value) {}
|
||||
explicit JsonObject(Json::object &&value) : Value(move(value)) {}
|
||||
};
|
||||
|
||||
class JsonNull final : public Value<Json::NUL, NullStruct> {
|
||||
public:
|
||||
JsonNull() : Value({}) {}
|
||||
};
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Static globals - static-init-safe
|
||||
*/
|
||||
struct Statics {
|
||||
const std::shared_ptr<JsonValue> null = make_shared<JsonNull>();
|
||||
const std::shared_ptr<JsonValue> t = make_shared<JsonBoolean>(true);
|
||||
const std::shared_ptr<JsonValue> f = make_shared<JsonBoolean>(false);
|
||||
const string empty_string;
|
||||
const vector<Json> empty_vector;
|
||||
const map<string, Json> empty_map;
|
||||
Statics() {}
|
||||
};
|
||||
|
||||
static const Statics & statics() {
|
||||
static const Statics s {};
|
||||
return s;
|
||||
}
|
||||
|
||||
static const Json & static_null() {
|
||||
// This has to be separate, not in Statics, because Json() accesses statics().null.
|
||||
static const Json json_null;
|
||||
return json_null;
|
||||
}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Constructors
|
||||
*/
|
||||
|
||||
Json::Json() noexcept : m_ptr(statics().null) {}
|
||||
Json::Json(std::nullptr_t) noexcept : m_ptr(statics().null) {}
|
||||
Json::Json(double value) : m_ptr(make_shared<JsonDouble>(value)) {}
|
||||
Json::Json(int value) : m_ptr(make_shared<JsonInt>(value)) {}
|
||||
Json::Json(bool value) : m_ptr(value ? statics().t : statics().f) {}
|
||||
Json::Json(const string &value) : m_ptr(make_shared<JsonString>(value)) {}
|
||||
Json::Json(string &&value) : m_ptr(make_shared<JsonString>(move(value))) {}
|
||||
Json::Json(const char * value) : m_ptr(make_shared<JsonString>(value)) {}
|
||||
Json::Json(const Json::array &values) : m_ptr(make_shared<JsonArray>(values)) {}
|
||||
Json::Json(Json::array &&values) : m_ptr(make_shared<JsonArray>(move(values))) {}
|
||||
Json::Json(const Json::object &values) : m_ptr(make_shared<JsonObject>(values)) {}
|
||||
Json::Json(Json::object &&values) : m_ptr(make_shared<JsonObject>(move(values))) {}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Accessors
|
||||
*/
|
||||
|
||||
Json::Type Json::type() const { return m_ptr->type(); }
|
||||
double Json::number_value() const { return m_ptr->number_value(); }
|
||||
int Json::int_value() const { return m_ptr->int_value(); }
|
||||
bool Json::bool_value() const { return m_ptr->bool_value(); }
|
||||
const string & Json::string_value() const { return m_ptr->string_value(); }
|
||||
const vector<Json> & Json::array_items() const { return m_ptr->array_items(); }
|
||||
const map<string, Json> & Json::object_items() const { return m_ptr->object_items(); }
|
||||
const Json & Json::operator[] (size_t i) const { return (*m_ptr)[i]; }
|
||||
const Json & Json::operator[] (const string &key) const { return (*m_ptr)[key]; }
|
||||
|
||||
double JsonValue::number_value() const { return 0; }
|
||||
int JsonValue::int_value() const { return 0; }
|
||||
bool JsonValue::bool_value() const { return false; }
|
||||
const string & JsonValue::string_value() const { return statics().empty_string; }
|
||||
const vector<Json> & JsonValue::array_items() const { return statics().empty_vector; }
|
||||
const map<string, Json> & JsonValue::object_items() const { return statics().empty_map; }
|
||||
const Json & JsonValue::operator[] (size_t) const { return static_null(); }
|
||||
const Json & JsonValue::operator[] (const string &) const { return static_null(); }
|
||||
|
||||
const Json & JsonObject::operator[] (const string &key) const {
|
||||
auto iter = m_value.find(key);
|
||||
return (iter == m_value.end()) ? static_null() : iter->second;
|
||||
}
|
||||
const Json & JsonArray::operator[] (size_t i) const {
|
||||
if (i >= m_value.size()) return static_null();
|
||||
else return m_value[i];
|
||||
}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Comparison
|
||||
*/
|
||||
|
||||
bool Json::operator== (const Json &other) const {
|
||||
if (m_ptr->type() != other.m_ptr->type())
|
||||
return false;
|
||||
|
||||
return m_ptr->equals(other.m_ptr.get());
|
||||
}
|
||||
|
||||
bool Json::operator< (const Json &other) const {
|
||||
if (m_ptr->type() != other.m_ptr->type())
|
||||
return m_ptr->type() < other.m_ptr->type();
|
||||
|
||||
return m_ptr->less(other.m_ptr.get());
|
||||
}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Parsing
|
||||
*/
|
||||
|
||||
/* esc(c)
|
||||
*
|
||||
* Format char c suitable for printing in an error message.
|
||||
*/
|
||||
static inline string esc(char c) {
|
||||
char buf[12];
|
||||
if (static_cast<uint8_t>(c) >= 0x20 && static_cast<uint8_t>(c) <= 0x7f) {
|
||||
snprintf(buf, sizeof buf, "'%c' (%d)", c, c);
|
||||
} else {
|
||||
snprintf(buf, sizeof buf, "(%d)", c);
|
||||
}
|
||||
return string(buf);
|
||||
}
|
||||
|
||||
static inline bool in_range(long x, long lower, long upper) {
|
||||
return (x >= lower && x <= upper);
|
||||
}
|
||||
|
||||
namespace {
|
||||
/* JsonParser
|
||||
*
|
||||
* Object that tracks all state of an in-progress parse.
|
||||
*/
|
||||
struct JsonParser final {
|
||||
|
||||
/* State
|
||||
*/
|
||||
const string &str;
|
||||
size_t i;
|
||||
string &err;
|
||||
bool failed;
|
||||
const JsonParse strategy;
|
||||
|
||||
/* fail(msg, err_ret = Json())
|
||||
*
|
||||
* Mark this parse as failed.
|
||||
*/
|
||||
Json fail(string &&msg) {
|
||||
return fail(move(msg), Json());
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
T fail(string &&msg, const T err_ret) {
|
||||
if (!failed)
|
||||
err = std::move(msg);
|
||||
failed = true;
|
||||
return err_ret;
|
||||
}
|
||||
|
||||
/* consume_whitespace()
|
||||
*
|
||||
* Advance until the current character is non-whitespace.
|
||||
*/
|
||||
void consume_whitespace() {
|
||||
while (str[i] == ' ' || str[i] == '\r' || str[i] == '\n' || str[i] == '\t')
|
||||
i++;
|
||||
}
|
||||
|
||||
/* consume_comment()
|
||||
*
|
||||
* Advance comments (c-style inline and multiline).
|
||||
*/
|
||||
bool consume_comment() {
|
||||
bool comment_found = false;
|
||||
if (str[i] == '/') {
|
||||
i++;
|
||||
if (i == str.size())
|
||||
return fail("unexpected end of input after start of comment", false);
|
||||
if (str[i] == '/') { // inline comment
|
||||
i++;
|
||||
// advance until next line, or end of input
|
||||
while (i < str.size() && str[i] != '\n') {
|
||||
i++;
|
||||
}
|
||||
comment_found = true;
|
||||
}
|
||||
else if (str[i] == '*') { // multiline comment
|
||||
i++;
|
||||
if (i > str.size()-2)
|
||||
return fail("unexpected end of input inside multi-line comment", false);
|
||||
// advance until closing tokens
|
||||
while (!(str[i] == '*' && str[i+1] == '/')) {
|
||||
i++;
|
||||
if (i > str.size()-2)
|
||||
return fail(
|
||||
"unexpected end of input inside multi-line comment", false);
|
||||
}
|
||||
i += 2;
|
||||
comment_found = true;
|
||||
}
|
||||
else
|
||||
return fail("malformed comment", false);
|
||||
}
|
||||
return comment_found;
|
||||
}
|
||||
|
||||
/* consume_garbage()
|
||||
*
|
||||
* Advance until the current character is non-whitespace and non-comment.
|
||||
*/
|
||||
void consume_garbage() {
|
||||
consume_whitespace();
|
||||
if(strategy == JsonParse::COMMENTS) {
|
||||
bool comment_found = false;
|
||||
do {
|
||||
comment_found = consume_comment();
|
||||
if (failed) return;
|
||||
consume_whitespace();
|
||||
}
|
||||
while(comment_found);
|
||||
}
|
||||
}
|
||||
|
||||
/* get_next_token()
|
||||
*
|
||||
* Return the next non-whitespace character. If the end of the input is reached,
|
||||
* flag an error and return 0.
|
||||
*/
|
||||
char get_next_token() {
|
||||
consume_garbage();
|
||||
if (failed) return (char)0;
|
||||
if (i == str.size())
|
||||
return fail("unexpected end of input", (char)0);
|
||||
|
||||
return str[i++];
|
||||
}
|
||||
|
||||
/* encode_utf8(pt, out)
|
||||
*
|
||||
* Encode pt as UTF-8 and add it to out.
|
||||
*/
|
||||
void encode_utf8(long pt, string & out) {
|
||||
if (pt < 0)
|
||||
return;
|
||||
|
||||
if (pt < 0x80) {
|
||||
out += static_cast<char>(pt);
|
||||
} else if (pt < 0x800) {
|
||||
out += static_cast<char>((pt >> 6) | 0xC0);
|
||||
out += static_cast<char>((pt & 0x3F) | 0x80);
|
||||
} else if (pt < 0x10000) {
|
||||
out += static_cast<char>((pt >> 12) | 0xE0);
|
||||
out += static_cast<char>(((pt >> 6) & 0x3F) | 0x80);
|
||||
out += static_cast<char>((pt & 0x3F) | 0x80);
|
||||
} else {
|
||||
out += static_cast<char>((pt >> 18) | 0xF0);
|
||||
out += static_cast<char>(((pt >> 12) & 0x3F) | 0x80);
|
||||
out += static_cast<char>(((pt >> 6) & 0x3F) | 0x80);
|
||||
out += static_cast<char>((pt & 0x3F) | 0x80);
|
||||
}
|
||||
}
|
||||
|
||||
/* parse_string()
|
||||
*
|
||||
* Parse a string, starting at the current position.
|
||||
*/
|
||||
string parse_string() {
|
||||
string out;
|
||||
long last_escaped_codepoint = -1;
|
||||
while (true) {
|
||||
if (i == str.size())
|
||||
return fail("unexpected end of input in string", "");
|
||||
|
||||
char ch = str[i++];
|
||||
|
||||
if (ch == '"') {
|
||||
encode_utf8(last_escaped_codepoint, out);
|
||||
return out;
|
||||
}
|
||||
|
||||
if (in_range(ch, 0, 0x1f))
|
||||
return fail("unescaped " + esc(ch) + " in string", "");
|
||||
|
||||
// The usual case: non-escaped characters
|
||||
if (ch != '\\') {
|
||||
encode_utf8(last_escaped_codepoint, out);
|
||||
last_escaped_codepoint = -1;
|
||||
out += ch;
|
||||
continue;
|
||||
}
|
||||
|
||||
// Handle escapes
|
||||
if (i == str.size())
|
||||
return fail("unexpected end of input in string", "");
|
||||
|
||||
ch = str[i++];
|
||||
|
||||
if (ch == 'u') {
|
||||
// Extract 4-byte escape sequence
|
||||
string esc = str.substr(i, 4);
|
||||
// Explicitly check length of the substring. The following loop
|
||||
// relies on std::string returning the terminating NUL when
|
||||
// accessing str[length]. Checking here reduces brittleness.
|
||||
if (esc.length() < 4) {
|
||||
return fail("bad \\u escape: " + esc, "");
|
||||
}
|
||||
for (size_t j = 0; j < 4; j++) {
|
||||
if (!in_range(esc[j], 'a', 'f') && !in_range(esc[j], 'A', 'F')
|
||||
&& !in_range(esc[j], '0', '9'))
|
||||
return fail("bad \\u escape: " + esc, "");
|
||||
}
|
||||
|
||||
long codepoint = strtol(esc.data(), nullptr, 16);
|
||||
|
||||
// JSON specifies that characters outside the BMP shall be encoded as a pair
|
||||
// of 4-hex-digit \u escapes encoding their surrogate pair components. Check
|
||||
// whether we're in the middle of such a beast: the previous codepoint was an
|
||||
// escaped lead (high) surrogate, and this is a trail (low) surrogate.
|
||||
if (in_range(last_escaped_codepoint, 0xD800, 0xDBFF)
|
||||
&& in_range(codepoint, 0xDC00, 0xDFFF)) {
|
||||
// Reassemble the two surrogate pairs into one astral-plane character, per
|
||||
// the UTF-16 algorithm.
|
||||
encode_utf8((((last_escaped_codepoint - 0xD800) << 10)
|
||||
| (codepoint - 0xDC00)) + 0x10000, out);
|
||||
last_escaped_codepoint = -1;
|
||||
} else {
|
||||
encode_utf8(last_escaped_codepoint, out);
|
||||
last_escaped_codepoint = codepoint;
|
||||
}
|
||||
|
||||
i += 4;
|
||||
continue;
|
||||
}
|
||||
|
||||
encode_utf8(last_escaped_codepoint, out);
|
||||
last_escaped_codepoint = -1;
|
||||
|
||||
if (ch == 'b') {
|
||||
out += '\b';
|
||||
} else if (ch == 'f') {
|
||||
out += '\f';
|
||||
} else if (ch == 'n') {
|
||||
out += '\n';
|
||||
} else if (ch == 'r') {
|
||||
out += '\r';
|
||||
} else if (ch == 't') {
|
||||
out += '\t';
|
||||
} else if (ch == '"' || ch == '\\' || ch == '/') {
|
||||
out += ch;
|
||||
} else {
|
||||
return fail("invalid escape character " + esc(ch), "");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* parse_number()
|
||||
*
|
||||
* Parse a double.
|
||||
*/
|
||||
Json parse_number() {
|
||||
size_t start_pos = i;
|
||||
|
||||
if (str[i] == '-')
|
||||
i++;
|
||||
|
||||
// Integer part
|
||||
if (str[i] == '0') {
|
||||
i++;
|
||||
if (in_range(str[i], '0', '9'))
|
||||
return fail("leading 0s not permitted in numbers");
|
||||
} else if (in_range(str[i], '1', '9')) {
|
||||
i++;
|
||||
while (in_range(str[i], '0', '9'))
|
||||
i++;
|
||||
} else {
|
||||
return fail("invalid " + esc(str[i]) + " in number");
|
||||
}
|
||||
|
||||
if (str[i] != '.' && str[i] != 'e' && str[i] != 'E'
|
||||
&& (i - start_pos) <= static_cast<size_t>(std::numeric_limits<int>::digits10)) {
|
||||
return std::atoi(str.c_str() + start_pos);
|
||||
}
|
||||
|
||||
// Decimal part
|
||||
if (str[i] == '.') {
|
||||
i++;
|
||||
if (!in_range(str[i], '0', '9'))
|
||||
return fail("at least one digit required in fractional part");
|
||||
|
||||
while (in_range(str[i], '0', '9'))
|
||||
i++;
|
||||
}
|
||||
|
||||
// Exponent part
|
||||
if (str[i] == 'e' || str[i] == 'E') {
|
||||
i++;
|
||||
|
||||
if (str[i] == '+' || str[i] == '-')
|
||||
i++;
|
||||
|
||||
if (!in_range(str[i], '0', '9'))
|
||||
return fail("at least one digit required in exponent");
|
||||
|
||||
while (in_range(str[i], '0', '9'))
|
||||
i++;
|
||||
}
|
||||
|
||||
return std::strtod(str.c_str() + start_pos, nullptr);
|
||||
}
|
||||
|
||||
/* expect(str, res)
|
||||
*
|
||||
* Expect that 'str' starts at the character that was just read. If it does, advance
|
||||
* the input and return res. If not, flag an error.
|
||||
*/
|
||||
Json expect(const string &expected, Json res) {
|
||||
assert(i != 0);
|
||||
i--;
|
||||
if (str.compare(i, expected.length(), expected) == 0) {
|
||||
i += expected.length();
|
||||
return res;
|
||||
} else {
|
||||
return fail("parse error: expected " + expected + ", got " + str.substr(i, expected.length()));
|
||||
}
|
||||
}
|
||||
|
||||
/* parse_json()
|
||||
*
|
||||
* Parse a JSON object.
|
||||
*/
|
||||
Json parse_json(int depth) {
|
||||
if (depth > max_depth) {
|
||||
return fail("exceeded maximum nesting depth");
|
||||
}
|
||||
|
||||
char ch = get_next_token();
|
||||
if (failed)
|
||||
return Json();
|
||||
|
||||
if (ch == '-' || (ch >= '0' && ch <= '9')) {
|
||||
i--;
|
||||
return parse_number();
|
||||
}
|
||||
|
||||
if (ch == 't')
|
||||
return expect("true", true);
|
||||
|
||||
if (ch == 'f')
|
||||
return expect("false", false);
|
||||
|
||||
if (ch == 'n')
|
||||
return expect("null", Json());
|
||||
|
||||
if (ch == '"')
|
||||
return parse_string();
|
||||
|
||||
if (ch == '{') {
|
||||
map<string, Json> data;
|
||||
ch = get_next_token();
|
||||
if (ch == '}')
|
||||
return data;
|
||||
|
||||
while (1) {
|
||||
if (ch != '"')
|
||||
return fail("expected '\"' in object, got " + esc(ch));
|
||||
|
||||
string key = parse_string();
|
||||
if (failed)
|
||||
return Json();
|
||||
|
||||
ch = get_next_token();
|
||||
if (ch != ':')
|
||||
return fail("expected ':' in object, got " + esc(ch));
|
||||
|
||||
data[std::move(key)] = parse_json(depth + 1);
|
||||
if (failed)
|
||||
return Json();
|
||||
|
||||
ch = get_next_token();
|
||||
if (ch == '}')
|
||||
break;
|
||||
if (ch != ',')
|
||||
return fail("expected ',' in object, got " + esc(ch));
|
||||
|
||||
ch = get_next_token();
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
if (ch == '[') {
|
||||
vector<Json> data;
|
||||
ch = get_next_token();
|
||||
if (ch == ']')
|
||||
return data;
|
||||
|
||||
while (1) {
|
||||
i--;
|
||||
data.push_back(parse_json(depth + 1));
|
||||
if (failed)
|
||||
return Json();
|
||||
|
||||
ch = get_next_token();
|
||||
if (ch == ']')
|
||||
break;
|
||||
if (ch != ',')
|
||||
return fail("expected ',' in list, got " + esc(ch));
|
||||
|
||||
ch = get_next_token();
|
||||
(void)ch;
|
||||
}
|
||||
return data;
|
||||
}
|
||||
|
||||
return fail("expected value, got " + esc(ch));
|
||||
}
|
||||
};
|
||||
}//namespace {
|
||||
|
||||
Json Json::parse(const string &in, string &err, JsonParse strategy) {
|
||||
JsonParser parser { in, 0, err, false, strategy };
|
||||
Json result = parser.parse_json(0);
|
||||
|
||||
// Check for any trailing garbage
|
||||
parser.consume_garbage();
|
||||
if (parser.failed)
|
||||
return Json();
|
||||
if (parser.i != in.size())
|
||||
return parser.fail("unexpected trailing " + esc(in[parser.i]));
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// Documented in json11.hpp
|
||||
vector<Json> Json::parse_multi(const string &in,
|
||||
std::string::size_type &parser_stop_pos,
|
||||
string &err,
|
||||
JsonParse strategy) {
|
||||
JsonParser parser { in, 0, err, false, strategy };
|
||||
parser_stop_pos = 0;
|
||||
vector<Json> json_vec;
|
||||
while (parser.i != in.size() && !parser.failed) {
|
||||
json_vec.push_back(parser.parse_json(0));
|
||||
if (parser.failed)
|
||||
break;
|
||||
|
||||
// Check for another object
|
||||
parser.consume_garbage();
|
||||
if (parser.failed)
|
||||
break;
|
||||
parser_stop_pos = parser.i;
|
||||
}
|
||||
return json_vec;
|
||||
}
|
||||
|
||||
/* * * * * * * * * * * * * * * * * * * *
|
||||
* Shape-checking
|
||||
*/
|
||||
|
||||
bool Json::has_shape(const shape & types, string & err) const {
|
||||
if (!is_object()) {
|
||||
err = "expected JSON object, got " + dump();
|
||||
return false;
|
||||
}
|
||||
|
||||
for (auto & item : types) {
|
||||
if ((*this)[item.first].type() != item.second) {
|
||||
err = "bad type for " + item.first + " in " + dump();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
} // namespace json11
|
||||
Vendored
+689
@@ -0,0 +1,689 @@
|
||||
#include <kaitai/kaitaistream.h>
|
||||
|
||||
#if defined(__APPLE__)
|
||||
#include <machine/endian.h>
|
||||
#include <libkern/OSByteOrder.h>
|
||||
#define bswap_16(x) OSSwapInt16(x)
|
||||
#define bswap_32(x) OSSwapInt32(x)
|
||||
#define bswap_64(x) OSSwapInt64(x)
|
||||
#define __BYTE_ORDER BYTE_ORDER
|
||||
#define __BIG_ENDIAN BIG_ENDIAN
|
||||
#define __LITTLE_ENDIAN LITTLE_ENDIAN
|
||||
#elif defined(_MSC_VER) // !__APPLE__
|
||||
#include <stdlib.h>
|
||||
#define __LITTLE_ENDIAN 1234
|
||||
#define __BIG_ENDIAN 4321
|
||||
#define __BYTE_ORDER __LITTLE_ENDIAN
|
||||
#define bswap_16(x) _byteswap_ushort(x)
|
||||
#define bswap_32(x) _byteswap_ulong(x)
|
||||
#define bswap_64(x) _byteswap_uint64(x)
|
||||
#else // !__APPLE__ or !_MSC_VER
|
||||
#include <endian.h>
|
||||
#include <byteswap.h>
|
||||
#endif
|
||||
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <stdexcept>
|
||||
|
||||
kaitai::kstream::kstream(std::istream* io) {
|
||||
m_io = io;
|
||||
init();
|
||||
}
|
||||
|
||||
kaitai::kstream::kstream(std::string& data): m_io_str(data) {
|
||||
m_io = &m_io_str;
|
||||
init();
|
||||
}
|
||||
|
||||
void kaitai::kstream::init() {
|
||||
exceptions_enable();
|
||||
align_to_byte();
|
||||
}
|
||||
|
||||
void kaitai::kstream::close() {
|
||||
// m_io->close();
|
||||
}
|
||||
|
||||
void kaitai::kstream::exceptions_enable() const {
|
||||
m_io->exceptions(
|
||||
std::istream::eofbit |
|
||||
std::istream::failbit |
|
||||
std::istream::badbit
|
||||
);
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Stream positioning
|
||||
// ========================================================================
|
||||
|
||||
bool kaitai::kstream::is_eof() const {
|
||||
if (m_bits_left > 0) {
|
||||
return false;
|
||||
}
|
||||
char t;
|
||||
m_io->exceptions(
|
||||
std::istream::badbit
|
||||
);
|
||||
m_io->get(t);
|
||||
if (m_io->eof()) {
|
||||
m_io->clear();
|
||||
exceptions_enable();
|
||||
return true;
|
||||
} else {
|
||||
m_io->unget();
|
||||
exceptions_enable();
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
void kaitai::kstream::seek(uint64_t pos) {
|
||||
m_io->seekg(pos);
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::pos() {
|
||||
return m_io->tellg();
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::size() {
|
||||
std::iostream::pos_type cur_pos = m_io->tellg();
|
||||
m_io->seekg(0, std::ios::end);
|
||||
std::iostream::pos_type len = m_io->tellg();
|
||||
m_io->seekg(cur_pos);
|
||||
return len;
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Integer numbers
|
||||
// ========================================================================
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
// Signed
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
int8_t kaitai::kstream::read_s1() {
|
||||
char t;
|
||||
m_io->get(t);
|
||||
return t;
|
||||
}
|
||||
|
||||
// ........................................................................
|
||||
// Big-endian
|
||||
// ........................................................................
|
||||
|
||||
int16_t kaitai::kstream::read_s2be() {
|
||||
int16_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 2);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_16(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
int32_t kaitai::kstream::read_s4be() {
|
||||
int32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
int64_t kaitai::kstream::read_s8be() {
|
||||
int64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
// ........................................................................
|
||||
// Little-endian
|
||||
// ........................................................................
|
||||
|
||||
int16_t kaitai::kstream::read_s2le() {
|
||||
int16_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 2);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_16(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
int32_t kaitai::kstream::read_s4le() {
|
||||
int32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
int64_t kaitai::kstream::read_s8le() {
|
||||
int64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------
|
||||
// Unsigned
|
||||
// ------------------------------------------------------------------------
|
||||
|
||||
uint8_t kaitai::kstream::read_u1() {
|
||||
char t;
|
||||
m_io->get(t);
|
||||
return t;
|
||||
}
|
||||
|
||||
// ........................................................................
|
||||
// Big-endian
|
||||
// ........................................................................
|
||||
|
||||
uint16_t kaitai::kstream::read_u2be() {
|
||||
uint16_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 2);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_16(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
uint32_t kaitai::kstream::read_u4be() {
|
||||
uint32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::read_u8be() {
|
||||
uint64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
// ........................................................................
|
||||
// Little-endian
|
||||
// ........................................................................
|
||||
|
||||
uint16_t kaitai::kstream::read_u2le() {
|
||||
uint16_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 2);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_16(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
uint32_t kaitai::kstream::read_u4le() {
|
||||
uint32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::read_u8le() {
|
||||
uint64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return t;
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Floating point numbers
|
||||
// ========================================================================
|
||||
|
||||
// ........................................................................
|
||||
// Big-endian
|
||||
// ........................................................................
|
||||
|
||||
float kaitai::kstream::read_f4be() {
|
||||
uint32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return reinterpret_cast<float&>(t);
|
||||
}
|
||||
|
||||
double kaitai::kstream::read_f8be() {
|
||||
uint64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __LITTLE_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return reinterpret_cast<double&>(t);
|
||||
}
|
||||
|
||||
// ........................................................................
|
||||
// Little-endian
|
||||
// ........................................................................
|
||||
|
||||
float kaitai::kstream::read_f4le() {
|
||||
uint32_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 4);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_32(t);
|
||||
#endif
|
||||
return reinterpret_cast<float&>(t);
|
||||
}
|
||||
|
||||
double kaitai::kstream::read_f8le() {
|
||||
uint64_t t;
|
||||
m_io->read(reinterpret_cast<char *>(&t), 8);
|
||||
#if __BYTE_ORDER == __BIG_ENDIAN
|
||||
t = bswap_64(t);
|
||||
#endif
|
||||
return reinterpret_cast<double&>(t);
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Unaligned bit values
|
||||
// ========================================================================
|
||||
|
||||
void kaitai::kstream::align_to_byte() {
|
||||
m_bits_left = 0;
|
||||
m_bits = 0;
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::read_bits_int_be(int n) {
|
||||
int bits_needed = n - m_bits_left;
|
||||
if (bits_needed > 0) {
|
||||
// 1 bit => 1 byte
|
||||
// 8 bits => 1 byte
|
||||
// 9 bits => 2 bytes
|
||||
int bytes_needed = ((bits_needed - 1) / 8) + 1;
|
||||
if (bytes_needed > 8)
|
||||
throw std::runtime_error("read_bits_int: more than 8 bytes requested");
|
||||
char buf[8];
|
||||
m_io->read(buf, bytes_needed);
|
||||
for (int i = 0; i < bytes_needed; i++) {
|
||||
uint8_t b = buf[i];
|
||||
m_bits <<= 8;
|
||||
m_bits |= b;
|
||||
m_bits_left += 8;
|
||||
}
|
||||
}
|
||||
|
||||
// raw mask with required number of 1s, starting from lowest bit
|
||||
uint64_t mask = get_mask_ones(n);
|
||||
// shift mask to align with highest bits available in @bits
|
||||
int shift_bits = m_bits_left - n;
|
||||
mask <<= shift_bits;
|
||||
// derive reading result
|
||||
uint64_t res = (m_bits & mask) >> shift_bits;
|
||||
// clear top bits that we've just read => AND with 1s
|
||||
m_bits_left -= n;
|
||||
mask = get_mask_ones(m_bits_left);
|
||||
m_bits &= mask;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
// Deprecated, use read_bits_int_be() instead.
|
||||
uint64_t kaitai::kstream::read_bits_int(int n) {
|
||||
return read_bits_int_be(n);
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::read_bits_int_le(int n) {
|
||||
int bits_needed = n - m_bits_left;
|
||||
if (bits_needed > 0) {
|
||||
// 1 bit => 1 byte
|
||||
// 8 bits => 1 byte
|
||||
// 9 bits => 2 bytes
|
||||
int bytes_needed = ((bits_needed - 1) / 8) + 1;
|
||||
if (bytes_needed > 8)
|
||||
throw std::runtime_error("read_bits_int_le: more than 8 bytes requested");
|
||||
char buf[8];
|
||||
m_io->read(buf, bytes_needed);
|
||||
for (int i = 0; i < bytes_needed; i++) {
|
||||
uint8_t b = buf[i];
|
||||
m_bits |= (static_cast<uint64_t>(b) << m_bits_left);
|
||||
m_bits_left += 8;
|
||||
}
|
||||
}
|
||||
|
||||
// raw mask with required number of 1s, starting from lowest bit
|
||||
uint64_t mask = get_mask_ones(n);
|
||||
// derive reading result
|
||||
uint64_t res = m_bits & mask;
|
||||
// remove bottom bits that we've just read by shifting
|
||||
m_bits >>= n;
|
||||
m_bits_left -= n;
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
uint64_t kaitai::kstream::get_mask_ones(int n) {
|
||||
if (n == 64) {
|
||||
return 0xFFFFFFFFFFFFFFFF;
|
||||
} else {
|
||||
return ((uint64_t) 1 << n) - 1;
|
||||
}
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Byte arrays
|
||||
// ========================================================================
|
||||
|
||||
std::string kaitai::kstream::read_bytes(std::streamsize len) {
|
||||
std::vector<char> result(len);
|
||||
|
||||
// NOTE: streamsize type is signed, negative values are only *supposed* to not be used.
|
||||
// http://en.cppreference.com/w/cpp/io/streamsize
|
||||
if (len < 0) {
|
||||
throw std::runtime_error("read_bytes: requested a negative amount");
|
||||
}
|
||||
|
||||
if (len > 0) {
|
||||
m_io->read(&result[0], len);
|
||||
}
|
||||
|
||||
return std::string(result.begin(), result.end());
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::read_bytes_full() {
|
||||
std::iostream::pos_type p1 = m_io->tellg();
|
||||
m_io->seekg(0, std::ios::end);
|
||||
std::iostream::pos_type p2 = m_io->tellg();
|
||||
size_t len = p2 - p1;
|
||||
|
||||
// Note: this requires a std::string to be backed with a
|
||||
// contiguous buffer. Officially, it's a only requirement since
|
||||
// C++11 (C++98 and C++03 didn't have this requirement), but all
|
||||
// major implementations had contiguous buffers anyway.
|
||||
std::string result(len, ' ');
|
||||
m_io->seekg(p1);
|
||||
m_io->read(&result[0], len);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::read_bytes_term(char term, bool include, bool consume, bool eos_error) {
|
||||
std::string result;
|
||||
std::getline(*m_io, result, term);
|
||||
if (m_io->eof()) {
|
||||
// encountered EOF
|
||||
if (eos_error) {
|
||||
throw std::runtime_error("read_bytes_term: encountered EOF");
|
||||
}
|
||||
} else {
|
||||
// encountered terminator
|
||||
if (include)
|
||||
result.push_back(term);
|
||||
if (!consume)
|
||||
m_io->unget();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::ensure_fixed_contents(std::string expected) {
|
||||
std::string actual = read_bytes(expected.length());
|
||||
|
||||
if (actual != expected) {
|
||||
// NOTE: I think printing it outright is not best idea, it could contain non-ascii charactes like backspace and beeps and whatnot. It would be better to print hexlified version, and also to redirect it to stderr.
|
||||
throw std::runtime_error("ensure_fixed_contents: actual data does not match expected data");
|
||||
}
|
||||
|
||||
return actual;
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::bytes_strip_right(std::string src, char pad_byte) {
|
||||
std::size_t new_len = src.length();
|
||||
|
||||
while (new_len > 0 && src[new_len - 1] == pad_byte)
|
||||
new_len--;
|
||||
|
||||
return src.substr(0, new_len);
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::bytes_terminate(std::string src, char term, bool include) {
|
||||
std::size_t new_len = 0;
|
||||
std::size_t max_len = src.length();
|
||||
|
||||
while (new_len < max_len && src[new_len] != term)
|
||||
new_len++;
|
||||
|
||||
if (include && new_len < max_len)
|
||||
new_len++;
|
||||
|
||||
return src.substr(0, new_len);
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Byte array processing
|
||||
// ========================================================================
|
||||
|
||||
std::string kaitai::kstream::process_xor_one(std::string data, uint8_t key) {
|
||||
size_t len = data.length();
|
||||
std::string result(len, ' ');
|
||||
|
||||
for (size_t i = 0; i < len; i++)
|
||||
result[i] = data[i] ^ key;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::process_xor_many(std::string data, std::string key) {
|
||||
size_t len = data.length();
|
||||
size_t kl = key.length();
|
||||
std::string result(len, ' ');
|
||||
|
||||
size_t ki = 0;
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
result[i] = data[i] ^ key[ki];
|
||||
ki++;
|
||||
if (ki >= kl)
|
||||
ki = 0;
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
std::string kaitai::kstream::process_rotate_left(std::string data, int amount) {
|
||||
size_t len = data.length();
|
||||
std::string result(len, ' ');
|
||||
|
||||
for (size_t i = 0; i < len; i++) {
|
||||
uint8_t bits = data[i];
|
||||
result[i] = (bits << amount) | (bits >> (8 - amount));
|
||||
}
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
#ifdef KS_ZLIB
|
||||
#include <zlib.h>
|
||||
|
||||
std::string kaitai::kstream::process_zlib(std::string data) {
|
||||
int ret;
|
||||
|
||||
unsigned char *src_ptr = reinterpret_cast<unsigned char*>(&data[0]);
|
||||
std::stringstream dst_strm;
|
||||
|
||||
z_stream strm;
|
||||
strm.zalloc = Z_NULL;
|
||||
strm.zfree = Z_NULL;
|
||||
strm.opaque = Z_NULL;
|
||||
|
||||
ret = inflateInit(&strm);
|
||||
if (ret != Z_OK)
|
||||
throw std::runtime_error("process_zlib: inflateInit error");
|
||||
|
||||
strm.next_in = src_ptr;
|
||||
strm.avail_in = data.length();
|
||||
|
||||
unsigned char outbuffer[ZLIB_BUF_SIZE];
|
||||
std::string outstring;
|
||||
|
||||
// get the decompressed bytes blockwise using repeated calls to inflate
|
||||
do {
|
||||
strm.next_out = reinterpret_cast<Bytef*>(outbuffer);
|
||||
strm.avail_out = sizeof(outbuffer);
|
||||
|
||||
ret = inflate(&strm, 0);
|
||||
|
||||
if (outstring.size() < strm.total_out)
|
||||
outstring.append(reinterpret_cast<char*>(outbuffer), strm.total_out - outstring.size());
|
||||
} while (ret == Z_OK);
|
||||
|
||||
if (ret != Z_STREAM_END) { // an error occurred that was not EOF
|
||||
std::ostringstream exc_msg;
|
||||
exc_msg << "process_zlib: error #" << ret << "): " << strm.msg;
|
||||
throw std::runtime_error(exc_msg.str());
|
||||
}
|
||||
|
||||
if (inflateEnd(&strm) != Z_OK)
|
||||
throw std::runtime_error("process_zlib: inflateEnd error");
|
||||
|
||||
return outstring;
|
||||
}
|
||||
#endif
|
||||
|
||||
// ========================================================================
|
||||
// Misc utility methods
|
||||
// ========================================================================
|
||||
|
||||
int kaitai::kstream::mod(int a, int b) {
|
||||
if (b <= 0)
|
||||
throw std::invalid_argument("mod: divisor b <= 0");
|
||||
int r = a % b;
|
||||
if (r < 0)
|
||||
r += b;
|
||||
return r;
|
||||
}
|
||||
|
||||
#include <stdio.h>
|
||||
std::string kaitai::kstream::to_string(int val) {
|
||||
// if int is 32 bits, "-2147483648" is the longest string representation
|
||||
// => 11 chars + zero => 12 chars
|
||||
// if int is 64 bits, "-9223372036854775808" is the longest
|
||||
// => 20 chars + zero => 21 chars
|
||||
char buf[25];
|
||||
int got_len = snprintf(buf, sizeof(buf), "%d", val);
|
||||
|
||||
// should never happen, but check nonetheless
|
||||
if (got_len > sizeof(buf))
|
||||
throw std::invalid_argument("to_string: integer is longer than string buffer");
|
||||
|
||||
return std::string(buf);
|
||||
}
|
||||
|
||||
#include <algorithm>
|
||||
std::string kaitai::kstream::reverse(std::string val) {
|
||||
std::reverse(val.begin(), val.end());
|
||||
|
||||
return val;
|
||||
}
|
||||
|
||||
uint8_t kaitai::kstream::byte_array_min(const std::string val) {
|
||||
uint8_t min = 0xff; // UINT8_MAX
|
||||
std::string::const_iterator end = val.end();
|
||||
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
|
||||
uint8_t cur = static_cast<uint8_t>(*it);
|
||||
if (cur < min) {
|
||||
min = cur;
|
||||
}
|
||||
}
|
||||
return min;
|
||||
}
|
||||
|
||||
uint8_t kaitai::kstream::byte_array_max(const std::string val) {
|
||||
uint8_t max = 0; // UINT8_MIN
|
||||
std::string::const_iterator end = val.end();
|
||||
for (std::string::const_iterator it = val.begin(); it != end; ++it) {
|
||||
uint8_t cur = static_cast<uint8_t>(*it);
|
||||
if (cur > max) {
|
||||
max = cur;
|
||||
}
|
||||
}
|
||||
return max;
|
||||
}
|
||||
|
||||
// ========================================================================
|
||||
// Other internal methods
|
||||
// ========================================================================
|
||||
|
||||
#ifndef KS_STR_DEFAULT_ENCODING
|
||||
#define KS_STR_DEFAULT_ENCODING "UTF-8"
|
||||
#endif
|
||||
|
||||
#ifdef KS_STR_ENCODING_ICONV
|
||||
|
||||
#include <iconv.h>
|
||||
#include <cerrno>
|
||||
#include <stdexcept>
|
||||
|
||||
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
|
||||
iconv_t cd = iconv_open(KS_STR_DEFAULT_ENCODING, src_enc.c_str());
|
||||
|
||||
if (cd == (iconv_t) -1) {
|
||||
if (errno == EINVAL) {
|
||||
throw std::runtime_error("bytes_to_str: invalid encoding pair conversion requested");
|
||||
} else {
|
||||
throw std::runtime_error("bytes_to_str: error opening iconv");
|
||||
}
|
||||
}
|
||||
|
||||
size_t src_len = src.length();
|
||||
size_t src_left = src_len;
|
||||
|
||||
// Start with a buffer length of double the source length.
|
||||
size_t dst_len = src_len * 2;
|
||||
std::string dst(dst_len, ' ');
|
||||
size_t dst_left = dst_len;
|
||||
|
||||
char *src_ptr = &src[0];
|
||||
char *dst_ptr = &dst[0];
|
||||
|
||||
while (true) {
|
||||
size_t res = iconv(cd, &src_ptr, &src_left, &dst_ptr, &dst_left);
|
||||
|
||||
if (res == (size_t) -1) {
|
||||
if (errno == E2BIG) {
|
||||
// dst buffer is not enough to accomodate whole string
|
||||
// enlarge the buffer and try again
|
||||
size_t dst_used = dst_len - dst_left;
|
||||
dst_left += dst_len;
|
||||
dst_len += dst_len;
|
||||
dst.resize(dst_len);
|
||||
|
||||
// dst.resize might have allocated destination buffer in another area
|
||||
// of memory, thus our previous pointer "dst" will be invalid; re-point
|
||||
// it using "dst_used".
|
||||
dst_ptr = &dst[dst_used];
|
||||
} else {
|
||||
throw std::runtime_error("bytes_to_str: iconv error");
|
||||
}
|
||||
} else {
|
||||
// conversion successful
|
||||
dst.resize(dst_len - dst_left);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (iconv_close(cd) != 0) {
|
||||
throw std::runtime_error("bytes_to_str: iconv close error");
|
||||
}
|
||||
|
||||
return dst;
|
||||
}
|
||||
#elif defined(KS_STR_ENCODING_NONE)
|
||||
std::string kaitai::kstream::bytes_to_str(std::string src, std::string src_enc) {
|
||||
return src;
|
||||
}
|
||||
#else
|
||||
#error Need to decide how to handle strings: please define one of: KS_STR_ENCODING_ICONV, KS_STR_ENCODING_NONE
|
||||
#endif
|
||||
Reference in New Issue
Block a user