diff --git a/phonelibs/acados/.gitignore b/phonelibs/acados/.gitignore index af1e27921e..90d32700b8 100644 --- a/phonelibs/acados/.gitignore +++ b/phonelibs/acados/.gitignore @@ -1 +1,4 @@ acados/ +!x86_64/ +!larch64/ +!aarch64/ diff --git a/phonelibs/acados/build.sh b/phonelibs/acados/build.sh index 2d5d05241b..ccc1031f0b 100755 --- a/phonelibs/acados/build.sh +++ b/phonelibs/acados/build.sh @@ -18,7 +18,7 @@ if [ ! -d acados/ ]; then fi cd acados git fetch -git checkout f63f0be563519a3df32463397f2bcd57b3958714 +git checkout 05bcbfe42818738c74572f27d06ad75a28d3b380 git submodule update --recursive --init # build diff --git a/phonelibs/acados/include/acados_c/ocp_nlp_interface.h b/phonelibs/acados/include/acados_c/ocp_nlp_interface.h index d4872f82e1..0f250a6628 100644 --- a/phonelibs/acados/include/acados_c/ocp_nlp_interface.h +++ b/phonelibs/acados/include/acados_c/ocp_nlp_interface.h @@ -227,8 +227,10 @@ int ocp_nlp_dynamics_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_n /// \param field The name of the field, either nls_res_jac, /// y_ref, W (others TBC) /// \param value Cost values. +int ocp_nlp_cost_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int start_stage, int end_stage, const char *field, void *value, int dim); int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, - int stage, const char *field, void *value); + int start_stage, const char *field, void *value); /// Sets the function pointers to the constraints functions for the given stage. @@ -239,6 +241,8 @@ int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_i /// \param stage Stage number. /// \param field The name of the field, either lb, ub (others TBC) /// \param value Constraints function or values. +int ocp_nlp_constraints_model_set_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, + int start_stage, int end_stage, const char *field, void *value, int dim); int ocp_nlp_constraints_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, int stage, const char *field, void *value); @@ -279,6 +283,9 @@ void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *ou void ocp_nlp_out_get(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, int stage, const char *field, void *value); +void ocp_nlp_out_get_slice(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out, + int start_stage, int end_stage, const char *field, void *value); + // void ocp_nlp_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver, int stage, const char *field, void *value); diff --git a/phonelibs/acados/larch64/lib/libacados.so b/phonelibs/acados/larch64/lib/libacados.so index fd1adffaa4..1f9dbe6f0a 100644 Binary files a/phonelibs/acados/larch64/lib/libacados.so and b/phonelibs/acados/larch64/lib/libacados.so differ diff --git a/phonelibs/acados/x86_64/lib/libacados.so b/phonelibs/acados/x86_64/lib/libacados.so index 30d3b71ef1..32d2498fe6 100644 Binary files a/phonelibs/acados/x86_64/lib/libacados.so and b/phonelibs/acados/x86_64/lib/libacados.so differ diff --git a/phonelibs/acados/x86_64/t_renderer b/phonelibs/acados/x86_64/t_renderer index e2b3f8b993..f2d22170a1 100755 Binary files a/phonelibs/acados/x86_64/t_renderer and b/phonelibs/acados/x86_64/t_renderer differ diff --git a/pyextra/acados_template/acados_layout.json b/pyextra/acados_template/acados_layout.json index 9458afb10c..f4e3323512 100644 --- a/pyextra/acados_template/acados_layout.json +++ b/pyextra/acados_template/acados_layout.json @@ -706,7 +706,10 @@ "int" ], "sim_method_jac_reuse": [ - "bool" + "ndarray", + [ + "N" + ] ], "qp_solver_cond_N": [ "int" diff --git a/pyextra/acados_template/acados_ocp.py b/pyextra/acados_template/acados_ocp.py index eec9b7e09f..9c2f02aa86 100644 --- a/pyextra/acados_template/acados_ocp.py +++ b/pyextra/acados_template/acados_ocp.py @@ -2123,7 +2123,7 @@ class AcadosOcpOptions: self.__sim_method_num_stages = 4 # number of stages in the integrator self.__sim_method_num_steps = 1 # number of steps in the integrator self.__sim_method_newton_iter = 3 # number of Newton iterations in simulation method - self.__sim_method_jac_reuse = False + self.__sim_method_jac_reuse = 0 self.__qp_solver_tol_stat = None # QP solver stationarity tolerance self.__qp_solver_tol_eq = None # QP solver equality tolerance self.__qp_solver_tol_ineq = None # QP solver inequality @@ -2172,7 +2172,7 @@ class AcadosOcpOptions: def integrator_type(self): """ Integrator type. - String in ('ERK', 'IRK', 'GNSF', 'DISCRETE'). + String in ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK'). Default: 'ERK'. """ return self.__integrator_type @@ -2252,8 +2252,9 @@ class AcadosOcpOptions: @property def sim_method_jac_reuse(self): """ - Boolean determining if jacobians are reused within integrator. - Default: False + Integer determining if jacobians are reused within integrator or ndarray of ints > 0 of shape (N,). + 0: False (no reuse); 1: True (reuse) + Default: 0 """ return self.__sim_method_jac_reuse @@ -2491,7 +2492,7 @@ class AcadosOcpOptions: @integrator_type.setter def integrator_type(self, integrator_type): - integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE') + integrator_types = ('ERK', 'IRK', 'GNSF', 'DISCRETE', 'LIFTED_IRK') if integrator_type in integrator_types: self.__integrator_type = integrator_type else: @@ -2557,10 +2558,10 @@ class AcadosOcpOptions: @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): - if sim_method_jac_reuse in (True, False): - self.__sim_method_jac_reuse = sim_method_jac_reuse - else: - raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') + # if sim_method_jac_reuse in (True, False): + self.__sim_method_jac_reuse = sim_method_jac_reuse + # else: + # raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') @nlp_solver_type.setter def nlp_solver_type(self, nlp_solver_type): diff --git a/pyextra/acados_template/acados_ocp_solver.py b/pyextra/acados_template/acados_ocp_solver.py index aa780e1a26..c27b0dbaf8 100644 --- a/pyextra/acados_template/acados_ocp_solver.py +++ b/pyextra/acados_template/acados_ocp_solver.py @@ -32,11 +32,13 @@ # POSSIBILITY OF SUCH DAMAGE.; # -import sys, os, json +import sys +import os +import json import numpy as np from datetime import datetime - -from ctypes import * +import ctypes +from ctypes import POINTER, cast, CDLL, c_void_p, c_char_p, c_double, c_int, c_int64, byref from copy import deepcopy @@ -88,9 +90,9 @@ def make_ocp_dims_consistent(acados_ocp): raise Exception('inconsistent dimension np, regarding model.p and parameter_values.' + \ f'\nGot np = {dims.np}, acados_ocp.parameter_values.shape = {acados_ocp.parameter_values.shape[0]}\n') - ## cost + # cost # initial stage - if not set, copy fields from path constraints - if cost.cost_type_0 == None: + if cost.cost_type_0 is None: cost.cost_type_0 = cost.cost_type cost.W_0 = cost.W cost.Vx_0 = cost.Vx @@ -162,7 +164,7 @@ def make_ocp_dims_consistent(acados_ocp): if cost.cost_type_e == 'LINEAR_LS': ny_e = cost.W_e.shape[0] if cost.Vx_e.shape[0] != ny_e: - raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ + raise Exception('inconsistent dimension ny_e: regarding W_e, cost_y_expr_e.' + \ f'\nGot W_e[{cost.W_e.shape}], Vx_e[{cost.Vx_e.shape}]') if cost.Vx_e.shape[1] != dims.nx and ny_e != 0: raise Exception('inconsistent dimension: Vx_e should have nx columns.') @@ -195,12 +197,12 @@ def make_ocp_dims_consistent(acados_ocp): dims.nbx_0 = constraints.lbx_0.size if all(constraints.lbx_0 == constraints.ubx_0) and dims.nbx_0 == dims.nx \ - and dims.nbxe_0 == None \ + and dims.nbxe_0 is None \ and (constraints.idxbxe_0.shape == constraints.idxbx_0.shape)\ - and all(constraints.idxbxe_0 == constraints.idxbx_0): + and all(constraints.idxbxe_0 == constraints.idxbx_0): # case: x0 was set: nbx0 are all equlities. dims.nbxe_0 = dims.nbx_0 - elif dims.nbxe_0 == None: + elif dims.nbxe_0 is None: # case: x0 was not set -> dont assume nbx0 to be equality constraints. dims.nbxe_0 = 0 @@ -468,6 +470,17 @@ def make_ocp_dims_consistent(acados_ocp): else: raise Exception("Wrong value for sim_method_num_stages. Should be either int or array of ints of shape (N,).") + # jac_reuse + if isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == 1: + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse.item() + + if isinstance(opts.sim_method_jac_reuse, (int, float)) and opts.sim_method_jac_reuse % 1 == 0: + opts.sim_method_jac_reuse = opts.sim_method_jac_reuse * np.ones((dims.N,), dtype=np.int64) + elif isinstance(opts.sim_method_jac_reuse, np.ndarray) and opts.sim_method_jac_reuse.size == dims.N \ + and np.all(np.equal(np.mod(opts.sim_method_jac_reuse, 1), 0)): + opts.sim_method_jac_reuse = np.reshape(opts.sim_method_jac_reuse, (dims.N,)).astype(np.int64) + else: + raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).") @@ -489,7 +502,8 @@ def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_n for acados_struct, v in ocp_layout.items(): # skip non dict attributes - if not isinstance(v, dict): continue + if not isinstance(v, dict): + continue # setattr(ocp_nlp, acados_struct, dict(getattr(acados_ocp, acados_struct).__dict__)) # Copy ocp object attributes dictionaries ocp_nlp_dict[acados_struct]=dict(getattr(acados_ocp, acados_struct).__dict__) @@ -529,10 +543,11 @@ def ocp_formulation_json_load(json_file='acados_ocp_nlp.json'): # load class dict acados_ocp.__dict__ = ocp_nlp_dict - # laod class attributes dict, dims, constraints, etc - for acados_struct, v in ocp_layout.items(): + # load class attributes dict, dims, constraints, etc + for acados_struct, v in ocp_layout.items(): # skip non dict attributes - if not isinstance(v, dict): continue + if not isinstance(v, dict): + continue acados_attribute = getattr(acados_ocp, acados_struct) acados_attribute.__dict__ = ocp_nlp_dict[acados_struct] setattr(acados_ocp, acados_struct, acados_attribute) @@ -561,6 +576,8 @@ def ocp_generate_external_functions(acados_ocp, model): elif acados_ocp.solver_options.integrator_type == 'IRK': # implicit model -- generate C code generate_c_code_implicit_ode(model, opts) + elif acados_ocp.solver_options.integrator_type == 'LIFTED_IRK': + generate_c_code_implicit_ode(model, opts) elif acados_ocp.solver_options.integrator_type == 'GNSF': generate_c_code_gnsf(model, opts) elif acados_ocp.solver_options.integrator_type == 'DISCRETE': @@ -665,7 +682,7 @@ def ocp_render_templates(acados_ocp, json_file): # constraints on outer function template_dir = f'{code_export_dir}/{name}_constraints/' in_file = 'phi_constraint.in.h' - out_file = f'{name}_phi_constraint.h' + out_file = f'{name}_phi_constraint.h' render_template(in_file, out_file, template_dir, json_path) # terminal constraints on convex over nonlinear function @@ -673,7 +690,7 @@ def ocp_render_templates(acados_ocp, json_file): # terminal constraints on outer function template_dir = f'{code_export_dir}/{name}_constraints/' in_file = 'phi_e_constraint.in.h' - out_file = f'{name}_phi_e_constraint.h' + out_file = f'{name}_phi_e_constraint.h' render_template(in_file, out_file, template_dir, json_path) # nonlinear constraints @@ -744,25 +761,21 @@ class AcadosOcpSolver: :param acados_ocp: type AcadosOcp - description of the OCP for acados :param json_file: name for the json file used to render the templated code - default: acados_ocp_nlp.json :param simulink_opts: Options to configure Simulink S-function blocks, mainly to activate possible Inputs and Outputs - :param build: Option to disable rendering templates and compiling if previously built - default: True """ if sys.platform=="win32": from ctypes import wintypes - dlclose = WinDLL('kernel32', use_last_error=True).FreeLibrary + dlclose = ctypes.WinDLL('kernel32', use_last_error=True).FreeLibrary dlclose.argtypes = [wintypes.HMODULE] else: dlclose = CDLL(None).dlclose dlclose.argtypes = [c_void_p] - def __init__(self, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True): - - self.solver_created = False - self.N = acados_ocp.dims.N + @classmethod + def generate(cls, acados_ocp, json_file='acados_ocp_nlp.json', simulink_opts=None, build=True): model = acados_ocp.model - if simulink_opts == None: - acados_path = get_acados_path() - json_path = os.path.join(acados_path, 'interfaces/acados_template/acados_template') + if simulink_opts is None: + json_path = os.path.dirname(os.path.realpath(__file__)) with open(json_path + '/simulink_default_opts.json', 'r') as f: simulink_opts = json.load(f) @@ -786,77 +799,79 @@ class AcadosOcpSolver: ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file) code_export_dir = acados_ocp.code_export_directory + # render templates + ocp_render_templates(acados_ocp, json_file) + if build: - # render templates - ocp_render_templates(acados_ocp, json_file) + ## Compile solver + cwd=os.getcwd() + os.chdir(code_export_dir) + os.system('make clean_ocp_shared_lib') + os.system('make ocp_shared_lib') + os.chdir(cwd) - ## Compile solver - cwd = os.getcwd() - os.chdir(code_export_dir) - os.system('make clean_ocp_shared_lib') - os.system('make ocp_shared_lib') - os.chdir(cwd) + def __init__(self, model_name, N, code_export_dir): + self.model_name = model_name + self.N = N - self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model.name}.so' + self.solver_created = False + self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{self.model_name}.so' # get shared_lib self.shared_lib = CDLL(self.shared_lib_name) # create capsule - getattr(self.shared_lib, f"{model.name}_acados_create_capsule").restype = c_void_p - self.capsule = getattr(self.shared_lib, f"{model.name}_acados_create_capsule")() + getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{self.model_name}_acados_create_capsule")() # create solver - getattr(self.shared_lib, f"{model.name}_acados_create").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_create").restype = c_int - assert getattr(self.shared_lib, f"{model.name}_acados_create")(self.capsule)==0 + getattr(self.shared_lib, f"{self.model_name}_acados_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_create").restype = c_int + assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0 self.solver_created = True # get pointers solver - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts").restype = c_void_p - self.nlp_opts = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_opts")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts").restype = c_void_p + self.nlp_opts = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_opts")(self.capsule) - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims").restype = c_void_p - self.nlp_dims = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_dims")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims").restype = c_void_p + self.nlp_dims = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_dims")(self.capsule) - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config").restype = c_void_p - self.nlp_config = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_config")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config").restype = c_void_p + self.nlp_config = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_config")(self.capsule) - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out").restype = c_void_p - self.nlp_out = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_out")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out").restype = c_void_p + self.nlp_out = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_out")(self.capsule) - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in").restype = c_void_p - self.nlp_in = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_in")(self.capsule) - - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver").restype = c_void_p - self.nlp_solver = getattr(self.shared_lib, f"{model.name}_acados_get_nlp_solver")(self.capsule) - - self.acados_ocp = acados_ocp + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in").restype = c_void_p + self.nlp_in = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_in")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver").restype = c_void_p + self.nlp_solver = getattr(self.shared_lib, f"{self.model_name}_acados_get_nlp_solver")(self.capsule) def solve(self): """ Solve the ocp with current input. """ - model = self.acados_ocp.model - getattr(self.shared_lib, f"{model.name}_acados_solve").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_solve").restype = c_int - status = getattr(self.shared_lib, f"{model.name}_acados_solve")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_solve").restype = c_int + status = getattr(self.shared_lib, f"{self.model_name}_acados_solve")(self.capsule) return status - def get(self, stage_, field_): + def get_slice(self, start_stage_, end_stage_, field_): """ Get the last solution of the solver: - :param stage: integer corresponding to shooting node + :param start_stage: integer corresponding to shooting node that indicates start of slice + :param end_stage: integer corresponding to shooting node that indicates end of slice :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] .. note:: regarding lam, t: \n @@ -870,50 +885,54 @@ class AcadosOcpSolver: sl: slack variables of soft lower inequality constraints \n su: slack variables of soft upper inequality constraints \n """ - out_fields = ['x', 'u', 'z', 'pi', 'lam', 't'] mem_fields = ['sl', 'su'] field = field_ field = field.encode('utf-8') if (field_ not in out_fields + mem_fields): - raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\ - \n Possible values are {}. Exiting.'.format(field_, out_fields + mem_fields)) + raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, out_fields)) - if not isinstance(stage_, int): - raise Exception('AcadosOcpSolver.get(): stage index must be Integer.') + if not isinstance(start_stage_, int): + raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') - if stage_ < 0 or stage_ > self.N: - raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N)) + if not isinstance(end_stage_, int): + raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') - if stage_ == self.N and field_ == 'pi': - raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\ - .format(field_, stage_)) + if start_stage_ >= end_stage_: + raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index') + if start_stage_ < 0 or end_stage_ > self.N + 1: + raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N)) self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p] self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage_, field) + self.nlp_dims, self.nlp_out, start_stage_, field) - out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64) + out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) if (field_ in out_fields): - self.shared_lib.ocp_nlp_out_get.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_out_get(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage_, field, out_data) + self.shared_lib.ocp_nlp_out_get_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \ + self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data) elif field_ in mem_fields: self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ - self.nlp_dims, self.nlp_solver, stage_, field, out_data) + self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data) return out + def get(self, stage_, field_): + return self.get_slice(stage_, stage_ + 1, field_)[0] + + def print_statistics(self): """ prints statistics of previous solver run as a table: @@ -965,7 +984,7 @@ class AcadosOcpSolver: :param overwrite: if false and filename exists add timestamp to filename """ if filename == '': - filename += self.acados_ocp.model.name + '_' + 'iterate' + '.json' + filename += self.model_name + '_' + 'iterate' + '.json' if not overwrite: # append timestamp @@ -1029,8 +1048,7 @@ class AcadosOcpSolver: 'qp_iter', # vector of QP iterations for last SQP call 'statistics', # table with info about last iteration 'stat_m', - 'stat_n', - ] + 'stat_n',] field = field_ field = field.encode('utf-8') @@ -1050,7 +1068,7 @@ class AcadosOcpSolver: min_size = min([stat_m, sqp_iter+1]) out = np.ascontiguousarray( - np.zeros( (stat_n[0]+1, min_size[0]) ), dtype=np.float64) + np.zeros((stat_n[0]+1, min_size[0])), dtype=np.float64) out_data = cast(out.ctypes.data, POINTER(c_double)) elif field_ == 'qp_iter': @@ -1122,12 +1140,13 @@ class AcadosOcpSolver: out_data = cast(out[3].ctypes.data, POINTER(c_double)) field = "res_comp".encode('utf-8') self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) - return out.flatten() # Note: this function should not be used anymore, better use cost_set, constraints_set + def set(self, stage_, field_, value_): + """ Set numerical data inside the solver. @@ -1155,8 +1174,6 @@ class AcadosOcpSolver: value_ = np.array([value_]) value_ = value_.astype(float) - model = self.acados_ocp.model - field = field_ field = field.encode('utf-8') @@ -1164,12 +1181,12 @@ class AcadosOcpSolver: # treat parameters separately if field_ == 'p': - getattr(self.shared_lib, f"{model.name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] - getattr(self.shared_lib, f"{model.name}_acados_update_params").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] + getattr(self.shared_lib, f"{self.model_name}_acados_update_params").restype = c_int value_data = cast(value_.ctypes.data, POINTER(c_double)) - assert getattr(self.shared_lib, f"{model.name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 + assert getattr(self.shared_lib, f"{self.model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 else: if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ @@ -1185,7 +1202,7 @@ class AcadosOcpSolver: if value_.shape[0] != dims: msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) - msg += 'with dimension {} (you have {})'.format(dims, value_.shape[0]) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape) raise Exception(msg) value_data = cast(value_.ctypes.data, POINTER(c_double)) @@ -1214,7 +1231,11 @@ class AcadosOcpSolver: return - def cost_set(self, stage_, field_, value_, api='warn'): + def cost_set(self, start_stage_, field_, value_, api='warn'): + self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') + return + + def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): """ Set numerical data in the cost module of the solver. @@ -1225,12 +1246,13 @@ class AcadosOcpSolver: # cast value_ to avoid conversion issues if isinstance(value_, (float, int)): value_ = np.array([value_]) - value_ = value_.astype(float) - + value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64) field = field_ field = field.encode('utf-8') + dim = np.product(value_.shape[1:]) - stage = c_int(stage_) + start_stage = c_int(start_stage_) + end_stage = c_int(end_stage_) self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int @@ -1239,13 +1261,14 @@ class AcadosOcpSolver: dims_data = cast(dims.ctypes.data, POINTER(c_int)) self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage_, field, dims_data) + self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) value_shape = value_.shape - if len(value_shape) == 1: - value_shape = (value_shape[0], 0) + expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) + if len(value_shape) == 2: + value_shape = (value_shape[0], value_shape[1], 0) - elif len(value_shape) == 2: + elif len(value_shape) == 3: if api=='old': pass elif api=='warn': @@ -1268,23 +1291,26 @@ class AcadosOcpSolver: else: raise Exception("Unknown api: '{}'".format(api)) - if value_shape != tuple(dims): + if value_shape != expected_shape: raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \ ' for field "{}" with dimension {} (you have {})'.format( \ - field_, tuple(dims), value_shape)) + field_, expected_shape, value_shape)) + value_data = cast(value_.ctypes.data, POINTER(c_double)) value_data_p = cast((value_data), c_void_p) - - self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] + self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \ + self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) return + def constraints_set(self, start_stage_, field_, value_, api='warn'): + self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') + return - def constraints_set(self, stage_, field_, value_, api='warn'): + def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): """ Set numerical data in the constraint module of the solver. @@ -1299,8 +1325,10 @@ class AcadosOcpSolver: field = field_ field = field.encode('utf-8') + dim = np.product(value_.shape[1:]) - stage = c_int(stage_) + start_stage = c_int(start_stage_) + end_stage = c_int(end_stage_) self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \ [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int @@ -1309,12 +1337,13 @@ class AcadosOcpSolver: dims_data = cast(dims.ctypes.data, POINTER(c_int)) self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ - self.nlp_dims, self.nlp_out, stage_, field, dims_data) + self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) value_shape = value_.shape - if len(value_shape) == 1: - value_shape = (value_shape[0], 0) - elif len(value_shape) == 2: + expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) + if len(value_shape) == 2: + value_shape = (value_shape[0], value_shape[1], 0) + elif len(value_shape) == 3: if api=='old': pass elif api=='warn': @@ -1336,18 +1365,18 @@ class AcadosOcpSolver: value_ = np.ravel(value_, order='F') else: raise Exception("Unknown api: '{}'".format(api)) - - if value_shape != tuple(dims): + + if value_shape != expected_shape: raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \ - ' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape)) + ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape)) value_data = cast(value_.ctypes.data, POINTER(c_double)) value_data_p = cast((value_data), c_void_p) - self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ - [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] - self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ - self.nlp_dims, self.nlp_in, stage, field, value_data_p) + self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] + self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \ + self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) return @@ -1448,21 +1477,18 @@ class AcadosOcpSolver: [c_void_p, c_void_p, c_char_p, c_void_p] self.shared_lib.ocp_nlp_solver_opts_set(self.nlp_config, \ self.nlp_opts, field, byref(value_ctypes)) - return def __del__(self): - model = self.acados_ocp.model - if self.solver_created: - getattr(self.shared_lib, f"{model.name}_acados_free").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_free").restype = c_int - getattr(self.shared_lib, f"{model.name}_acados_free")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_free").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_free").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_free")(self.capsule) - getattr(self.shared_lib, f"{model.name}_acados_free_capsule").argtypes = [c_void_p] - getattr(self.shared_lib, f"{model.name}_acados_free_capsule").restype = c_int - getattr(self.shared_lib, f"{model.name}_acados_free_capsule")(self.capsule) + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").argtypes = [c_void_p] + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule").restype = c_int + getattr(self.shared_lib, f"{self.model_name}_acados_free_capsule")(self.capsule) try: self.dlclose(self.shared_lib._handle) diff --git a/pyextra/acados_template/acados_ocp_solver_fast.py b/pyextra/acados_template/acados_ocp_solver_fast.py new file mode 100644 index 0000000000..656d288f1c --- /dev/null +++ b/pyextra/acados_template/acados_ocp_solver_fast.py @@ -0,0 +1,402 @@ +import sys +import os +import json +import numpy as np +from datetime import datetime + +from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p + +from copy import deepcopy + +from .generate_c_code_explicit_ode import generate_c_code_explicit_ode +from .generate_c_code_implicit_ode import generate_c_code_implicit_ode +from .generate_c_code_gnsf import generate_c_code_gnsf +from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics +from .generate_c_code_constraint import generate_c_code_constraint +from .generate_c_code_nls_cost import generate_c_code_nls_cost +from .generate_c_code_external_cost import generate_c_code_external_cost +from .acados_ocp import AcadosOcp +from .acados_model import acados_model_strip_casadi_symbolics +from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\ + format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ + set_up_imported_gnsf_model, get_acados_path + + +class AcadosOcpSolverFast: + dlclose = CDLL(None).dlclose + dlclose.argtypes = [c_void_p] + + def __init__(self, model_name, N, code_export_dir): + + self.solver_created = False + self.N = N + self.model_name = model_name + + self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so' + + # get shared_lib + self.shared_lib = CDLL(self.shared_lib_name) + + # create capsule + getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p + self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")() + + # create solver + getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int + assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0 + self.solver_created = True + + # get pointers solver + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p + self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p + self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p + self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p + self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p + self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule) + + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p + self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule) + + + def solve(self): + """ + Solve the ocp with current input. + """ + model_name = self.model_name + + getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p] + getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int + status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule) + return status + + def cost_set(self, start_stage_, field_, value_, api='warn'): + self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') + return + + def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): + """ + Set numerical data in the cost module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64) + field = field_ + field = field.encode('utf-8') + dim = np.product(value_.shape[1:]) + + start_stage = c_int(start_stage_) + end_stage = c_int(end_stage_) + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, + self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) + + value_shape = value_.shape + expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) + if len(value_shape) == 2: + value_shape = (value_shape[0], value_shape[1], 0) + + elif len(value_shape) == 3: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to cost_set to restore old incorrect behaviour\n" + + " * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + + if value_shape != expected_shape: + raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', + ' for field "{}" with dimension {} (you have {})'.format( + field_, expected_shape, value_shape)) + + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] + self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, + self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) + return + + def constraints_set(self, start_stage_, field_, value_, api='warn'): + self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') + return + + def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): + """ + Set numerical data in the constraint module of the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi'] + :param value: of appropriate size + """ + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + field = field_ + field = field.encode('utf-8') + dim = np.product(value_.shape[1:]) + + start_stage = c_int(start_stage_) + end_stage = c_int(end_stage_) + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int + + dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) + dims_data = cast(dims.ctypes.data, POINTER(c_int)) + + self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) + + value_shape = value_.shape + expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) + if len(value_shape) == 2: + value_shape = (value_shape[0], value_shape[1], 0) + elif len(value_shape) == 3: + if api=='old': + pass + elif api=='warn': + if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): + raise Exception("Ambiguity in API detected.\n" + "Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" + "Are you seeing this error suddenly in previously running code? Read on.\n" + " You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + + " acados_template now correctly passes on any matrices to acados in column major format.\n" + + " Two options to fix this error: \n" + + " * Add api='old' to constraints_set to restore old incorrect behaviour\n" + + " * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " + + "such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + + "If there is no such manipulation, then you have probably been getting an incorrect solution before.") + # Get elements in column major order + value_ = np.ravel(value_, order='F') + elif api=='new': + # Get elements in column major order + value_ = np.ravel(value_, order='F') + else: + raise Exception("Unknown api: '{}'".format(api)) + if value_shape != expected_shape: + raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \ + ' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape)) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] + self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \ + self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) + return + + # Note: this function should not be used anymore, better use cost_set, constraints_set + def set(self, stage_, field_, value_): + """ + Set numerical data inside the solver. + + :param stage: integer corresponding to shooting node + :param field: string in ['x', 'u', 'pi', 'lam', 't', 'p'] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + cost_fields = ['y_ref', 'yref'] + constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] + out_fields = ['x', 'u', 'pi', 'lam', 't', 'z'] + mem_fields = ['sl', 'su'] + + # cast value_ to avoid conversion issues + if isinstance(value_, (float, int)): + value_ = np.array([value_]) + value_ = value_.astype(float) + + model_name = self.model_name + + field = field_ + field = field.encode('utf-8') + + stage = c_int(stage_) + + # treat parameters separately + if field_ == 'p': + getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] + getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + + assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 + else: + if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: + raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ + \nPossible values are {}. Exiting.".format(field, \ + constraints_fields + cost_fields + out_fields + ['p'])) + + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage_, field) + + if value_.shape[0] != dims: + msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) + msg += 'with dimension {} (you have {})'.format(dims, value_.shape) + raise Exception(msg) + + value_data = cast(value_.ctypes.data, POINTER(c_double)) + value_data_p = cast((value_data), c_void_p) + + if field_ in constraints_fields: + self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in cost_fields: + self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ + self.nlp_dims, self.nlp_in, stage, field, value_data_p) + elif field_ in out_fields: + self.shared_lib.ocp_nlp_out_set.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_set(self.nlp_config, \ + self.nlp_dims, self.nlp_out, stage, field, value_data_p) + elif field_ in mem_fields: + self.shared_lib.ocp_nlp_set.argtypes = \ + [c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_set(self.nlp_config, \ + self.nlp_solver, stage, field, value_data_p) + return + + + def get_slice(self, start_stage_, end_stage_, field_): + """ + Get the last solution of the solver: + + :param start_stage: integer corresponding to shooting node that indicates start of slice + :param end_stage: integer corresponding to shooting node that indicates end of slice + :param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] + + .. note:: regarding lam, t: \n + the inequalities are internally organized in the following order: \n + [ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n + lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] + + .. note:: pi: multipliers for dynamics equality constraints \n + lam: multipliers for inequalities \n + t: slack variables corresponding to evaluation of all inequalities (at the solution) \n + sl: slack variables of soft lower inequality constraints \n + su: slack variables of soft upper inequality constraints \n + """ + out_fields = ['x', 'u', 'z', 'pi', 'lam', 't'] + mem_fields = ['sl', 'su'] + field = field_ + field = field.encode('utf-8') + + if (field_ not in out_fields + mem_fields): + raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\ + \n Possible values are {}. Exiting.'.format(field_, out_fields)) + + if not isinstance(start_stage_, int): + raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') + + if not isinstance(end_stage_, int): + raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') + + if start_stage_ >= end_stage_: + raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index') + + if start_stage_ < 0 or end_stage_ > self.N + 1: + raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N)) + self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p] + self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int + + dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ + self.nlp_dims, self.nlp_out, start_stage_, field) + + out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + if (field_ in out_fields): + self.shared_lib.ocp_nlp_out_get_slice.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \ + self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data) + elif field_ in mem_fields: + self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ + [c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] + self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ + self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data) + + return out + + def get_cost(self): + """ + Returns the cost value of the current solution. + """ + # compute cost internally + self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] + self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out) + + # create output array + out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64) + out_data = cast(out.ctypes.data, POINTER(c_double)) + + # call getter + self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] + + field = "cost_value".encode('utf-8') + self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) + + return out[0] diff --git a/pyextra/acados_template/acados_sim.py b/pyextra/acados_template/acados_sim.py index 9f2e19c5df..3ae1988651 100644 --- a/pyextra/acados_template/acados_sim.py +++ b/pyextra/acados_template/acados_sim.py @@ -117,7 +117,7 @@ class AcadosSimOpts: self.__sens_algebraic = False self.__sens_hess = False self.__output_z = False - self.__sim_method_jac_reuse = False + self.__sim_method_jac_reuse = 0 @property def integrator_type(self): @@ -166,7 +166,7 @@ class AcadosSimOpts: @property def sim_method_jac_reuse(self): - """Boolean determining if jacobians are reused. Default: False""" + """Integer determining if jacobians are reused (0 or 1). Default: 0""" return self.__sim_method_jac_reuse @property @@ -245,10 +245,10 @@ class AcadosSimOpts: @sim_method_jac_reuse.setter def sim_method_jac_reuse(self, sim_method_jac_reuse): - if sim_method_jac_reuse in (True, False): + if sim_method_jac_reuse in (0, 1): self.__sim_method_jac_reuse = sim_method_jac_reuse else: - raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be a Boolean.') + raise Exception('Invalid sim_method_jac_reuse value. sim_method_jac_reuse must be 0 or 1.') class AcadosSim: """ diff --git a/pyextra/acados_template/c_templates_tera/Makefile.in b/pyextra/acados_template/c_templates_tera/Makefile.in index 4f1899de4e..330a5aba11 100644 --- a/pyextra/acados_template/c_templates_tera/Makefile.in +++ b/pyextra/acados_template/c_templates_tera/Makefile.in @@ -153,6 +153,12 @@ MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_jac_x_xdot_u_z.o {%- if hessian_approx == "EXACT" %} MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o {%- endif %} +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun.o +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_fun_jac_x_xdot_u.o +{%- if hessian_approx == "EXACT" %} +MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_impl_dae_hess.o +{%- endif %} {%- elif solver_options.integrator_type == "GNSF" %} MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun.o MODEL_OBJ+= {{ model.name }}_model/{{ model.name }}_gnsf_phi_fun_jac_y.o @@ -287,6 +293,14 @@ CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c {%- if hessian_approx == "EXACT" %} CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c {%- endif %} +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c +# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_z.c +# CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_jac_x_xdot_u_z.c +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun_jac_x_xdot_u.c +{%- if hessian_approx == "EXACT" %} +CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_hess.c +{%- endif %} {%- elif solver_options.integrator_type == "GNSF" %} CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun.c CASADI_MODEL_SOURCE+= {{ model.name }}_gnsf_phi_fun_jac_y.c diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c index 615978d02f..390506420f 100644 --- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c @@ -81,6 +81,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) int nx = {{ dims.nx }}; int nu = {{ dims.nu }}; int nz = {{ dims.nz }}; + bool tmp_bool; {#// double Tsim = {{ solver_options.tf / dims.N }};#} double Tsim = {{ solver_options.Tsim }}; @@ -242,8 +243,6 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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); - bool tmp_bool = {{ solver_options.sim_method_jac_reuse }}; - sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "jac_reuse", &tmp_bool); {% if problem_class == "SIM" %} tmp_int = {{ solver_options.sim_method_num_stages }}; @@ -269,6 +268,8 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule) 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 diff --git a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h index 88a1787416..de332e8359 100644 --- a/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h +++ b/pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h @@ -88,7 +88,7 @@ sim_opts * {{ model.name }}_acados_get_sim_opts(sim_solver_capsule *capsule); sim_solver * {{ model.name }}_acados_get_sim_solver(sim_solver_capsule *capsule); -sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(); +sim_solver_capsule * {{ model.name }}_acados_sim_solver_create_capsule(void); int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule *capsule); #ifdef __cplusplus diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.c b/pyextra/acados_template/c_templates_tera/acados_solver.in.c index c79e826365..da73f6e29d 100644 --- a/pyextra/acados_template/c_templates_tera/acados_solver.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.c @@ -195,23 +195,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) /************************************************ * dimensions ************************************************/ - int nx[N+1]; - int nu[N+1]; - int nbx[N+1]; - int nbu[N+1]; - int nsbx[N+1]; - int nsbu[N+1]; - int nsg[N+1]; - int nsh[N+1]; - int nsphi[N+1]; - int ns[N+1]; - int ng[N+1]; - int nh[N+1]; - int nphi[N+1]; - int nz[N+1]; - int ny[N+1]; - int nr[N+1]; - int nbxe[N+1]; + #define NINTNP1MEMS 17 + int* intNp1mem = (int*)malloc( (N+1)*sizeof(int)*NINTNP1MEMS ); + + int* nx = intNp1mem + (N+1)*0; + int* nu = intNp1mem + (N+1)*1; + int* nbx = intNp1mem + (N+1)*2; + int* nbu = intNp1mem + (N+1)*3; + int* nsbx = intNp1mem + (N+1)*4; + int* nsbu = intNp1mem + (N+1)*5; + int* nsg = intNp1mem + (N+1)*6; + int* nsh = intNp1mem + (N+1)*7; + int* nsphi = intNp1mem + (N+1)*8; + int* ns = intNp1mem + (N+1)*9; + int* ng = intNp1mem + (N+1)*10; + int* nh = intNp1mem + (N+1)*11; + int* nphi = intNp1mem + (N+1)*12; + int* nz = intNp1mem + (N+1)*13; + int* ny = intNp1mem + (N+1)*14; + int* nr = intNp1mem + (N+1)*15; + int* nbxe = intNp1mem + (N+1)*16; for (int i = 0; i < N+1; i++) { @@ -227,7 +230,7 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) nbu[i] = NBU; nsbx[i] = NSBX; nsbu[i] = NSBU; - nsg[i] = NSG; + nsg[i] = NSG; nsh[i] = NSH; nsphi[i] = NSPHI; ng[i] = NG; @@ -317,6 +320,8 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_dims_set_cost(nlp_config, nlp_dims, N, "ny", &ny[N]); {%- endif %} + free(intNp1mem); + {% if solver_options.integrator_type == "GNSF" -%} // GNSF specific dimensions int gnsf_nx1 = {{ dims.gnsf_nx1 }}; @@ -517,6 +522,28 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) external_function_param_casadi_create(&capsule->impl_dae_hess[i], {{ dims.np }}); } {%- endif %} +{% elif solver_options.integrator_type == "LIFTED_IRK" %} + // external functions (implicit model) + capsule->impl_dae_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); + for (int i = 0; i < N; i++) { + capsule->impl_dae_fun[i].casadi_fun = &{{ model.name }}_impl_dae_fun; + capsule->impl_dae_fun[i].casadi_work = &{{ model.name }}_impl_dae_fun_work; + capsule->impl_dae_fun[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_sparsity_in; + capsule->impl_dae_fun[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out; + capsule->impl_dae_fun[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in; + capsule->impl_dae_fun[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out; + external_function_param_casadi_create(&capsule->impl_dae_fun[i], {{ dims.np }}); + } + + capsule->impl_dae_fun_jac_x_xdot_u = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); for (int i = 0; i < N; i++) { + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u; + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_work; + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in; + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out; + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in; + capsule->impl_dae_fun_jac_x_xdot_u[i].casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out; + external_function_param_casadi_create(&capsule->impl_dae_fun_jac_x_xdot_u[i], {{ dims.np }}); + } {% elif solver_options.integrator_type == "GNSF" %} capsule->gnsf_phi_fun = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi)*N); @@ -858,7 +885,27 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_in * nlp_in = ocp_nlp_in_create(nlp_config, nlp_dims); capsule->nlp_in = nlp_in; - double time_steps[N]; + // set up time_steps + {% set all_equal = true -%} + {%- set val = solver_options.time_steps[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.time_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + {%- if all_equal == true -%} + // all time_steps are identical + double time_step = {{ solver_options.time_steps[0] }}; + for (int i = 0; i < N; i++) + { + ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_step); + ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_step); + } + {%- else -%} + // time_steps are different + double* time_steps = malloc(N*sizeof(double)); {%- for j in range(end=dims.N) %} time_steps[{{ j }}] = {{ solver_options.time_steps[j] }}; {%- endfor %} @@ -868,6 +915,8 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &time_steps[i]); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &time_steps[i]); } + free(time_steps); + {%- endif %} /**** Dynamics ****/ for (int i = 0; i < N; i++) @@ -887,6 +936,10 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) {%- if solver_options.hessian_approx == "EXACT" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_hess", &capsule->impl_dae_hess[i]); {%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "impl_dae_fun", &capsule->impl_dae_fun[i]); + ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, + "impl_dae_fun_jac_x_xdot_u", &capsule->impl_dae_fun_jac_x_xdot_u[i]); {% elif solver_options.integrator_type == "GNSF" %} ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun", &capsule->gnsf_phi_fun[i]); ocp_nlp_dynamics_model_set(nlp_config, nlp_dims, nlp_in, i, "phi_fun_jac_y", &capsule->gnsf_phi_fun_jac_y[i]); @@ -910,34 +963,48 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) /**** Cost ****/ {%- if cost.cost_type_0 == "NONLINEAR_LS" or cost.cost_type_0 == "LINEAR_LS" %} {% if dims.ny_0 > 0 %} - double W_0[NY0*NY0]; - {% for j in range(end=dims.ny_0) %} + double* W_0 = calloc(NY0*NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.ny_0) %} + {%- if cost.W_0[j][k] != 0 %} W_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.W_0[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "W", W_0); + free(W_0); - double yref_0[NY0]; - {% for j in range(end=dims.ny_0) %} + double* yref_0 = calloc(NY0, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny_0) %} + {%- if cost.yref_0[j] != 0 %} yref_0[{{ j }}] = {{ cost.yref_0[j] }}; + {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "yref", yref_0); + free(yref_0); {% endif %} {% endif %} {%- if cost.cost_type == "NONLINEAR_LS" or cost.cost_type == "LINEAR_LS" %} {% if dims.ny > 0 %} - double W[NY*NY]; + double* W = calloc(NY*NY, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.ny) %} + {%- if cost.W[j][k] != 0 %} W[{{ j }}+(NY) * {{ k }}] = {{ cost.W[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} - double yref[NY]; - {% for j in range(end=dims.ny) %} + double* yref = calloc(NY, sizeof(double)); + // change only the non-zero elements: + {%- for j in range(end=dims.ny) %} + {%- if cost.yref[j] != 0 %} yref[{{ j }}] = {{ cost.yref[j] }}; + {%- endif %} {%- endfor %} for (int i = 1; i < N; i++) @@ -945,56 +1012,77 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "W", W); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", yref); } + free(W); + free(yref); {% endif %} {% endif %} {%- if cost.cost_type_0 == "LINEAR_LS" %} - double Vx_0[NY0*NX]; + double* Vx_0 = calloc(NY0*NX, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_0[j][k] != 0 %} Vx_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vx_0[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vx", Vx_0); + free(Vx_0); {% if dims.ny_0 > 0 and dims.nu > 0 %} - double Vu_0[NY0*NU]; + double* Vu_0 = calloc(NY0*NU, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nu) %} + {%- if cost.Vu_0[j][k] != 0 %} Vu_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vu_0[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vu", Vu_0); + free(Vu_0); {% endif %} {% if dims.ny_0 > 0 and dims.nz > 0 %} - double Vz_0[NY0*NZ]; + double* Vz_0 = calloc(NY0*NZ, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_0) %} {%- for k in range(end=dims.nz) %} + {%- if cost.Vz_0[j][k] != 0 %} Vz_0[{{ j }}+(NY0) * {{ k }}] = {{ cost.Vz_0[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, 0, "Vz", Vz_0); + free(Vz_0); {%- endif %} {%- endif %}{# LINEAR LS #} {%- if cost.cost_type == "LINEAR_LS" %} - double Vx[NY*NX]; + double* Vx = calloc(NY*NX, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.nx) %} + {%- if cost.Vx[j][k] != 0 %} Vx[{{ j }}+(NY) * {{ k }}] = {{ cost.Vx[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} for (int i = 1; i < N; i++) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vx", Vx); } + free(Vx); {% if dims.ny > 0 and dims.nu > 0 %} - double Vu[NY*NU]; + double* Vu = calloc(NY*NU, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.nu) %} + {%- if cost.Vu[j][k] != 0 %} Vu[{{ j }}+(NY) * {{ k }}] = {{ cost.Vu[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} @@ -1002,13 +1090,17 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vu", Vu); } + free(Vu); {% endif %} {% if dims.ny > 0 and dims.nz > 0 %} - double Vz[NY*NZ]; + double* Vz = calloc(NY*NZ, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny) %} {%- for k in range(end=dims.nz) %} + {%- if cost.Vz[j][k] != 0 %} Vz[{{ j }}+(NY) * {{ k }}] = {{ cost.Vz[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} @@ -1016,6 +1108,7 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) { ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "Vz", Vz); } + free(Vz); {%- endif %} {%- endif %}{# LINEAR LS #} @@ -1048,24 +1141,34 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) {% if dims.ns > 0 %} - double Zl[NS]; - double Zu[NS]; - double zl[NS]; - double zu[NS]; - {% for j in range(end=dims.ns) %} + double* zlumem = calloc(4*NS, sizeof(double)); + double* Zl = zlumem+NS*0; + double* Zu = zlumem+NS*1; + double* zl = zlumem+NS*2; + double* zu = zlumem+NS*3; + // change only the non-zero elements: + {%- for j in range(end=dims.ns) %} + {%- if cost.Zl[j] != 0 %} Zl[{{ j }}] = {{ cost.Zl[j] }}; + {%- endif %} {%- endfor %} - {% for j in range(end=dims.ns) %} + {%- for j in range(end=dims.ns) %} + {%- if cost.Zu[j] != 0 %} Zu[{{ j }}] = {{ cost.Zu[j] }}; + {%- endif %} {%- endfor %} - {% for j in range(end=dims.ns) %} + {%- for j in range(end=dims.ns) %} + {%- if cost.zl[j] != 0 %} zl[{{ j }}] = {{ cost.zl[j] }}; + {%- endif %} {%- endfor %} - {% for j in range(end=dims.ns) %} + {%- for j in range(end=dims.ns) %} + {%- if cost.zu[j] != 0 %} zu[{{ j }}] = {{ cost.zu[j] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1075,33 +1178,46 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zl", zl); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "zu", zu); } + free(zlumem); {% endif %} // terminal cost {% if cost.cost_type_e == "LINEAR_LS" or cost.cost_type_e == "NONLINEAR_LS" %} {% if dims.ny_e > 0 %} - double yref_e[NYN]; + double* yref_e = calloc(NYN, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_e) %} + {%- if cost.yref_e[j] != 0 %} yref_e[{{ j }}] = {{ cost.yref_e[j] }}; + {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", yref_e); + free(yref_e); - double W_e[NYN*NYN]; + double* W_e = calloc(NYN*NYN, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_e) %} {%- for k in range(end=dims.ny_e) %} + {%- if cost.W_e[j][k] != 0 %} W_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.W_e[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "W", W_e); + free(W_e); {%- if cost.cost_type_e == "LINEAR_LS" %} - double Vx_e[NYN*NX]; + double* Vx_e = calloc(NYN*NX, sizeof(double)); + // change only the non-zero elements: {% for j in range(end=dims.ny_e) %} {%- for k in range(end=dims.nx) %} + {%- if cost.Vx_e[j][k] != 0 %} Vx_e[{{ j }}+(NYN) * {{ k }}] = {{ cost.Vx_e[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Vx", Vx_e); + free(Vx_e); {%- endif %} {%- if cost.cost_type_e == "NONLINEAR_LS" %} @@ -1118,31 +1234,42 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) {%- endif %} {% if dims.ns_e > 0 %} - double Zl_e[NSN]; - double Zu_e[NSN]; - double zl_e[NSN]; - double zu_e[NSN]; + double* zluemem = calloc(4*NSN, sizeof(double)); + double* Zl_e = zluemem+NSN*0; + double* Zu_e = zluemem+NSN*1; + double* zl_e = zluemem+NSN*2; + double* zu_e = zluemem+NSN*3; + // change only the non-zero elements: {% for j in range(end=dims.ns_e) %} + {%- if cost.Zl_e[j] != 0 %} Zl_e[{{ j }}] = {{ cost.Zl_e[j] }}; + {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} + {%- if cost.Zu_e[j] != 0 %} Zu_e[{{ j }}] = {{ cost.Zu_e[j] }}; + {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} + {%- if cost.zl_e[j] != 0 %} zl_e[{{ j }}] = {{ cost.zl_e[j] }}; + {%- endif %} {%- endfor %} {% for j in range(end=dims.ns_e) %} + {%- if cost.zu_e[j] != 0 %} zu_e[{{ j }}] = {{ cost.zu_e[j] }}; + {%- endif %} {%- endfor %} ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zl", Zl_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "Zu", Zu_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zl", zl_e); ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "zu", zu_e); + free(zluemem); {%- endif %} /**** Constraints ****/ @@ -1150,29 +1277,38 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) // bounds for initial stage {% if dims.nbx_0 > 0 %} // x0 - int idxbx0[{{ dims.nbx_0 }}]; - {% for i in range(end=dims.nbx_0) %} + int* idxbx0 = malloc(NBX0 * sizeof(int)); + {%- 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) %} + double* lubx0 = calloc(2*NBX0, sizeof(double)); + double* lbx0 = lubx0; + double* ubx0 = lubx0 + NBX0; + // change only the non-zero elements: + {%- for i in range(end=dims.nbx_0) %} + {%- if constraints.lbx_0[i] != 0 %} lbx0[{{ i }}] = {{ constraints.lbx_0[i] }}; + {%- endif %} + {%- if constraints.ubx_0[i] != 0 %} ubx0[{{ i }}] = {{ constraints.ubx_0[i] }}; + {%- endif %} {%- 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); + free(idxbx0); + free(lubx0); {% endif %} {% if dims.nbxe_0 > 0 %} // idxbxe_0 - int idxbxe_0[{{ dims.nbxe_0 }}]; + int* idxbxe_0 = malloc({{ dims.nbxe_0 }} * sizeof(int)); {% for i in range(end=dims.nbxe_0) %} idxbxe_0[{{ i }}] = {{ constraints.idxbxe_0[i] }}; {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "idxbxe", idxbxe_0); + free(idxbxe_0); {% endif %} /* constraints that are the same for initial and intermediate */ @@ -1183,15 +1319,21 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) // ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "usbx", usbx); // soft bounds on x - int idxsbx[NSBX]; + int* idxsbx = malloc(NSBX * sizeof(int)); {% for i in range(end=dims.nsbx) %} idxsbx[{{ i }}] = {{ constraints.idxsbx[i] }}; {%- endfor %} - double lsbx[NSBX]; - double usbx[NSBX]; + + double* lusbx = calloc(2*NSBX, sizeof(double)); + double* lsbx = lusbx; + double* usbx = lusbx + NSBX; {% for i in range(end=dims.nsbx) %} + {%- if constraints.lsbx[i] != 0 %} lsbx[{{ i }}] = {{ constraints.lsbx[i] }}; + {%- endif %} + {%- if constraints.usbx[i] != 0 %} usbx[{{ i }}] = {{ constraints.usbx[i] }}; + {%- endif %} {%- endfor %} for (int i = 1; i < N; i++) @@ -1200,20 +1342,27 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbx", lsbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbx", usbx); } + free(idxsbx); + free(lusbx); {%- endif %} {% if dims.nbu > 0 %} // u - int idxbu[NBU]; + int* idxbu = malloc(NBU * sizeof(int)); {% for i in range(end=dims.nbu) %} idxbu[{{ i }}] = {{ constraints.idxbu[i] }}; {%- endfor %} - double lbu[NBU]; - double ubu[NBU]; + double* lubu = calloc(2*NBU, sizeof(double)); + double* lbu = lubu; + double* ubu = lubu + NBU; {% for i in range(end=dims.nbu) %} + {%- if constraints.lbu[i] != 0 %} lbu[{{ i }}] = {{ constraints.lbu[i] }}; + {%- endif %} + {%- if constraints.ubu[i] != 0 %} ubu[{{ i }}] = {{ constraints.ubu[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1222,19 +1371,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbu", lbu); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubu", ubu); } + free(idxbu); + free(lubu); {% endif %} {% if dims.nsbu > 0 %} // set up soft bounds for u - int idxsbu[NSBU]; + int* idxsbu = malloc(NSBU * sizeof(int)); {% for i in range(end=dims.nsbu) %} idxsbu[{{ i }}] = {{ constraints.idxsbu[i] }}; {%- endfor %} - double lsbu[NSBU]; - double usbu[NSBU]; + double* lusbu = calloc(2*NSBU, sizeof(double)); + double* lsbu = lusbu; + double* usbu = lusbu + NSBU; {% for i in range(end=dims.nsbu) %} + {%- if constraints.lsbu[i] != 0 %} lsbu[{{ i }}] = {{ constraints.lsbu[i] }}; + {%- endif %} + {%- if constraints.usbu[i] != 0 %} usbu[{{ i }}] = {{ constraints.usbu[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) { @@ -1242,19 +1398,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsbu", lsbu); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usbu", usbu); } + free(idxsbu); + free(lusbu); {% endif %} {% if dims.nsg > 0 %} // set up soft bounds for general linear constraints - int idxsg[NSG]; + int* idxsg = malloc(NSG * sizeof(int)); {% for i in range(end=dims.nsg) %} idxsg[{{ i }}] = {{ constraints.idxsg[i] }}; {%- endfor %} - double lsg[NSG]; - double usg[NSG]; + double* lusg = calloc(2*NSG, sizeof(double)); + double* lsg = lusg; + double* usg = lusg + NSG; {% for i in range(end=dims.nsg) %} + {%- if constraints.lsg[i] != 0 %} lsg[{{ i }}] = {{ constraints.lsg[i] }}; + {%- endif %} + {%- if constraints.usg[i] != 0 %} usg[{{ i }}] = {{ constraints.usg[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1263,19 +1426,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsg", lsg); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usg", usg); } + free(idxsg); + free(lusg); {% endif %} {% if dims.nsh > 0 %} // set up soft bounds for nonlinear constraints - int idxsh[NSH]; + int* idxsh = malloc(NSH * sizeof(int)); {% for i in range(end=dims.nsh) %} idxsh[{{ i }}] = {{ constraints.idxsh[i] }}; {%- endfor %} - double lsh[NSH]; - double ush[NSH]; + double* lush = calloc(2*NSH, sizeof(double)); + double* lsh = lush; + double* ush = lush + NSH; {% for i in range(end=dims.nsh) %} + {%- if constraints.lsh[i] != 0 %} lsh[{{ i }}] = {{ constraints.lsh[i] }}; + {%- endif %} + {%- if constraints.ush[i] != 0 %} ush[{{ i }}] = {{ constraints.ush[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1284,19 +1454,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsh", lsh); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ush", ush); } + free(idxsh); + free(lush); {% endif %} {% if dims.nsphi > 0 %} // set up soft bounds for convex-over-nonlinear constraints - int idxsphi[NSPHI]; + int* idxsphi = malloc(NSPHI * sizeof(int)); {% for i in range(end=dims.nsphi) %} idxsphi[{{ i }}] = {{ constraints.idxsphi[i] }}; {%- endfor %} - double lsphi[NSPHI]; - double usphi[NSPHI]; + double* lusphi = calloc(2*NSPHI, sizeof(double)); + double* lsphi = lusphi; + double* usphi = lusphi + NSPHI; {% for i in range(end=dims.nsphi) %} + {%- if constraints.lsphi[i] != 0 %} lsphi[{{ i }}] = {{ constraints.lsphi[i] }}; + {%- endif %} + {%- if constraints.usphi[i] != 0 %} usphi[{{ i }}] = {{ constraints.usphi[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1305,19 +1482,26 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lsphi", lsphi); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "usphi", usphi); } + free(idxsphi); + free(lusphi); {% endif %} {% if dims.nbx > 0 %} // x - int idxbx[NBX]; + int* idxbx = malloc(NBX * sizeof(int)); {% for i in range(end=dims.nbx) %} idxbx[{{ i }}] = {{ constraints.idxbx[i] }}; {%- endfor %} - double lbx[NBX]; - double ubx[NBX]; + double* lubx = calloc(2*NBX, sizeof(double)); + double* lbx = lubx; + double* ubx = lubx + NBX; {% for i in range(end=dims.nbx) %} + {%- if constraints.lbx[i] != 0 %} lbx[{{ i }}] = {{ constraints.lbx[i] }}; + {%- endif %} + {%- if constraints.ubx[i] != 0 %} ubx[{{ i }}] = {{ constraints.ubx[i] }}; + {%- endif %} {%- endfor %} for (int i = 1; i < N; i++) @@ -1326,33 +1510,44 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lbx", lbx); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ubx", ubx); } + free(idxbx); + free(lubx); {% endif %} {% if dims.ng > 0 %} // set up general constraints for stage 0 to N-1 - double D[NG*NU]; - double C[NG*NX]; - double lg[NG]; - double ug[NG]; - - {% for j in range(end=dims.ng) %} - {%- for k in range(end=dims.nu) %} + double* D = calloc(NG*NU, sizeof(double)); + double* C = calloc(NG*NX, sizeof(double)); + double* lug = calloc(2*NG, sizeof(double)); + double* lg = lug; + double* ug = lug + NG; + + {% for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nu) %} + {%- if constraints.D[j][k] != 0 %} D[{{ j }}+NG * {{ k }}] = {{ constraints.D[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} - {% for j in range(end=dims.ng) %} - {%- for k in range(end=dims.nx) %} + {% for j in range(end=dims.ng) -%} + {% for k in range(end=dims.nx) %} + {%- if constraints.C[j][k] != 0 %} C[{{ j }}+NG * {{ k }}] = {{ constraints.C[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} {% for i in range(end=dims.ng) %} + {%- if constraints.lg[i] != 0 %} lg[{{ i }}] = {{ constraints.lg[i] }}; + {%- endif %} {%- endfor %} {% for i in range(end=dims.ng) %} + {%- if constraints.ug[i] != 0 %} ug[{{ i }}] = {{ constraints.ug[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1362,19 +1557,27 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lg", lg); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "ug", ug); } + free(D); + free(C); + free(lug); {% endif %} {% if dims.nh > 0 %} - // set up nonlinear constraints for stage 0 to N-1 - double lh[NH]; - double uh[NH]; + // set up nonlinear constraints for stage 0 to N-1 + double* luh = calloc(2*NH, sizeof(double)); + double* lh = luh; + double* uh = luh + NH; {% for i in range(end=dims.nh) %} + {%- if constraints.lh[i] != 0 %} lh[{{ i }}] = {{ constraints.lh[i] }}; + {%- endif %} {%- endfor %} {% for i in range(end=dims.nh) %} + {%- if constraints.uh[i] != 0 %} uh[{{ i }}] = {{ constraints.uh[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1391,19 +1594,24 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lh", lh); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uh", uh); } + free(luh); {% endif %} {% if dims.nphi > 0 and constraints.constr_type == "BGP" %} - // set up convex-over-nonlinear constraints for stage 0 to N-1 - double lphi[NPHI]; - double uphi[NPHI]; - + // set up convex-over-nonlinear constraints for stage 0 to N-1 + double* luphi = calloc(2*NPHI, sizeof(double)); + double* lphi = luphi; + double* uphi = luphi + NPHI; {% for i in range(end=dims.nphi) %} + {%- if constraints.lphi[i] != 0 %} lphi[{{ i }}] = {{ constraints.lphi[i] }}; + {%- endif %} {%- endfor %} {% for i in range(end=dims.nphi) %} + {%- if constraints.uphi[i] != 0 %} uphi[{{ i }}] = {{ constraints.uphi[i] }}; + {%- endif %} {%- endfor %} for (int i = 0; i < N; i++) @@ -1413,132 +1621,181 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "lphi", lphi); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, i, "uphi", uphi); } + free(luphi); {% endif %} /* terminal constraints */ {% if dims.nbx_e > 0 %} // set up bounds for last stage // x - int idxbx_e[NBXN]; + int* idxbx_e = malloc(NBXN * sizeof(int)); {% for i in range(end=dims.nbx_e) %} idxbx_e[{{ i }}] = {{ constraints.idxbx_e[i] }}; {%- endfor %} - double lbx_e[NBXN]; - double ubx_e[NBXN]; + double* lubx_e = calloc(2*NBXN, sizeof(double)); + double* lbx_e = lubx_e; + double* ubx_e = lubx_e + NBXN; {% for i in range(end=dims.nbx_e) %} + {%- if constraints.lbx_e[i] != 0 %} lbx_e[{{ i }}] = {{ constraints.lbx_e[i] }}; + {%- endif %} + {%- if constraints.ubx_e[i] != 0 %} ubx_e[{{ i }}] = {{ constraints.ubx_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxbx", idxbx_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lbx", lbx_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ubx", ubx_e); + free(idxbx_e); + free(lubx_e); {%- endif %} {% if dims.nsg_e > 0 %} // set up soft bounds for general linear constraints - int idxsg_e[NSGN]; + int* idxsg_e = calloc(NSGN, sizeof(int)); {% for i in range(end=dims.nsg_e) %} idxsg_e[{{ i }}] = {{ constraints.idxsg_e[i] }}; {%- endfor %} - double lsg_e[NSGN]; - double usg_e[NSGN]; + double* lusg_e = calloc(2*NSGN, sizeof(double)); + double* lsg_e = lusg_e; + double* usg_e = lusg_e + NSGN; {% for i in range(end=dims.nsg_e) %} + {%- if constraints.lsg_e[i] != 0 %} lsg_e[{{ i }}] = {{ constraints.lsg_e[i] }}; + {%- endif %} + {%- if constraints.usg_e[i] != 0 %} usg_e[{{ i }}] = {{ constraints.usg_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsg", idxsg_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsg", lsg_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usg", usg_e); + free(idxsg_e); + free(lusg_e); {%- endif %} {% if dims.nsh_e > 0 %} // set up soft bounds for nonlinear constraints - int idxsh_e[NSHN]; + int* idxsh_e = malloc(NSHN * sizeof(int)); {% for i in range(end=dims.nsh_e) %} idxsh_e[{{ i }}] = {{ constraints.idxsh_e[i] }}; {%- endfor %} - double lsh_e[NSHN]; - double ush_e[NSHN]; + double* lush_e = calloc(2*NSHN, sizeof(double)); + double* lsh_e = lush_e; + double* ush_e = lush_e + NSHN; {% for i in range(end=dims.nsh_e) %} + {%- if constraints.lsh_e[i] != 0 %} lsh_e[{{ i }}] = {{ constraints.lsh_e[i] }}; + {%- endif %} + {%- if constraints.ush_e[i] != 0 %} ush_e[{{ i }}] = {{ constraints.ush_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsh", idxsh_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsh", lsh_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ush", ush_e); + free(idxsh_e); + free(lush_e); {%- endif %} {% if dims.nsphi_e > 0 %} // set up soft bounds for convex-over-nonlinear constraints - int idxsphi_e[NSPHIN]; + int* idxsphi_e = malloc(NSPHIN * sizeof(int)); {% for i in range(end=dims.nsphi_e) %} idxsphi_e[{{ i }}] = {{ constraints.idxsphi_e[i] }}; {%- endfor %} - double lsphi_e[NSPHIN]; - double usphi_e[NSPHIN]; + double* lusphi_e = calloc(2*NSPHIN, sizeof(double)); + double* lsphi_e = lusphi_e; + double* usphi_e = lusphi_e + NSPHIN; {% for i in range(end=dims.nsphi_e) %} + {%- if constraints.lsphi_e[i] != 0 %} lsphi_e[{{ i }}] = {{ constraints.lsphi_e[i] }}; + {%- endif %} + {%- if constraints.usphi_e[i] != 0 %} usphi_e[{{ i }}] = {{ constraints.usphi_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsphi", idxsphi_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsphi", lsphi_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usphi", usphi_e); + free(idxsphi_e); + free(lusphi_e); {%- endif %} {% if dims.nsbx_e > 0 %} // soft bounds on x - int idxsbx_e[NSBXN]; + int* idxsbx_e = malloc(NSBXN * sizeof(int)); {% for i in range(end=dims.nsbx_e) %} idxsbx_e[{{ i }}] = {{ constraints.idxsbx_e[i] }}; {%- endfor %} - double lsbx_e[NSBXN]; - double usbx_e[NSBXN]; + double* lusbx_e = calloc(2*NSBXN, sizeof(double)); + double* lsbx_e = lusbx_e; + double* usbx_e = lusbx_e + NSBXN; {% for i in range(end=dims.nsbx_e) %} + {%- if constraints.lsbx_e[i] != 0 %} lsbx_e[{{ i }}] = {{ constraints.lsbx_e[i] }}; + {%- endif %} + {%- if constraints.usbx_e[i] != 0 %} usbx_e[{{ i }}] = {{ constraints.usbx_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "idxsbx", idxsbx_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lsbx", lsbx_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "usbx", usbx_e); + free(idxsbx_e); + free(lusbx_e); {% endif %} {% if dims.ng_e > 0 %} // set up general constraints for last stage - double C_e[NGN*NX]; - double lg_e[NGN]; - double ug_e[NGN]; + double* C_e = calloc(NGN*NX, sizeof(double)); + double* lug_e = calloc(2*NGN, sizeof(double)); + double* lg_e = lug_e; + double* ug_e = lug_e + NGN; {% for j in range(end=dims.ng) %} {%- for k in range(end=dims.nx) %} + {%- if constraints.C_e[j][k] != 0 %} C_e[{{ j }}+NG * {{ k }}] = {{ constraints.C_e[j][k] }}; + {%- endif %} {%- endfor %} {%- endfor %} {% for i in range(end=dims.ng_e) %} + {%- if constraints.lg_e[i] != 0 %} lg_e[{{ i }}] = {{ constraints.lg_e[i] }}; + {%- endif %} + {%- if constraints.ug_e[i] != 0 %} ug_e[{{ i }}] = {{ constraints.ug_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "C", C_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lg", lg_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "ug", ug_e); + free(C_e); + free(lug_e); {%- endif %} {% if dims.nh_e > 0 %} - // set up nonlinear constraints for last stage - double lh_e[NHN]; - double uh_e[NHN]; - + // set up nonlinear constraints for last stage + double* luh_e = calloc(2*NHN, sizeof(double)); + double* lh_e = luh_e; + double* uh_e = luh_e + NHN; {% for i in range(end=dims.nh_e) %} + {%- if constraints.lh_e[i] != 0 %} lh_e[{{ i }}] = {{ constraints.lh_e[i] }}; + {%- endif %} {%- endfor %} {% for i in range(end=dims.nh_e) %} + {%- if constraints.uh_e[i] != 0 %} uh_e[{{ i }}] = {{ constraints.uh_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_h_fun_jac", &capsule->nl_constr_h_e_fun_jac); @@ -1549,25 +1806,28 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) {% endif %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lh", lh_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uh", uh_e); + free(luh_e); {%- endif %} {% if dims.nphi_e > 0 and constraints.constr_type_e == "BGP" %} // set up convex-over-nonlinear constraints for last stage - double lphi_e[NPHIN]; - double uphi_e[NPHIN]; - + double* luphi_e = calloc(2*NPHIN, sizeof(double)); + double* lphi_e = luphi_e; + double* uphi_e = luphi_e + NPHIN; {% for i in range(end=dims.nphi_e) %} + {%- if constraints.lphi_e[i] != 0 %} lphi_e[{{ i }}] = {{ constraints.lphi_e[i] }}; - {%- endfor %} - - {% for i in range(end=dims.nphi_e) %} + {%- endif %} + {%- if constraints.uphi_e[i] != 0 %} uphi_e[{{ i }}] = {{ constraints.uphi_e[i] }}; + {%- endif %} {%- endfor %} ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "lphi", lphi_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "uphi", uphi_e); ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, N, "nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux", &capsule->phi_e_constraint); + free(luphi_e); {% endif %} @@ -1619,29 +1879,87 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) {%- if solver_options.integrator_type != "DISCRETE" %} - int sim_method_num_steps[N]; + // set up sim_method_num_steps + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_num_steps[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_num_steps[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + {%- if all_equal == true %} + // all sim_method_num_steps are identical + int sim_method_num_steps = {{ solver_options.sim_method_num_steps[0] }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps); + {%- else %} + // sim_method_num_steps are different + int* sim_method_num_steps = malloc(N*sizeof(int)); {%- for j in range(end=dims.N) %} sim_method_num_steps[{{ j }}] = {{ solver_options.sim_method_num_steps[j] }}; {%- endfor %} for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_steps", &sim_method_num_steps[i]); + free(sim_method_num_steps); + {%- endif %} - int sim_method_num_stages[N]; + // set up sim_method_num_stages + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_num_stages[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_num_stages[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + + {%- if all_equal == true %} + // all sim_method_num_stages are identical + int sim_method_num_stages = {{ solver_options.sim_method_num_stages[0] }}; + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages); + {%- else %} + int* sim_method_num_stages = malloc(N*sizeof(int)); {%- for j in range(end=dims.N) %} sim_method_num_stages[{{ j }}] = {{ solver_options.sim_method_num_stages[j] }}; {%- endfor %} for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_num_stages", &sim_method_num_stages[i]); + free(sim_method_num_stages); + {%- endif %} int newton_iter_val = {{ solver_options.sim_method_newton_iter }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_newton_iter", &newton_iter_val); - bool tmp_bool = {{ solver_options.sim_method_jac_reuse }}; + + // set up sim_method_jac_reuse + {%- set all_equal = true %} + {%- set val = solver_options.sim_method_jac_reuse[0] %} + {%- for j in range(start=1, end=dims.N) %} + {%- if val != solver_options.sim_method_jac_reuse[j] %} + {%- set_global all_equal = false %} + {%- break %} + {%- endif %} + {%- endfor %} + {%- if all_equal == true %} + bool tmp_bool = (bool) {{ solver_options.sim_method_jac_reuse[0] }}; for (int i = 0; i < N; i++) ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &tmp_bool); + {%- else %} + bool* sim_method_jac_reuse = malloc(N*sizeof(bool)); + {%- for j in range(end=dims.N) %} + sim_method_jac_reuse[{{ j }}] = (bool){{ solver_options.sim_method_jac_reuse[j] }}; + {%- endfor %} + + for (int i = 0; i < N; i++) + ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_jac_reuse", &sim_method_jac_reuse[i]); + free(sim_method_jac_reuse); + {%- endif %} {%- endif %} @@ -1739,23 +2057,20 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) capsule->nlp_out = nlp_out; // initialize primal solution - double x0[{{ dims.nx }}]; + double* xu0 = calloc(NX+NU, sizeof(double)); + double* x0 = xu0; {% if dims.nbx_0 == dims.nx %} // initialize with x0 {% for item in constraints.lbx_0 %} + {%- if item != 0 %} x0[{{ loop.index0 }}] = {{ item }}; + {%- endif %} {%- endfor %} {% else %} // initialize with zeros - {% for i in range(end=dims.nx) %} - x0[{{ i }}] = 0.0; - {%- endfor %} {%- endif %} - double u0[NU]; - {% for i in range(end=dims.nu) %} - u0[{{ i }}] = 0.0; - {%- endfor %} + double* u0 = xu0 + NX; for (int i = 0; i < N; i++) { @@ -1765,21 +2080,25 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule) 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", x0); + free(xu0); capsule->nlp_solver = ocp_nlp_solver_create(nlp_config, nlp_dims, capsule->nlp_opts); {% if dims.np > 0 %} // initialize parameters to nominal value - double p[{{ dims.np }}]; - {% for i in range(end=dims.np) %} - p[{{ i }}] = {{ parameter_values[i] }}; + double* p = calloc(NP, sizeof(double)); + {% for item in parameter_values %} + {%- if item != 0 %} + p[{{ loop.index0 }}] = {{ item }}; + {%- endif %} {%- endfor %} for (int i = 0; i <= N; i++) { {{ model.name }}_acados_update_params(capsule, i, p, NP); } + free(p); {%- endif %}{# if dims.np #} status = ocp_nlp_precompute(capsule->nlp_solver, nlp_in, nlp_out); @@ -1816,6 +2135,9 @@ int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stag {%- if solver_options.hessian_approx == "EXACT" %} capsule->impl_dae_hess[stage].set_param(capsule->impl_dae_hess+stage, p); {%- endif %} + {% elif solver_options.integrator_type == "LIFTED_IRK" %} + capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p); + capsule->impl_dae_fun_jac_x_xdot_z[stage].set_param(capsule->impl_dae_fun_jac_x_xdot_z+stage, p); {% elif solver_options.integrator_type == "ERK" %} capsule->forw_vde_casadi[stage].set_param(capsule->forw_vde_casadi+stage, p); capsule->expl_ode_fun[stage].set_param(capsule->expl_ode_fun+stage, p); @@ -1945,6 +2267,15 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule) free(capsule->impl_dae_hess); {%- endif %} +{%- elif solver_options.integrator_type == "LIFTED_IRK" %} + for (int i = 0; i < {{ dims.N }}; i++) + { + external_function_param_casadi_free(&capsule->impl_dae_fun[i]); + external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_u[i]); + } + free(capsule->impl_dae_fun); + free(capsule->impl_dae_fun_jac_x_xdot_u); + {%- elif solver_options.integrator_type == "ERK" %} for (int i = 0; i < {{ dims.N }}; i++) { diff --git a/pyextra/acados_template/c_templates_tera/acados_solver.in.h b/pyextra/acados_template/c_templates_tera/acados_solver.in.h index e630b8fe4e..e8966c254c 100644 --- a/pyextra/acados_template/c_templates_tera/acados_solver.in.h +++ b/pyextra/acados_template/c_templates_tera/acados_solver.in.h @@ -64,6 +64,7 @@ typedef struct nlp_solver_capsule external_function_param_casadi *impl_dae_fun; external_function_param_casadi *impl_dae_fun_jac_x_xdot_z; external_function_param_casadi *impl_dae_jac_x_xdot_u_z; + external_function_param_casadi *impl_dae_fun_jac_x_xdot_u; external_function_param_casadi *impl_dae_hess; external_function_param_casadi *gnsf_phi_fun; external_function_param_casadi *gnsf_phi_fun_jac_y; @@ -108,7 +109,7 @@ typedef struct nlp_solver_capsule external_function_param_casadi nl_constr_h_e_fun_jac_hess; } nlp_solver_capsule; -nlp_solver_capsule * {{ model.name }}_acados_create_capsule(); +nlp_solver_capsule * {{ model.name }}_acados_create_capsule(void); int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule); int {{ model.name }}_acados_create(nlp_solver_capsule * capsule); diff --git a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c index 93635d9848..e70c5cafe3 100644 --- a/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c +++ b/pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c @@ -317,6 +317,21 @@ static void mdlInitializeSizes (SimStruct *S) 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 ); @@ -650,7 +665,7 @@ static void mdlOutputs(SimStruct *S, int_T tid) /* 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; + 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 #} @@ -702,6 +717,24 @@ static void mdlOutputs(SimStruct *S, int_T tid) 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 }}); diff --git a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h index 5dc611dd38..347446e3f4 100644 --- a/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h +++ b/pyextra/acados_template/c_templates_tera/cost_y_0_fun.in.h @@ -44,22 +44,22 @@ int {{ model.name }}_cost_y_0_fun(const real_t** arg, real_t** res, int* iw, rea int {{ model.name }}_cost_y_0_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_0_fun_sparsity_in(int); const int *{{ model.name }}_cost_y_0_fun_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_n_in(); -int {{ model.name }}_cost_y_0_fun_n_out(); +int {{ model.name }}_cost_y_0_fun_n_in(void); +int {{ model.name }}_cost_y_0_fun_n_out(void); int {{ model.name }}_cost_y_0_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_0_fun_jac_ut_xt_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_in(int); const int *{{ model.name }}_cost_y_0_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(); -int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_0_fun_jac_ut_xt_n_out(void); int {{ model.name }}_cost_y_0_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_0_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_0_hess_sparsity_in(int); const int *{{ model.name }}_cost_y_0_hess_sparsity_out(int); -int {{ model.name }}_cost_y_0_hess_n_in(); -int {{ model.name }}_cost_y_0_hess_n_out(); +int {{ model.name }}_cost_y_0_hess_n_in(void); +int {{ model.name }}_cost_y_0_hess_n_out(void); {% endif %} #ifdef __cplusplus diff --git a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h index 041f356939..acc99009fe 100644 --- a/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h +++ b/pyextra/acados_template/c_templates_tera/cost_y_e_fun.in.h @@ -44,22 +44,22 @@ int {{ model.name }}_cost_y_e_fun(const real_t** arg, real_t** res, int* iw, rea int {{ model.name }}_cost_y_e_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_e_fun_sparsity_in(int); const int *{{ model.name }}_cost_y_e_fun_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_n_in(); -int {{ model.name }}_cost_y_e_fun_n_out(); +int {{ model.name }}_cost_y_e_fun_n_in(void); +int {{ model.name }}_cost_y_e_fun_n_out(void); int {{ model.name }}_cost_y_e_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_e_fun_jac_ut_xt_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_in(int); const int *{{ model.name }}_cost_y_e_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(); -int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_e_fun_jac_ut_xt_n_out(void); int {{ model.name }}_cost_y_e_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_e_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_e_hess_sparsity_in(int); const int *{{ model.name }}_cost_y_e_hess_sparsity_out(int); -int {{ model.name }}_cost_y_e_hess_n_in(); -int {{ model.name }}_cost_y_e_hess_n_out(); +int {{ model.name }}_cost_y_e_hess_n_in(void); +int {{ model.name }}_cost_y_e_hess_n_out(void); {% endif %} #ifdef __cplusplus diff --git a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h index ec2119603f..1e03780cc1 100644 --- a/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h +++ b/pyextra/acados_template/c_templates_tera/cost_y_fun.in.h @@ -44,22 +44,22 @@ int {{ model.name }}_cost_y_fun(const real_t** arg, real_t** res, int* iw, real_ int {{ model.name }}_cost_y_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_fun_sparsity_in(int); const int *{{ model.name }}_cost_y_fun_sparsity_out(int); -int {{ model.name }}_cost_y_fun_n_in(); -int {{ model.name }}_cost_y_fun_n_out(); +int {{ model.name }}_cost_y_fun_n_in(void); +int {{ model.name }}_cost_y_fun_n_out(void); int {{ model.name }}_cost_y_fun_jac_ut_xt(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_fun_jac_ut_xt_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_in(int); const int *{{ model.name }}_cost_y_fun_jac_ut_xt_sparsity_out(int); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(); -int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_in(void); +int {{ model.name }}_cost_y_fun_jac_ut_xt_n_out(void); int {{ model.name }}_cost_y_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_y_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_y_hess_sparsity_in(int); const int *{{ model.name }}_cost_y_hess_sparsity_out(int); -int {{ model.name }}_cost_y_hess_n_in(); -int {{ model.name }}_cost_y_hess_n_out(); +int {{ model.name }}_cost_y_hess_n_in(void); +int {{ model.name }}_cost_y_hess_n_out(void); {% endif %} #ifdef __cplusplus diff --git a/pyextra/acados_template/c_templates_tera/external_cost.in.h b/pyextra/acados_template/c_templates_tera/external_cost.in.h index 19280182e5..d200dba45e 100644 --- a/pyextra/acados_template/c_templates_tera/external_cost.in.h +++ b/pyextra/acados_template/c_templates_tera/external_cost.in.h @@ -45,22 +45,22 @@ int {{ model.name }}_cost_ext_cost_fun(const real_t** arg, real_t** res, int* iw int {{ model.name }}_cost_ext_cost_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_fun_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_n_in(); -int {{ model.name }}_cost_ext_cost_fun_n_out(); +int {{ model.name }}_cost_ext_cost_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_n_out(void); int {{ model.name }}_cost_ext_cost_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_fun_jac_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(); -int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_hess_n_out(void); int {{ model.name }}_cost_ext_cost_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_fun_jac_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_fun_jac_n_in(); -int {{ model.name }}_cost_ext_cost_fun_jac_n_out(); +int {{ model.name }}_cost_ext_cost_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_fun_jac_n_out(void); {% endif %} {% else %} diff --git a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h index 0367e3d0c6..8152447e5f 100644 --- a/pyextra/acados_template/c_templates_tera/external_cost_0.in.h +++ b/pyextra/acados_template/c_templates_tera/external_cost_0.in.h @@ -46,22 +46,22 @@ int {{ model.name }}_cost_ext_cost_0_fun(const real_t** arg, real_t** res, int* int {{ model.name }}_cost_ext_cost_0_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_0_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_n_in(); -int {{ model.name }}_cost_ext_cost_0_fun_n_out(); +int {{ model.name }}_cost_ext_cost_0_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_n_out(void); int {{ model.name }}_cost_ext_cost_0_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_0_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(); -int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_hess_n_out(void); int {{ model.name }}_cost_ext_cost_0_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_0_fun_jac_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_0_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(); -int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_0_fun_jac_n_out(void); {% endif %} {% else %} diff --git a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h index de4f608b62..56485db4c7 100644 --- a/pyextra/acados_template/c_templates_tera/external_cost_e.in.h +++ b/pyextra/acados_template/c_templates_tera/external_cost_e.in.h @@ -45,22 +45,22 @@ int {{ model.name }}_cost_ext_cost_e_fun(const real_t** arg, real_t** res, int* int {{ model.name }}_cost_ext_cost_e_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_e_fun_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_n_in(); -int {{ model.name }}_cost_ext_cost_e_fun_n_out(); +int {{ model.name }}_cost_ext_cost_e_fun_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_n_out(void); int {{ model.name }}_cost_ext_cost_e_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_e_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(); -int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_hess_n_out(void); int {{ model.name }}_cost_ext_cost_e_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_cost_ext_cost_e_fun_jac_work(int *, int *, int *, int *); const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_in(int); const int *{{ model.name }}_cost_ext_cost_e_fun_jac_sparsity_out(int); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(); -int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_in(void); +int {{ model.name }}_cost_ext_cost_e_fun_jac_n_out(void); {% endif %} {% else %} diff --git a/pyextra/acados_template/c_templates_tera/h_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_constraint.in.h index 82e3d60840..e49176c6ef 100644 --- a/pyextra/acados_template/c_templates_tera/h_constraint.in.h +++ b/pyextra/acados_template/c_templates_tera/h_constraint.in.h @@ -43,23 +43,23 @@ int {{ model.name }}_constr_h_fun_jac_uxt_zt(const real_t** arg, real_t** res, i int {{ model.name }}_constr_h_fun_jac_uxt_zt_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_in(int); const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_n_out(void); int {{ model.name }}_constr_h_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_constr_h_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_fun_sparsity_in(int); const int *{{ model.name }}_constr_h_fun_sparsity_out(int); -int {{ model.name }}_constr_h_fun_n_in(); -int {{ model.name }}_constr_h_fun_n_out(); +int {{ model.name }}_constr_h_fun_n_in(void); +int {{ model.name }}_constr_h_fun_n_out(void); {% if solver_options.hessian_approx == "EXACT" -%} int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_in(int); const int *{{ model.name }}_constr_h_fun_jac_uxt_zt_hess_sparsity_out(int); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(); -int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_fun_jac_uxt_zt_hess_n_out(void); {% endif %} {% endif %} diff --git a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h index 7ac689faa6..a5dd711641 100644 --- a/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h +++ b/pyextra/acados_template/c_templates_tera/h_e_constraint.in.h @@ -44,23 +44,23 @@ int {{ model.name }}_constr_h_e_fun_jac_uxt_zt(const real_t** arg, real_t** res, int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_in(int); const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_n_out(void); int {{ model.name }}_constr_h_e_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_constr_h_e_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_e_fun_sparsity_in(int); const int *{{ model.name }}_constr_h_e_fun_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_n_in(); -int {{ model.name }}_constr_h_e_fun_n_out(); +int {{ model.name }}_constr_h_e_fun_n_in(void); +int {{ model.name }}_constr_h_e_fun_n_out(void); {% if solver_options.hessian_approx == "EXACT" -%} int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_in(int); const int *{{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_sparsity_out(int); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(); -int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_in(void); +int {{ model.name }}_constr_h_e_fun_jac_uxt_zt_hess_n_out(void); {% endif %} {% endif %} diff --git a/pyextra/acados_template/c_templates_tera/make_sfun.in.m b/pyextra/acados_template/c_templates_tera/make_sfun.in.m index 3299ceb39d..172da654ee 100644 --- a/pyextra/acados_template/c_templates_tera/make_sfun.in.m +++ b/pyextra/acados_template/c_templates_tera/make_sfun.in.m @@ -312,6 +312,21 @@ i_out = i_out + 1; output_note = strcat(output_note, num2str(i_out), ') CPU time\n '); {%- endif %} +{%- if simulink_opts.outputs.CPU_time_sim == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time integrator\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_qp == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time QP solution\n '); +{%- endif %} + +{%- if simulink_opts.outputs.CPU_time_lin == 1 %} +i_out = i_out + 1; +output_note = strcat(output_note, num2str(i_out), ') CPU time linearization (including integrator)\n '); +{%- endif %} + {%- if simulink_opts.outputs.sqp_iter == 1 %} i_out = i_out + 1; output_note = strcat(output_note, num2str(i_out), ') SQP iterations\n '); diff --git a/pyextra/acados_template/c_templates_tera/model.in.h b/pyextra/acados_template/c_templates_tera/model.in.h index fd8871c8b8..661811232c 100644 --- a/pyextra/acados_template/c_templates_tera/model.in.h +++ b/pyextra/acados_template/c_templates_tera/model.in.h @@ -46,46 +46,46 @@ extern "C" { {%- set hessian_approx = "GAUSS_NEWTON" %} {%- endif %} -{% if solver_options.integrator_type == "IRK" %} +{% if solver_options.integrator_type == "IRK" or solver_options.integrator_type == "LIFTED_IRK" %} // implicit ODE int {{ model.name }}_impl_dae_fun(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_fun_sparsity_in(int); const int *{{ model.name }}_impl_dae_fun_sparsity_out(int); -int {{ model.name }}_impl_dae_fun_n_in(); -int {{ model.name }}_impl_dae_fun_n_out(); +int {{ model.name }}_impl_dae_fun_n_in(void); +int {{ model.name }}_impl_dae_fun_n_out(void); // implicit ODE int {{ model.name }}_impl_dae_fun_jac_x_xdot_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_in(int); const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out(int); -int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(); -int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out(void); // implicit ODE int {{ model.name }}_impl_dae_jac_x_xdot_u_z(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_jac_x_xdot_u_z_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_in(int); const int *{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out(int); -int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(); -int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in(void); +int {{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out(void); -// // implicit ODE - for lifted_irk -// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); -// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); -// const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); -// const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); -// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(); -// int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(); +// implicit ODE - for lifted_irk +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_work(int *, int *, int *, int *); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_in(int); +const int *{{ model.name }}_impl_dae_fun_jac_x_xdot_u_sparsity_out(int); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_in(void); +int {{ model.name }}_impl_dae_fun_jac_x_xdot_u_n_out(void); {%- if hessian_approx == "EXACT" %} int {{ model.name }}_impl_dae_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_impl_dae_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_impl_dae_hess_sparsity_in(int); const int *{{ model.name }}_impl_dae_hess_sparsity_out(int); -int {{ model.name }}_impl_dae_hess_n_in(); -int {{ model.name }}_impl_dae_hess_n_out(); +int {{ model.name }}_impl_dae_hess_n_in(void); +int {{ model.name }}_impl_dae_hess_n_out(void); {%- endif %} {% elif solver_options.integrator_type == "GNSF" %} @@ -95,40 +95,40 @@ int {{ model.name }}_gnsf_get_matrices_fun(const double** arg, double** r int {{ model.name }}_gnsf_get_matrices_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_in(int); const int *{{ model.name }}_gnsf_get_matrices_fun_sparsity_out(int); -int {{ model.name }}_gnsf_get_matrices_fun_n_in(); -int {{ model.name }}_gnsf_get_matrices_fun_n_out(); +int {{ model.name }}_gnsf_get_matrices_fun_n_in(void); +int {{ model.name }}_gnsf_get_matrices_fun_n_out(void); // phi_fun int {{ model.name }}_gnsf_phi_fun(const double** arg, double** res, int* iw, double* w, void *mem); int {{ model.name }}_gnsf_phi_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_gnsf_phi_fun_sparsity_in(int); const int *{{ model.name }}_gnsf_phi_fun_sparsity_out(int); -int {{ model.name }}_gnsf_phi_fun_n_in(); -int {{ model.name }}_gnsf_phi_fun_n_out(); +int {{ model.name }}_gnsf_phi_fun_n_in(void); +int {{ model.name }}_gnsf_phi_fun_n_out(void); // phi_fun_jac_y int {{ model.name }}_gnsf_phi_fun_jac_y(const double** arg, double** res, int* iw, double* w, void *mem); int {{ model.name }}_gnsf_phi_fun_jac_y_work(int *, int *, int *, int *); const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in(int); const int *{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out(int); -int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(); -int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_in(void); +int {{ model.name }}_gnsf_phi_fun_jac_y_n_out(void); // phi_jac_y_uhat int {{ model.name }}_gnsf_phi_jac_y_uhat(const double** arg, double** res, int* iw, double* w, void *mem); int {{ model.name }}_gnsf_phi_jac_y_uhat_work(int *, int *, int *, int *); const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in(int); const int *{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out(int); -int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(); -int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_in(void); +int {{ model.name }}_gnsf_phi_jac_y_uhat_n_out(void); // f_lo_fun_jac_x1k1uz int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz(const double** arg, double** res, int* iw, double* w, void *mem); int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work(int *, int *, int *, int *); const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in(int); const int *{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out(int); -int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(); -int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in(void); +int {{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_out(void); {% elif solver_options.integrator_type == "ERK" %} /* explicit ODE */ @@ -138,32 +138,32 @@ int {{ model.name }}_expl_ode_fun(const real_t** arg, real_t** res, int* iw, rea int {{ model.name }}_expl_ode_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_expl_ode_fun_sparsity_in(int); const int *{{ model.name }}_expl_ode_fun_sparsity_out(int); -int {{ model.name }}_expl_ode_fun_n_in(); -int {{ model.name }}_expl_ode_fun_n_out(); +int {{ model.name }}_expl_ode_fun_n_in(void); +int {{ model.name }}_expl_ode_fun_n_out(void); // explicit forward VDE int {{ model.name }}_expl_vde_forw(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_expl_vde_forw_work(int *, int *, int *, int *); const int *{{ model.name }}_expl_vde_forw_sparsity_in(int); const int *{{ model.name }}_expl_vde_forw_sparsity_out(int); -int {{ model.name }}_expl_vde_forw_n_in(); -int {{ model.name }}_expl_vde_forw_n_out(); +int {{ model.name }}_expl_vde_forw_n_in(void); +int {{ model.name }}_expl_vde_forw_n_out(void); // explicit adjoint VDE int {{ model.name }}_expl_vde_adj(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_expl_vde_adj_work(int *, int *, int *, int *); const int *{{ model.name }}_expl_vde_adj_sparsity_in(int); const int *{{ model.name }}_expl_vde_adj_sparsity_out(int); -int {{ model.name }}_expl_vde_adj_n_in(); -int {{ model.name }}_expl_vde_adj_n_out(); +int {{ model.name }}_expl_vde_adj_n_in(void); +int {{ model.name }}_expl_vde_adj_n_out(void); {%- if hessian_approx == "EXACT" %} int {{ model.name }}_expl_ode_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_expl_ode_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_expl_ode_hess_sparsity_in(int); const int *{{ model.name }}_expl_ode_hess_sparsity_out(int); -int {{ model.name }}_expl_ode_hess_n_in(); -int {{ model.name }}_expl_ode_hess_n_out(); +int {{ model.name }}_expl_ode_hess_n_in(void); +int {{ model.name }}_expl_ode_hess_n_out(void); {%- endif %} {% elif solver_options.integrator_type == "DISCRETE" %} @@ -173,23 +173,23 @@ int {{ model.name }}_dyn_disc_phi_fun(const real_t** arg, real_t** res, int* iw, int {{ model.name }}_dyn_disc_phi_fun_work(int *, int *, int *, int *); const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_in(int); const int *{{ model.name }}_dyn_disc_phi_fun_sparsity_out(int); -int {{ model.name }}_dyn_disc_phi_fun_n_in(); -int {{ model.name }}_dyn_disc_phi_fun_n_out(); +int {{ model.name }}_dyn_disc_phi_fun_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_n_out(void); int {{ model.name }}_dyn_disc_phi_fun_jac(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_dyn_disc_phi_fun_jac_work(int *, int *, int *, int *); const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_in(int); const int *{{ model.name }}_dyn_disc_phi_fun_jac_sparsity_out(int); -int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(); -int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_n_out(void); {%- if hessian_approx == "EXACT" %} int {{ model.name }}_dyn_disc_phi_fun_jac_hess(const real_t** arg, real_t** res, int* iw, real_t* w, void *mem); int {{ model.name }}_dyn_disc_phi_fun_jac_hess_work(int *, int *, int *, int *); const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_in(int); const int *{{ model.name }}_dyn_disc_phi_fun_jac_hess_sparsity_out(int); -int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(); -int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_in(void); +int {{ model.name }}_dyn_disc_phi_fun_jac_hess_n_out(void); {%- endif %} {% else %} {%- if hessian_approx == "EXACT" %} diff --git a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h index e1c15938b0..283ed7f889 100644 --- a/pyextra/acados_template/c_templates_tera/phi_constraint.in.h +++ b/pyextra/acados_template/c_templates_tera/phi_constraint.in.h @@ -44,8 +44,8 @@ int {{ model.name }}_phi_constraint(const real_t** arg, real_t** res, int* iw, r int {{ model.name }}_phi_constraint_work(int *, int *, int *, int *); const int *{{ model.name }}_phi_constraint_sparsity_in(int); const int *{{ model.name }}_phi_constraint_sparsity_out(int); -int {{ model.name }}_phi_constraint_n_in(); -int {{ model.name }}_phi_constraint_n_out(); +int {{ model.name }}_phi_constraint_n_in(void); +int {{ model.name }}_phi_constraint_n_out(void); {% endif %} #ifdef __cplusplus diff --git a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h index a8fc09475d..dc8e293ad7 100644 --- a/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h +++ b/pyextra/acados_template/c_templates_tera/phi_e_constraint.in.h @@ -10,8 +10,8 @@ int {{ model.name }}_phi_e_constraint(const real_t** arg, real_t** res, int* iw, int {{ model.name }}_phi_e_constraint_work(int *, int *, int *, int *); const int *{{ model.name }}_phi_e_constraint_sparsity_in(int); const int *{{ model.name }}_phi_e_constraint_sparsity_out(int); -int {{ model.name }}_phi_e_constraint_n_in(); -int {{ model.name }}_phi_e_constraint_n_out(); +int {{ model.name }}_phi_e_constraint_n_in(void); +int {{ model.name }}_phi_e_constraint_n_out(void); {% endif %} #ifdef __cplusplus diff --git a/pyextra/acados_template/generate_c_code_implicit_ode.py b/pyextra/acados_template/generate_c_code_implicit_ode.py index 5ef417c414..e0138c69f6 100644 --- a/pyextra/acados_template/generate_c_code_implicit_ode.py +++ b/pyextra/acados_template/generate_c_code_implicit_ode.py @@ -129,6 +129,9 @@ def generate_c_code_implicit_ode( model, opts ): fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u_z' impl_dae_fun_jac_x_xdot_u_z.generate(fun_name, casadi_opts) + fun_name = model_name + '_impl_dae_fun_jac_x_xdot_u' + impl_dae_fun_jac_x_xdot_u.generate(fun_name, casadi_opts) + if generate_hess: fun_name = model_name + '_impl_dae_hess' impl_dae_hess.generate(fun_name, casadi_opts) diff --git a/pyextra/acados_template/simulink_default_opts.json b/pyextra/acados_template/simulink_default_opts.json index 16074a4027..258a224cb2 100644 --- a/pyextra/acados_template/simulink_default_opts.json +++ b/pyextra/acados_template/simulink_default_opts.json @@ -7,6 +7,9 @@ "KKT_residual": 1, "x1": 1, "CPU_time": 1, + "CPU_time_sim": 0, + "CPU_time_qp": 0, + "CPU_time_lin": 0, "sqp_iter": 1 }, "inputs": {