parent
cf4ad99554
commit
22c8564413
|
@ -1 +1,4 @@
|
|||
acados/
|
||||
!x86_64/
|
||||
!larch64/
|
||||
!aarch64/
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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);
|
||||
|
|
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
@ -706,7 +706,10 @@
|
|||
"int"
|
||||
],
|
||||
"sim_method_jac_reuse": [
|
||||
"bool"
|
||||
"ndarray",
|
||||
[
|
||||
"N"
|
||||
]
|
||||
],
|
||||
"qp_solver_cond_N": [
|
||||
"int"
|
||||
|
|
|
@ -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):
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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]
|
|
@ -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:
|
||||
"""
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
File diff suppressed because it is too large
Load Diff
|
@ -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);
|
||||
|
|
|
@ -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 }});
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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 %}
|
||||
|
|
|
@ -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 %}
|
||||
|
|
|
@ -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 %}
|
||||
|
|
|
@ -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 %}
|
||||
|
||||
|
|
|
@ -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 %}
|
||||
|
||||
|
|
|
@ -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 ');
|
||||
|
|
|
@ -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" %}
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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
|
||||
|
|
|
@ -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)
|
||||
|
|
|
@ -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": {
|
||||
|
|
Loading…
Reference in New Issue