mawei/MoreTore
1
0
Fork 0
mirror of https://github.com/MoreTore/openpilot.git synced 2026-08-03 08:41:47 +08:00
Code Issues Packages Projects Releases Wiki Activity

cython wrapper for acados (#22784)

Browse Source 
* cython wrapper for acados

* fix building

* sconscript cleanup

* no cython numpy

* cleanup

* upgrade build script

* try without slices

* new acados commit

* c3 update acados libs

* c2 libs

* make faster

* undo profiling

* fix build

* somewhat faster

* tryout cost_set_slice

* Revert "tryout cost_set_slice"

This reverts commit d358d93a133270e4edab9e7c07ffb6f577c52bd6.

* cleanup

* undo t_renderer change

Co-authored-by: Comma Device <device@comma.ai>
This commit is contained in:
Joost Wooning
2021-11-12 17:09:08 +01:00
committed by GitHub
parent 8a8cee5e54
commit 89d0a52d16
60 changed files with 1738 additions and 443 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SConstruct
+1
View File
@@ -68,6 +68,7 @@ lenv = {
"PYTHONPATH": Dir("#").abspath + ":" + Dir("#pyextra/").abspath,
"ACADOS_SOURCE_DIR": Dir("#third_party/acados/acados").abspath,
"ACADOS_PYTHON_INTERFACE_PATH": Dir("#pyextra/acados_template").abspath,
"TERA_PATH": Dir("#").abspath + f"/third_party/acados/{arch}/t_renderer",
}
pyextra/acados_template/.gitignore
+5
View File
@@ -1 +1,6 @@
__pycache__/
# Cython intermediates
*_pyx.c
*_pyx.o
*_pyx.so
pyextra/acados_template/acados_layout.json
+3
View File
@@ -666,6 +666,9 @@
"nlp_solver_type": [
"str"
],
"collocation_type": [
"str"
],
"globalization": [
"str"
],
pyextra/acados_template/acados_model.py
+9
View File
@@ -96,6 +96,9 @@ class AcadosModel():
self.cost_expr_ext_cost = None #: CasADi expression for external cost; Default: :code:`None`
self.cost_expr_ext_cost_e = None #: CasADi expression for external cost, terminal; Default: :code:`None`
self.cost_expr_ext_cost_0 = None #: CasADi expression for external cost, initial; Default: :code:`None`
self.cost_expr_ext_cost_custom_hess = None #: CasADi expression for custom hessian (only for external cost); Default: :code:`None`
self.cost_expr_ext_cost_custom_hess_e = None #: CasADi expression for custom hessian (only for external cost), terminal; Default: :code:`None`
self.cost_expr_ext_cost_custom_hess_0 = None #: CasADi expression for custom hessian (only for external cost), initial; Default: :code:`None`
def acados_model_strip_casadi_symbolics(model):
@@ -147,5 +150,11 @@ def acados_model_strip_casadi_symbolics(model):
del out['cost_expr_ext_cost_e']
if 'cost_expr_ext_cost_0' in out.keys():
del out['cost_expr_ext_cost_0']
if 'cost_expr_ext_cost_custom_hess' in out.keys():
del out['cost_expr_ext_cost_custom_hess']
if 'cost_expr_ext_cost_custom_hess_e' in out.keys():
del out['cost_expr_ext_cost_custom_hess_e']
if 'cost_expr_ext_cost_custom_hess_0' in out.keys():
del out['cost_expr_ext_cost_custom_hess_0']
return out
pyextra/acados_template/acados_ocp.py
+19
View File
@@ -2120,6 +2120,7 @@ class AcadosOcpOptions:
self.__globalization = 'FIXED_STEP'
self.__nlp_solver_step_length = 1.0 # fixed Newton step length
self.__levenberg_marquardt = 0.0
self.__collocation_type = 'GAUSS_LEGENDRE'
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
@@ -2195,6 +2196,15 @@ class AcadosOcpOptions:
"""
return self.__globalization
@property
def collocation_type(self):
"""Collocation type: relevant for implicit integrators
-- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}.
Default: GAUSS_LEGENDRE
"""
return self.__collocation_type
@property
def regularize_method(self):
"""Regularization method for the Hessian.
@@ -2481,6 +2491,15 @@ class AcadosOcpOptions:
raise Exception('Invalid regularize_method value. Possible values are:\n\n' \
+ ',\n'.join(regularize_methods) + '.\n\nYou have: ' + regularize_method + '.\n\nExiting.')
@collocation_type.setter
def collocation_type(self, collocation_type):
collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
if collocation_type in collocation_types:
self.__collocation_type = collocation_type
else:
raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
+ ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
@hessian_approx.setter
def hessian_approx(self, hessian_approx):
hessian_approxs = ('GAUSS_NEWTON', 'EXACT')
pyextra/acados_template/acados_ocp_solver.py
+299 -125
View File
@@ -53,7 +53,7 @@ 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
set_up_imported_gnsf_model, get_acados_path, get_ocp_nlp_layout, get_python_interface_path
def make_ocp_dims_consistent(acados_ocp):
@@ -102,6 +102,7 @@ def make_ocp_dims_consistent(acados_ocp):
cost.cost_ext_fun_type_0 = cost.cost_ext_fun_type
model.cost_y_expr_0 = model.cost_y_expr
model.cost_expr_ext_cost_0 = model.cost_expr_ext_cost
model.cost_expr_ext_cost_custom_hess_0 = model.cost_expr_ext_cost_custom_hess
if cost.cost_type_0 == 'LINEAR_LS':
ny_0 = cost.W_0.shape[0]
@@ -433,9 +434,19 @@ def make_ocp_dims_consistent(acados_ocp):
if np.shape(opts.shooting_nodes)[0] != dims.N+1:
raise Exception('inconsistent dimension N, regarding shooting_nodes.')
# time_steps = opts.shooting_nodes[1:] - opts.shooting_nodes[0:-1]
# # identify constant time-steps: due to numerical reasons the content of time_steps might vary a bit
# delta_time_steps = time_steps[1:] - time_steps[0:-1]
# avg_time_steps = np.average(time_steps)
# # criterion for constant time-step detection: the min/max difference in values normalized by the average
# check_const_time_step = np.max(delta_time_steps)-np.min(delta_time_steps) / avg_time_steps
# # if the criterion is small, we have a constant time-step
# if check_const_time_step < 1e-9:
# time_steps[:] = avg_time_steps # if we have a constant time-step: apply the average time-step
time_steps = np.zeros((dims.N,))
for i in range(dims.N):
time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i]
time_steps[i] = opts.shooting_nodes[i+1] - opts.shooting_nodes[i] # TODO use commented code above
opts.time_steps = time_steps
elif (not is_empty(opts.time_steps)) and (not is_empty(opts.shooting_nodes)):
@@ -483,13 +494,12 @@ def make_ocp_dims_consistent(acados_ocp):
raise Exception("Wrong value for sim_method_jac_reuse. Should be either int or array of ints of shape (N,).")
def get_ocp_nlp_layout():
current_module = sys.modules[__name__]
acados_path = os.path.dirname(current_module.__file__)
with open(acados_path + '/acados_layout.json', 'r') as f:
ocp_nlp_layout = json.load(f)
return ocp_nlp_layout
def get_simulink_default_opts():
python_interface_path = get_python_interface_path()
abs_path = os.path.join(python_interface_path, 'simulink_default_opts.json')
with open(abs_path , 'r') as f:
simulink_default_opts = json.load(f)
return simulink_default_opts
def ocp_formulation_json_dump(acados_ocp, simulink_opts, json_file='acados_ocp_nlp.json'):
@@ -622,9 +632,7 @@ def ocp_render_templates(acados_ocp, json_file):
name = acados_ocp.model.name
# setting up loader and environment
json_path = '{cwd}/{json_file}'.format(
cwd=os.getcwd(),
json_file=json_file)
json_path = os.path.join(os.getcwd(), json_file)
if not os.path.exists(json_path):
raise Exception('{} not found!'.format(json_path))
@@ -645,6 +653,10 @@ def ocp_render_templates(acados_ocp, json_file):
out_file = f'acados_solver_{name}.h'
render_template(in_file, out_file, template_dir, json_path)
in_file = 'acados_solver.in.pxd'
out_file = f'acados_solver.pxd'
render_template(in_file, out_file, template_dir, json_path)
in_file = 'Makefile.in'
out_file = 'Makefile'
render_template(in_file, out_file, template_dir, json_path)
@@ -671,8 +683,7 @@ def ocp_render_templates(acados_ocp, json_file):
render_template(in_file, out_file, template_dir, json_path)
## folder model
template_dir = f'{code_export_dir}/{name}_model/'
template_dir = os.path.join(code_export_dir, name + '_model')
in_file = 'model.in.h'
out_file = f'{name}_model.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -680,7 +691,7 @@ def ocp_render_templates(acados_ocp, json_file):
# constraints on convex over nonlinear function
if acados_ocp.constraints.constr_type == 'BGP' and acados_ocp.dims.nphi > 0:
# constraints on outer function
template_dir = f'{code_export_dir}/{name}_constraints/'
template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'phi_constraint.in.h'
out_file = f'{name}_phi_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -688,62 +699,62 @@ def ocp_render_templates(acados_ocp, json_file):
# terminal constraints on convex over nonlinear function
if acados_ocp.constraints.constr_type_e == 'BGP' and acados_ocp.dims.nphi_e > 0:
# terminal constraints on outer function
template_dir = f'{code_export_dir}/{name}_constraints/'
template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'phi_e_constraint.in.h'
out_file = f'{name}_phi_e_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# nonlinear constraints
if acados_ocp.constraints.constr_type == 'BGH' and acados_ocp.dims.nh > 0:
template_dir = f'{code_export_dir}/{name}_constraints/'
template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'h_constraint.in.h'
out_file = f'{name}_h_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# terminal nonlinear constraints
if acados_ocp.constraints.constr_type_e == 'BGH' and acados_ocp.dims.nh_e > 0:
template_dir = f'{code_export_dir}/{name}_constraints/'
template_dir = os.path.join(code_export_dir, name + '_constraints')
in_file = 'h_e_constraint.in.h'
out_file = f'{name}_h_e_constraint.h'
render_template(in_file, out_file, template_dir, json_path)
# initial stage Nonlinear LS cost function
if acados_ocp.cost.cost_type_0 == 'NONLINEAR_LS':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_0_fun.in.h'
out_file = f'{name}_cost_y_0_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost - terminal
elif acados_ocp.cost.cost_type_0 == 'EXTERNAL':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost_0.in.h'
out_file = f'{name}_external_cost_0.h'
render_template(in_file, out_file, template_dir, json_path)
# path Nonlinear LS cost function
if acados_ocp.cost.cost_type == 'NONLINEAR_LS':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_fun.in.h'
out_file = f'{name}_cost_y_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# terminal Nonlinear LS cost function
if acados_ocp.cost.cost_type_e == 'NONLINEAR_LS':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'cost_y_e_fun.in.h'
out_file = f'{name}_cost_y_e_fun.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost
if acados_ocp.cost.cost_type == 'EXTERNAL':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost.in.h'
out_file = f'{name}_external_cost.h'
render_template(in_file, out_file, template_dir, json_path)
# external cost - terminal
if acados_ocp.cost.cost_type_e == 'EXTERNAL':
template_dir = f'{code_export_dir}/{name}_cost/'
template_dir = os.path.join(code_export_dir, name + '_cost')
in_file = 'external_cost_e.in.h'
out_file = f'{name}_external_cost_e.h'
render_template(in_file, out_file, template_dir, json_path)
@@ -775,9 +786,7 @@ class AcadosOcpSolver:
model = acados_ocp.model
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)
simulink_opts = get_simulink_default_opts()
# make dims consistent
make_ocp_dims_consistent(acados_ocp)
@@ -830,6 +839,14 @@ class AcadosOcpSolver:
assert getattr(self.shared_lib, f"{self.model_name}_acados_create")(self.capsule)==0
self.solver_created = True
# get pointers solver
self.__get_pointers_solver()
def __get_pointers_solver(self):
"""
Private function to get the pointers for solver
"""
# get pointers solver
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
@@ -863,14 +880,10 @@ class AcadosOcpSolver:
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
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_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
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_constraints_model_set.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
@@ -881,15 +894,6 @@ class AcadosOcpSolver:
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_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
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_get_at_stage.argtypes = \
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p]
def solve(self):
"""
Solve the ocp with current input.
@@ -901,46 +905,118 @@ class AcadosOcpSolver:
return status
def get_slice(self, start_stage_, end_stage_, field_):
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)
self.fill_in_slice(start_stage_, end_stage_, field_, out)
return out
def set_new_time_steps(self, new_time_steps):
"""
Set new time steps before solving. Only reload library without code generation but with new time steps.
def fill_in_slice(self, start_stage_, end_stage_, field_, arr):
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't']
mem_fields = ['sl', 'su']
:param new_time_steps: vector of new time steps for the solver
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
do not require a new code export and compilation.
"""
# unlikely but still possible
if not self.solver_created:
raise Exception('Solver was not yet created!')
# check if time steps really changed in value
if np.array_equal(self.acados_ocp.solver_options.time_steps, new_time_steps):
return
N = new_time_steps.size
model = self.acados_ocp.model
new_time_steps_data = cast(new_time_steps.ctypes.data, POINTER(c_double))
# check if recreation of acados is necessary (no need to recreate acados if sizes are identical)
if self.acados_ocp.solver_options.time_steps.size == N:
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").argtypes = [c_void_p, c_int, c_void_p]
getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps").restype = c_int
assert getattr(self.shared_lib, f"{self.model_name}_acados_update_time_steps")(self.capsule, N, new_time_steps_data) == 0
else: # recreate the solver with the new time steps
self.solver_created = False
# delete old memory (analog to __del__)
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)
# store N and new time steps
self.N = self.acados_ocp.dims.N = N
self.acados_ocp.solver_options.time_steps = new_time_steps
self.acados_ocp.solver_options.Tsim = self.acados_ocp.solver_options.time_steps[0]
# create solver with new time steps
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").argtypes = [c_void_p, c_int, c_void_p]
getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization").restype = c_int
assert getattr(self.shared_lib, f"{self.model_name}_acados_create_with_discretization")(self.capsule, N, new_time_steps_data) == 0
self.solver_created = True
# get pointers solver
self.__get_pointers_solver()
def get(self, stage_, field_):
"""
Get the last solution of the solver:
:param stage: integer corresponding to shooting node
: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', 'sl', 'su']
# 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.\
if (field_ not in out_fields):
raise Exception('AcadosOcpSolver.get(): {} 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(stage_, int):
raise Exception('AcadosOcpSolver.get(): stage index must be Integer.')
if not isinstance(end_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 start_stage_ >= end_stage_:
raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index')
if stage_ == self.N and field_ == 'pi':
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
.format(field_, stage_))
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
out_data = cast(arr.ctypes.data, POINTER(c_double))
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
out = np.ascontiguousarray(np.zeros((dims,)), dtype=np.float64)
out_data = cast(out.ctypes.data, POINTER(c_double))
if (field_ in out_fields):
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(self.nlp_config, \
self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data)
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)
# 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)
def get(self, stage_, field_):
return self.get_slice(stage_, stage_ + 1, field_)[0]
return out
def print_statistics(self):
@@ -1176,8 +1252,7 @@ class AcadosOcpSolver:
"""
cost_fields = ['y_ref', 'yref']
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu']
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z']
mem_fields = ['sl', 'su']
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z', 'sl', 'su']
# cast value_ to avoid conversion issues
if isinstance(value_, (float, int)):
@@ -1189,47 +1264,52 @@ class AcadosOcpSolver:
stage = c_int(stage_)
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']))
# treat parameters separately
if field_ == 'p':
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
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
value_data = cast(value_.ctypes.data, POINTER(c_double))
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage_, field)
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:
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
\nPossible values are {}. Exiting.".format(field, \
constraints_fields + cost_fields + out_fields + ['p']))
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)
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[0])
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(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(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(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(self.nlp_config, \
# self.nlp_solver, stage, field, value_data_p)
value_data_p = cast(value_.ctypes.data, c_void_p)
if field_ in constraints_fields:
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(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(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(self.nlp_config, \
self.nlp_solver, stage, field, value_data_p)
return
def set_param(self, stage_, value_):
value_data = cast(value_.ctypes.data, POINTER(c_double))
self._set_param(self.capsule, stage_, value_data, value_.shape[0])
def cost_set(self, start_stage_, field_, value_, api='warn'):
self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn')
def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
def cost_set(self, stage_, field_, value_, api='warn'):
"""
Set numerical data in the cost module of the solver.
@@ -1238,39 +1318,133 @@ class AcadosOcpSolver:
:param value: of appropriate size
"""
# cast value_ to avoid conversion issues
field = field_.encode('utf-8')
if len(value_.shape) > 2:
dim = value_.shape[1]*value_.shape[2]
else:
dim = value_.shape[1]
if isinstance(value_, (float, int)):
value_ = np.array([value_])
value_ = value_.astype(float)
self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, \
self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
cast(value_.ctypes.data, c_void_p), dim)
field = field_
field = field.encode('utf-8')
stage = c_int(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, stage_, field, dims_data)
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
elif len(value_shape) == 2:
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 != tuple(dims):
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
' for field "{}" with dimension {} (you have {})'.format( \
field_, tuple(dims), 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)
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')
def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'):
def constraints_set(self, 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 field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi', 'C', 'D']
: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_.encode('utf-8')
if len(value_.shape) > 2:
dim = value_.shape[1]*value_.shape[2]
else:
dim = value_.shape[1]
field = field_
field = field.encode('utf-8')
self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \
self.nlp_dims, self.nlp_in, start_stage_, end_stage_, field,
cast(value_.ctypes.data, c_void_p), dim)
stage = c_int(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, stage_, field, dims_data)
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
elif len(value_shape) == 2:
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 != tuple(dims):
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), 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)
return
def dynamics_get(self, stage_, field_):
pyextra/acados_template/acados_ocp_solver_pyx.pyx
+427
View File
@@ -0,0 +1,427 @@
# -*- coding: future_fstrings -*-
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
# cython: language_level=3
# cython: profile=False
# distutils: language=c
cimport cython
from libc cimport string
cimport acados_solver_common
cimport acados_solver
cimport numpy as cnp
import os
import numpy as np
cdef class AcadosOcpSolverFast:
"""
Class to interact with the acados ocp solver C object.
: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
"""
cdef acados_solver.nlp_solver_capsule *capsule
cdef void *nlp_opts
cdef acados_solver_common.ocp_nlp_dims *nlp_dims
cdef acados_solver_common.ocp_nlp_config *nlp_config
cdef acados_solver_common.ocp_nlp_out *nlp_out
cdef acados_solver_common.ocp_nlp_in *nlp_in
cdef acados_solver_common.ocp_nlp_solver *nlp_solver
cdef str model_name
cdef int N
cdef bint solver_created
def __cinit__(self, str model_name, int N, str code_export_dir):
self.model_name = model_name
self.N = N
self.solver_created = False
# create capsule
self.capsule = acados_solver.acados_create_capsule()
# create solver
assert acados_solver.acados_create(self.capsule) == 0
self.solver_created = True
# get pointers solver
self.nlp_opts = acados_solver.acados_get_nlp_opts(self.capsule)
self.nlp_dims = acados_solver.acados_get_nlp_dims(self.capsule)
self.nlp_config = acados_solver.acados_get_nlp_config(self.capsule)
self.nlp_out = acados_solver.acados_get_nlp_out(self.capsule)
self.nlp_in = acados_solver.acados_get_nlp_in(self.capsule)
self.nlp_solver = acados_solver.acados_get_nlp_solver(self.capsule)
def solve(self):
"""
Solve the ocp with current input.
"""
return acados_solver.acados_solve(self.capsule)
def set_new_time_steps(self, new_time_steps):
"""
Set new time steps before solving. Only reload library without code generation but with new time steps.
:param new_time_steps: vector of new time steps for the solver
.. note:: This allows for different use-cases: either set a new size of time-steps or a new distribution of
the shooting nodes without changing the number, e.g., to reach a different final time. Both cases
do not require a new code export and compilation.
"""
raise NotImplementedError()
def get(self, int stage, str field_):
"""
Get the last solution of the solver:
:param stage: integer corresponding to shooting node
: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', 'sl', 'su']
field = field_.encode('utf-8')
if field_ not in out_fields:
raise Exception('AcadosOcpSolver.get(): {} is an invalid argument.\
\n Possible values are {}. Exiting.'.format(field_, out_fields))
if stage < 0 or stage > self.N:
raise Exception('AcadosOcpSolver.get(): stage index must be in [0, N], got: {}.'.format(self.N))
if stage == self.N and field_ == 'pi':
raise Exception('AcadosOcpSolver.get(): field {} does not exist at final stage {}.'\
.format(field_, stage))
cdef int dims = acados_solver_common.ocp_nlp_dims_get_from_attr(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field)
cdef cnp.ndarray[cnp.float64_t, ndim=1] out = np.zeros((dims,))
acados_solver_common.ocp_nlp_out_get(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, <void *> out.data)
return out
def print_statistics(self):
"""
prints statistics of previous solver run as a table:
- iter: iteration number
- res_stat: stationarity residual
- res_eq: residual wrt equality constraints (dynamics)
- res_ineq: residual wrt inequality constraints (constraints)
- res_comp: residual wrt complementarity conditions
- qp_stat: status of QP solver
- qp_iter: number of QP iterations
- qp_res_stat: stationarity residual of the last QP solution
- qp_res_eq: residual wrt equality constraints (dynamics) of the last QP solution
- qp_res_ineq: residual wrt inequality constraints (constraints) of the last QP solution
- qp_res_comp: residual wrt complementarity conditions of the last QP solution
"""
raise NotImplementedError()
def store_iterate(self, filename='', overwrite=False):
"""
Stores the current iterate of the ocp solver in a json file.
:param filename: if not set, use model_name + timestamp + '.json'
:param overwrite: if false and filename exists add timestamp to filename
"""
raise NotImplementedError()
def load_iterate(self, filename):
"""
Loads the iterate stored in json file with filename into the ocp solver.
"""
raise NotImplementedError()
def get_stats(self, field_):
"""
Get the information of the last solver call.
:param field: string in ['statistics', 'time_tot', 'time_lin', 'time_sim', 'time_sim_ad', 'time_sim_la', 'time_qp', 'time_qp_solver_call', 'time_reg', 'sqp_iter']
"""
raise NotImplementedError()
def get_cost(self):
"""
Returns the cost value of the current solution.
"""
# compute cost internally
acados_solver_common.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out)
# create output
cdef double out
# call getter
acados_solver_common.ocp_nlp_get(self.nlp_config, self.nlp_solver, "cost_value", <void *> &out)
return out
def get_residuals(self):
"""
Returns an array of the form [res_stat, res_eq, res_ineq, res_comp].
"""
raise NotImplementedError()
# Note: this function should not be used anymore, better use cost_set, constraints_set
def set(self, int stage, str 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', 'sl', 'su']
field = field_.encode('utf-8')
cdef double[::1] value
# treat parameters separately
if field_ == 'p':
value = np.ascontiguousarray(value_, dtype=np.double)
assert acados_solver.acados_update_params(self.capsule, stage, <double *> &value[0], value.shape[0]) == 0
else:
if field_ not in constraints_fields + cost_fields + out_fields:
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\
\nPossible values are {}. Exiting.".format(field, \
constraints_fields + cost_fields + out_fields + ['p']))
dims = acados_solver_common.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[0])
raise Exception(msg)
value = np.ascontiguousarray(value_, dtype=np.double)
if field_ in constraints_fields:
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config,
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
elif field_ in cost_fields:
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config,
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0])
elif field_ in out_fields:
acados_solver_common.ocp_nlp_out_set(self.nlp_config,
self.nlp_dims, self.nlp_out, stage, field, <void *> &value[0])
def cost_set(self, int stage, str field_, value_):
"""
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
"""
field = field_.encode('utf-8')
cdef int dims[2]
acados_solver_common.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
cdef double[::1,:] value
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
value = np.asfortranarray(value_[None,:])
elif len(value_shape) == 2:
# Get elements in column major order
value = np.asfortranarray(value_)
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', \
' for field "{}" with dimension {} (you have {})'.format( \
field_, tuple(dims), value_shape))
acados_solver_common.ocp_nlp_cost_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
def constraints_set(self, int stage, str field_, value_):
"""
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', 'C', 'D']
:param value: of appropriate size
"""
field = field_.encode('utf-8')
cdef int dims[2]
acados_solver_common.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \
self.nlp_dims, self.nlp_out, stage, field, &dims[0])
cdef double[::1,:] value
value_shape = value_.shape
if len(value_shape) == 1:
value_shape = (value_shape[0], 0)
value = np.asfortranarray(value_[None,:])
elif len(value_shape) == 2:
# Get elements in column major order
value = np.asfortranarray(value_)
if value_shape[0] != dims[0] or value_shape[1] != dims[1]:
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \
' for field "{}" with dimension {} (you have {})'.format(field_, tuple(dims), value_shape))
acados_solver_common.ocp_nlp_constraints_model_set(self.nlp_config, \
self.nlp_dims, self.nlp_in, stage, field, <void *> &value[0][0])
return
def dynamics_get(self, int stage, str field_):
"""
Get numerical data from the dynamics module of the solver:
:param stage: integer corresponding to shooting node
:param field: string, e.g. 'A'
"""
field = field_.encode('utf-8')
# get dims
cdef int[2] dims
acados_solver_common.ocp_nlp_dynamics_dims_get_from_attr(self.nlp_config, self.nlp_dims, self.nlp_out, stage, field, &dims[0])
# create output data
out = np.zeros((dims[0], dims[1]), order='F', dtype=np.float64)
# call getter
acados_solver_common.ocp_nlp_get_at_stage(self.nlp_config, self.nlp_dims, self.nlp_solver, stage, field, <void *> out.data)
return out
def options_set(self, str field_, value_):
"""
Set options of the solver.
:param field: string, e.g. 'print_level', 'rti_phase', 'initialize_t_slacks', 'step_length', 'alpha_min', 'alpha_reduction'
:param value: of type int, float
"""
int_fields = ['print_level', 'rti_phase', 'initialize_t_slacks']
double_fields = ['step_length', 'tol_eq', 'tol_stat', 'tol_ineq', 'tol_comp', 'alpha_min', 'alpha_reduction']
string_fields = ['globalization']
# encode
field = field_.encode('utf-8')
cdef int int_value
cdef double double_value
cdef unsigned char[::1] string_value
# check field availability and type
if field_ in int_fields:
if not isinstance(value_, int):
raise Exception('solver option {} must be of type int. You have {}.'.format(field_, type(value_)))
if field_ == 'rti_phase':
if value_ < 0 or value_ > 2:
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
'take only values 0, 1, 2 for SQP-RTI-type solvers')
if self.acados_ocp.solver_options.nlp_solver_type != 'SQP_RTI' and value_ > 0:
raise Exception('AcadosOcpSolver.solve(): argument \'rti_phase\' can '
'take only value 0 for SQP-type solvers')
int_value = value_
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &int_value)
elif field_ in double_fields:
if not isinstance(value_, float):
raise Exception('solver option {} must be of type float. You have {}.'.format(field_, type(value_)))
double_value = value_
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &double_value)
elif field_ in string_fields:
if not isinstance(value_, bytes):
raise Exception('solver option {} must be of type str. You have {}.'.format(field_, type(value_)))
string_value = value_.encode('utf-8')
acados_solver_common.ocp_nlp_solver_opts_set(self.nlp_config, self.nlp_opts, field, <void *> &string_value[0])
raise Exception('AcadosOcpSolver.options_set() does not support field {}.'\
'\n Possible values are {}.'.format(field_, ', '.join(int_fields + double_fields + string_fields)))
def __del__(self):
if self.solver_created:
acados_solver.acados_free(self.capsule)
acados_solver.acados_free_capsule(self.capsule)
pyextra/acados_template/acados_sim.py
+37 -1
View File
@@ -106,7 +106,8 @@ class AcadosSimOpts:
"""
def __init__(self):
self.__integrator_type = 'ERK'
self.__tf = None
self.__collocation_type = 'GAUSS_LEGENDRE'
self.__Tsim = None
# ints
self.__sim_method_num_stages = 1
self.__sim_method_num_steps = 1
@@ -174,6 +175,15 @@ class AcadosSimOpts:
"""Time horizon"""
return self.__Tsim
@property
def collocation_type(self):
"""Collocation type: relevant for implicit integrators
-- string in {GAUSS_RADAU_IIA, GAUSS_LEGENDRE}
Default: GAUSS_LEGENDRE
"""
return self.__collocation_type
@integrator_type.setter
def integrator_type(self, integrator_type):
integrator_types = ('ERK', 'IRK', 'GNSF')
@@ -183,6 +193,15 @@ class AcadosSimOpts:
raise Exception('Invalid integrator_type value. Possible values are:\n\n' \
+ ',\n'.join(integrator_types) + '.\n\nYou have: ' + integrator_type + '.\n\nExiting.')
@collocation_type.setter
def collocation_type(self, collocation_type):
collocation_types = ('GAUSS_RADAU_IIA', 'GAUSS_LEGENDRE')
if collocation_type in collocation_types:
self.__collocation_type = collocation_type
else:
raise Exception('Invalid collocation_type value. Possible values are:\n\n' \
+ ',\n'.join(collocation_types) + '.\n\nYou have: ' + collocation_type + '.\n\nExiting.')
@T.setter
def T(self, T):
self.__Tsim = T
@@ -262,6 +281,8 @@ class AcadosSim:
- :py:attr:`acados_include_path` (set automatically)
- :py:attr:`acados_lib_path` (set automatically)
- :py:attr:`parameter_values` - used to initialize the parameters (can be changed)
"""
def __init__(self, acados_path=''):
if acados_path == '':
@@ -281,6 +302,21 @@ class AcadosSim:
self.code_export_directory = 'c_generated_code'
"""Path to where code will be exported. Default: `c_generated_code`."""
self.__parameter_values = np.array([])
@property
def parameter_values(self):
""":math:`p` - initial values for parameter - can be updated"""
return self.__parameter_values
@parameter_values.setter
def parameter_values(self, parameter_values):
if isinstance(parameter_values, np.ndarray):
self.__parameter_values = parameter_values
else:
raise Exception('Invalid parameter_values value. ' +
f'Expected numpy array, got {type(parameter_values)}.')
def set(self, attr, value):
# tokenize string
tokens = attr.split('_', 1)
pyextra/acados_template/acados_sim_layout.json
+3
View File
@@ -28,6 +28,9 @@
"integrator_type": [
"str"
],
"collocation_type": [
"str"
],
"Tsim": [
"float"
],
pyextra/acados_template/acados_sim_solver.py
+6 -9
View File
@@ -46,7 +46,7 @@ from .acados_sim import AcadosSim
from .acados_ocp import AcadosOcp
from .acados_model import acados_model_strip_casadi_symbolics
from .utils import is_column, render_template, format_class_dict, np_array_to_list,\
make_model_consistent, set_up_imported_gnsf_model
make_model_consistent, set_up_imported_gnsf_model, get_python_interface_path
def make_sim_dims_consistent(acados_sim):
@@ -84,14 +84,13 @@ def make_sim_dims_consistent(acados_sim):
def get_sim_layout():
current_module = sys.modules[__name__]
acados_path = os.path.dirname(current_module.__file__)
with open(acados_path + '/acados_sim_layout.json', 'r') as f:
python_interface_path = get_python_interface_path()
abs_path = os.path.join(python_interface_path, 'acados_sim_layout.json')
with open(abs_path, 'r') as f:
sim_layout = json.load(f)
return sim_layout
def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
# Load acados_sim structure description
sim_layout = get_sim_layout()
@@ -114,9 +113,7 @@ def sim_formulation_json_dump(acados_sim, json_file='acados_sim.json'):
def sim_render_templates(json_file, model_name, code_export_dir):
# setting up loader and environment
json_path = '{cwd}/{json_file}'.format(
cwd=os.getcwd(),
json_file=json_file)
json_path = os.path.join(os.getcwd(), json_file)
if not os.path.exists(json_path):
raise Exception(f"{json_path} not found!")
@@ -141,7 +138,7 @@ def sim_render_templates(json_file, model_name, code_export_dir):
render_template(in_file, out_file, template_dir, json_path)
## folder model
template_dir = f'{code_export_dir}/{model_name}_model/'
template_dir = os.path.join(code_export_dir, model_name + '_model')
in_file = 'model.in.h'
out_file = f'{model_name}_model.h'
pyextra/acados_template/acados_solver_common.pxd
+102
View File
@@ -0,0 +1,102 @@
# -*- coding: future_fstrings -*-
#
# Copyright 2019 Gianluca Frison, Dimitris Kouzoupis, Robin Verschueren,
# Andrea Zanelli, Niels van Duijkeren, Jonathan Frey, Tommaso Sartor,
# Branimir Novoselnik, Rien Quirynen, Rezart Qelibari, Dang Doan,
# Jonas Koenemann, Yutao Chen, Tobias Schöls, Jonas Schlagenhauf, Moritz Diehl
#
# This file is part of acados.
#
# The 2-Clause BSD License
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
# this list of conditions and the following disclaimer.
#
# 2. Redistributions in binary form must reproduce the above copyright notice,
# this list of conditions and the following disclaimer in the documentation
# and/or other materials provided with the distribution.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.;
#
cdef extern from "acados/ocp_nlp/ocp_nlp_common.h":
ctypedef struct ocp_nlp_config:
pass
ctypedef struct ocp_nlp_dims:
pass
ctypedef struct ocp_nlp_in:
pass
ctypedef struct ocp_nlp_out:
pass
cdef extern from "acados_c/ocp_nlp_interface.h":
ctypedef enum ocp_nlp_solver_t:
pass
ctypedef enum ocp_nlp_cost_t:
pass
ctypedef enum ocp_nlp_dynamics_t:
pass
ctypedef enum ocp_nlp_constraints_t:
pass
ctypedef enum ocp_nlp_reg_t:
pass
ctypedef struct ocp_nlp_plan:
pass
ctypedef struct ocp_nlp_solver:
pass
int ocp_nlp_cost_model_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in_,
int start_stage, const char *field, void *value)
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)
# out
void ocp_nlp_out_set(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, void *value)
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_get_at_stage(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_solver *solver,
int stage, const char *field, void *value)
int ocp_nlp_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field)
void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out)
# opts
void ocp_nlp_solver_opts_set(ocp_nlp_config *config, void *opts_, const char *field, void* value)
# solver
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out)
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in_, ocp_nlp_out *nlp_out)
# get/set
void ocp_nlp_get(ocp_nlp_config *config, ocp_nlp_solver *solver, const char *field, void *return_value_)
void ocp_nlp_set(ocp_nlp_config *config, ocp_nlp_solver *solver, int stage, const char *field, void *value)
pyextra/acados_template/c_templates_tera/Makefile.in
+30 -5
View File
@@ -279,12 +279,12 @@ all: clean casadi_fun example_sim example
shared_lib: bundled_shared_lib ocp_shared_lib sim_shared_lib
{%- endif %}
CASADI_MODEL_SOURCE=
CASADI_MODEL_SOURCE=
{%- if solver_options.integrator_type == "ERK" %}
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_fun.c
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_vde_forw.c
{%- if hessian_approx == "EXACT" %}
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
CASADI_MODEL_SOURCE+= {{ model.name }}_expl_ode_hess.c
{%- endif %}
{%- elif solver_options.integrator_type == "IRK" %}
CASADI_MODEL_SOURCE+= {{ model.name }}_impl_dae_fun.c
@@ -453,7 +453,7 @@ ocp_shared_lib: casadi_fun ocp_solver
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
{{ link_libs }} \
-lm \
{%- else %}
ocp_shared_lib: casadi_fun ocp_solver
@@ -465,9 +465,34 @@ ocp_shared_lib: casadi_fun ocp_solver
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
{{ link_libs }} \
-lm \
{%- endif %}
ocp_cython_c: ocp_shared_lib
cython \
-o acados_ocp_solver_pyx.c \
-I $(INCLUDE_PATH)/../interfaces/acados_template/acados_template \
$(INCLUDE_PATH)/../interfaces/acados_template/acados_template/acados_ocp_solver_pyx.pyx \
ocp_cython_o: ocp_cython_c
clang $(ACADOS_FLAGS) -c -O2 \
-o acados_ocp_solver_pyx.o \
-I /usr/include/python3.8 \
-I $(INCLUDE_PATH)/blasfeo/include/ \
-I $(INCLUDE_PATH)/hpipm/include/ \
-I $(INCLUDE_PATH) \
acados_ocp_solver_pyx.c \
ocp_cython: ocp_cython_o
clang $(ACADOS_FLAGS) -shared \
-o acados_ocp_solver_pyx.so \
-Wl,-rpath=$(LIB_PATH) \
acados_ocp_solver_pyx.o \
$(abspath .)/libacados_ocp_solver_{{ model.name }}.so \
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo -lqpOASES_e \
{{ link_libs }} \
-lm \
sim_shared_lib: casadi_fun sim_solver
gcc $(ACADOS_FLAGS) -shared -o libacados_sim_solver_{{ model.name }}.so $(SIM_OBJ) $(MODEL_OBJ) -L$(EXTERNAL_DIR) -l$(EXTERNAL_LIB) \
-L $(LIB_PATH) -lacados -lhpipm -lblasfeo \
pyextra/acados_template/c_templates_tera/acados_mex_create.in.c
+1 -1
View File
@@ -52,7 +52,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
int status = 0;
// create solver
nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
status = {{ model.name }}_acados_create(acados_ocp_capsule);
pyextra/acados_template/c_templates_tera/acados_mex_free.in.c
+1 -1
View File
@@ -51,7 +51,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
const mxArray *C_ocp = prhs[0];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
status = {{ model.name }}_acados_free(capsule);
if (status)
pyextra/acados_template/c_templates_tera/acados_mex_set.in.c
+1 -1
View File
@@ -66,7 +66,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
const mxArray *C_ocp = prhs[2];
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// plan
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "plan" ) );
ocp_nlp_plan *plan = (ocp_nlp_plan *) ptr[0];
pyextra/acados_template/c_templates_tera/acados_mex_solve.in.c
+1 -1
View File
@@ -51,7 +51,7 @@ void mexFunction(int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[])
// capsule
ptr = (long long *) mxGetData( mxGetField( C_ocp, 0, "capsule" ) );
nlp_solver_capsule *capsule = (nlp_solver_capsule *) ptr[0];
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) ptr[0];
// solve
{{ model.name }}_acados_solve(capsule);
pyextra/acados_template/c_templates_tera/acados_sim_solver.in.c
+28 -42
View File
@@ -78,9 +78,10 @@ int {{ model.name }}_acados_sim_solver_free_capsule(sim_solver_capsule * capsule
int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
{
// initialize
int nx = {{ dims.nx }};
int nu = {{ dims.nu }};
int nz = {{ dims.nz }};
const int nx = {{ model.name | upper }}_NX;
const int nu = {{ model.name | upper }}_NU;
const int nz = {{ model.name | upper }}_NZ;
const int np = {{ model.name | upper }}_NP;
bool tmp_bool;
{#// double Tsim = {{ solver_options.tf / dims.N }};#}
@@ -98,7 +99,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_fun->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_sparsity_out;
capsule->sim_impl_dae_fun->casadi_n_in = &{{ model.name }}_impl_dae_fun_n_in;
capsule->sim_impl_dae_fun->casadi_n_out = &{{ model.name }}_impl_dae_fun_n_out;
external_function_param_casadi_create(capsule->sim_impl_dae_fun, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_impl_dae_fun, np);
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_fun = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_work = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_work;
@@ -106,7 +107,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_sparsity_out;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_in = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_in;
capsule->sim_impl_dae_fun_jac_x_xdot_z->casadi_n_out = &{{ model.name }}_impl_dae_fun_jac_x_xdot_z_n_out;
external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_impl_dae_fun_jac_x_xdot_z, np);
// external_function_param_casadi impl_dae_jac_x_xdot_u_z;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_fun = &{{ model.name }}_impl_dae_jac_x_xdot_u_z;
@@ -115,7 +116,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_sparsity_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_sparsity_out;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_in = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_in;
capsule->sim_impl_dae_jac_x_xdot_u_z->casadi_n_out = &{{ model.name }}_impl_dae_jac_x_xdot_u_z_n_out;
external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_impl_dae_jac_x_xdot_u_z, np);
{%- if hessian_approx == "EXACT" %}
capsule->sim_impl_dae_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
@@ -126,7 +127,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_impl_dae_hess->casadi_sparsity_out = &{{ model.name }}_impl_dae_hess_sparsity_out;
capsule->sim_impl_dae_hess->casadi_n_in = &{{ model.name }}_impl_dae_hess_n_in;
capsule->sim_impl_dae_hess->casadi_n_out = &{{ model.name }}_impl_dae_hess_n_out;
external_function_param_casadi_create(capsule->sim_impl_dae_hess, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_impl_dae_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "ERK" %}
@@ -140,7 +141,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_forw_vde_casadi->casadi_sparsity_in = &{{ model.name }}_expl_vde_forw_sparsity_in;
capsule->sim_forw_vde_casadi->casadi_sparsity_out = &{{ model.name }}_expl_vde_forw_sparsity_out;
capsule->sim_forw_vde_casadi->casadi_work = &{{ model.name }}_expl_vde_forw_work;
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_forw_vde_casadi, np);
capsule->sim_expl_ode_fun_casadi->casadi_fun = &{{ model.name }}_expl_ode_fun;
capsule->sim_expl_ode_fun_casadi->casadi_n_in = &{{ model.name }}_expl_ode_fun_n_in;
@@ -148,7 +149,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_in = &{{ model.name }}_expl_ode_fun_sparsity_in;
capsule->sim_expl_ode_fun_casadi->casadi_sparsity_out = &{{ model.name }}_expl_ode_fun_sparsity_out;
capsule->sim_expl_ode_fun_casadi->casadi_work = &{{ model.name }}_expl_ode_fun_work;
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_expl_ode_fun_casadi, np);
{%- if hessian_approx == "EXACT" %}
capsule->sim_expl_ode_hess = (external_function_param_casadi *) malloc(sizeof(external_function_param_casadi));
@@ -159,7 +160,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_expl_ode_hess->casadi_sparsity_out = &{{ model.name }}_expl_ode_hess_sparsity_out;
capsule->sim_expl_ode_hess->casadi_n_in = &{{ model.name }}_expl_ode_hess_n_in;
capsule->sim_expl_ode_hess->casadi_n_out = &{{ model.name }}_expl_ode_hess_n_out;
external_function_param_casadi_create(capsule->sim_expl_ode_hess, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_expl_ode_hess, np);
{%- endif %}
{% elif solver_options.integrator_type == "GNSF" -%}
@@ -175,7 +176,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_sparsity_in;
capsule->sim_gnsf_phi_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_sparsity_out;
capsule->sim_gnsf_phi_fun->casadi_work = &{{ model.name }}_gnsf_phi_fun_work;
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun, np);
capsule->sim_gnsf_phi_fun_jac_y->casadi_fun = &{{ model.name }}_gnsf_phi_fun_jac_y;
capsule->sim_gnsf_phi_fun_jac_y->casadi_n_in = &{{ model.name }}_gnsf_phi_fun_jac_y_n_in;
@@ -183,7 +184,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_in;
capsule->sim_gnsf_phi_fun_jac_y->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_fun_jac_y_sparsity_out;
capsule->sim_gnsf_phi_fun_jac_y->casadi_work = &{{ model.name }}_gnsf_phi_fun_jac_y_work;
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_gnsf_phi_fun_jac_y, np);
capsule->sim_gnsf_phi_jac_y_uhat->casadi_fun = &{{ model.name }}_gnsf_phi_jac_y_uhat;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_n_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_n_in;
@@ -191,7 +192,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_in = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_in;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_sparsity_out = &{{ model.name }}_gnsf_phi_jac_y_uhat_sparsity_out;
capsule->sim_gnsf_phi_jac_y_uhat->casadi_work = &{{ model.name }}_gnsf_phi_jac_y_uhat_work;
external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_gnsf_phi_jac_y_uhat, np);
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_fun = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_n_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_n_in;
@@ -199,7 +200,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_in = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_in;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_sparsity_out = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_sparsity_out;
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z->casadi_work = &{{ model.name }}_gnsf_f_lo_fun_jac_x1k1uz_work;
external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, np);
capsule->sim_gnsf_get_matrices_fun->casadi_fun = &{{ model.name }}_gnsf_get_matrices_fun;
capsule->sim_gnsf_get_matrices_fun->casadi_n_in = &{{ model.name }}_gnsf_get_matrices_fun_n_in;
@@ -207,7 +208,7 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_in = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_in;
capsule->sim_gnsf_get_matrices_fun->casadi_sparsity_out = &{{ model.name }}_gnsf_get_matrices_fun_sparsity_out;
capsule->sim_gnsf_get_matrices_fun->casadi_work = &{{ model.name }}_gnsf_get_matrices_fun_work;
external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, {{ dims.np }});
external_function_param_casadi_create(capsule->sim_gnsf_get_matrices_fun, np);
{% endif %}
// sim plan & config
@@ -243,6 +244,8 @@ 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);
sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
sim_opts_set({{ model.name }}_sim_config, {{ model.name }}_sim_opts, "collocation_type", &collocation_type);
{% if problem_class == "SIM" %}
tmp_int = {{ solver_options.sim_method_num_stages }};
@@ -321,35 +324,18 @@ int {{ model.name }}_acados_sim_create(sim_solver_capsule * capsule)
{{ model.name }}_sim_dims, {{ model.name }}_sim_opts);
capsule->acados_sim_solver = {{ model.name }}_sim_solver;
{% if dims.np > 0 %}
/* initialize parameter values */
{% 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 %}
{%- if solver_options.integrator_type == "ERK" %}
capsule->sim_forw_vde_casadi[0].set_param(capsule->sim_forw_vde_casadi, p);
capsule->sim_expl_ode_fun_casadi[0].set_param(capsule->sim_expl_ode_fun_casadi, p);
{%- if hessian_approx == "EXACT" %}
capsule->sim_expl_ode_hess[0].set_param(capsule->sim_expl_ode_hess, p);
{%- endif %}
{%- elif solver_options.integrator_type == "IRK" %}
capsule->sim_impl_dae_fun[0].set_param(capsule->sim_impl_dae_fun, p);
capsule->sim_impl_dae_fun_jac_x_xdot_z[0].set_param(capsule->sim_impl_dae_fun_jac_x_xdot_z, p);
capsule->sim_impl_dae_jac_x_xdot_u_z[0].set_param(capsule->sim_impl_dae_jac_x_xdot_u_z, p);
{%- if hessian_approx == "EXACT" %}
capsule->sim_impl_dae_hess[0].set_param(capsule->sim_impl_dae_hess, p);
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
capsule->sim_gnsf_phi_fun[0].set_param(capsule->sim_gnsf_phi_fun, p);
capsule->sim_gnsf_phi_fun_jac_y[0].set_param(capsule->sim_gnsf_phi_fun_jac_y, p);
capsule->sim_gnsf_phi_jac_y_uhat[0].set_param(capsule->sim_gnsf_phi_jac_y_uhat, p);
capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z[0].set_param(capsule->sim_gnsf_f_lo_jac_x1_x1dot_u_z, p);
capsule->sim_gnsf_get_matrices_fun[0].set_param(capsule->sim_gnsf_get_matrices_fun, p);
{% endif %}
{% endif %}{# if dims.np #}
{{ model.name }}_acados_sim_update_params(capsule, p, np);
free(p);
{% endif %}{# if dims.np #}
/* initialize input */
// x
@@ -437,7 +423,7 @@ int {{ model.name }}_acados_sim_free(sim_solver_capsule *capsule)
int {{ model.name }}_acados_sim_update_params(sim_solver_capsule *capsule, double *p, int np)
{
int status = 0;
int casadi_np = {{ dims.np }};
int casadi_np = {{ model.name | upper }}_NP;
if (casadi_np != np) {
printf("{{ model.name }}_acados_sim_update_params: trying to set %i parameters for external functions."
pyextra/acados_template/c_templates_tera/acados_sim_solver.in.h
+5
View File
@@ -37,6 +37,11 @@
#include "acados_c/sim_interface.h"
#include "acados_c/external_function_interface.h"
#define {{ model.name | upper }}_NX {{ dims.nx }}
#define {{ model.name | upper }}_NZ {{ dims.nz }}
#define {{ model.name | upper }}_NU {{ dims.nu }}
#define {{ model.name | upper }}_NP {{ dims.np }}
#ifdef __cplusplus
extern "C" {
#endif
pyextra/acados_template/c_templates_tera/acados_solver.in.c
+121 -76
View File
@@ -71,59 +71,97 @@
#include "acados_solver_{{ model.name }}.h"
#define NX {{ dims.nx }}
#define NZ {{ dims.nz }}
#define NU {{ dims.nu }}
#define NP {{ dims.np }}
#define NBX {{ dims.nbx }}
#define NBX0 {{ dims.nbx_0 }}
#define NBU {{ dims.nbu }}
#define NSBX {{ dims.nsbx }}
#define NSBU {{ dims.nsbu }}
#define NSH {{ dims.nsh }}
#define NSG {{ dims.nsg }}
#define NSPHI {{ dims.nsphi }}
#define NSHN {{ dims.nsh_e }}
#define NSGN {{ dims.nsg_e }}
#define NSPHIN {{ dims.nsphi_e }}
#define NSBXN {{ dims.nsbx_e }}
#define NS {{ dims.ns }}
#define NSN {{ dims.ns_e }}
#define NG {{ dims.ng }}
#define NBXN {{ dims.nbx_e }}
#define NGN {{ dims.ng_e }}
#define NY0 {{ dims.ny_0 }}
#define NY {{ dims.ny }}
#define NYN {{ dims.ny_e }}
#define N {{ dims.N }}
#define NH {{ dims.nh }}
#define NPHI {{ dims.nphi }}
#define NHN {{ dims.nh_e }}
#define NPHIN {{ dims.nphi_e }}
#define NR {{ dims.nr }}
#define NX {{ model.name | upper }}_NX
#define NZ {{ model.name | upper }}_NZ
#define NU {{ model.name | upper }}_NU
#define NP {{ model.name | upper }}_NP
#define NBX {{ model.name | upper }}_NBX
#define NBX0 {{ model.name | upper }}_NBX0
#define NBU {{ model.name | upper }}_NBU
#define NSBX {{ model.name | upper }}_NSBX
#define NSBU {{ model.name | upper }}_NSBU
#define NSH {{ model.name | upper }}_NSH
#define NSG {{ model.name | upper }}_NSG
#define NSPHI {{ model.name | upper }}_NSPHI
#define NSHN {{ model.name | upper }}_NSHN
#define NSGN {{ model.name | upper }}_NSGN
#define NSPHIN {{ model.name | upper }}_NSPHIN
#define NSBXN {{ model.name | upper }}_NSBXN
#define NS {{ model.name | upper }}_NS
#define NSN {{ model.name | upper }}_NSN
#define NG {{ model.name | upper }}_NG
#define NBXN {{ model.name | upper }}_NBXN
#define NGN {{ model.name | upper }}_NGN
#define NY0 {{ model.name | upper }}_NY0
#define NY {{ model.name | upper }}_NY
#define NYN {{ model.name | upper }}_NYN
// #define N {{ model.name | upper }}_N
#define NH {{ model.name | upper }}_NH
#define NPHI {{ model.name | upper }}_NPHI
#define NHN {{ model.name | upper }}_NHN
#define NPHIN {{ model.name | upper }}_NPHIN
#define NR {{ model.name | upper }}_NR
// ** solver data **
nlp_solver_capsule * {{ model.name }}_acados_create_capsule()
{{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void)
{
void* capsule_mem = malloc(sizeof(nlp_solver_capsule));
nlp_solver_capsule *capsule = (nlp_solver_capsule *) capsule_mem;
void* capsule_mem = malloc(sizeof({{ model.name }}_solver_capsule));
{{ model.name }}_solver_capsule *capsule = ({{ model.name }}_solver_capsule *) capsule_mem;
return capsule;
}
int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule)
int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule)
{
free(capsule);
return 0;
}
int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule)
{
int N_shooting_intervals = {{ model.name | upper }}_N;
double* new_time_steps = NULL; // NULL -> don't alter the code generated time-steps
return {{ model.name }}_acados_create_with_discretization(capsule, N_shooting_intervals, new_time_steps);
}
int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
{
if (N != capsule->nlp_solver_plan->N) {
fprintf(stderr, "{{ model.name }}_acados_update_time_steps: given number of time steps (= %d) " \
"differs from the currently allocated number of " \
"time steps (= %d)!\n" \
"Please recreate with new discretization and provide a new vector of time_stamps!\n",
N, capsule->nlp_solver_plan->N);
return 1;
}
ocp_nlp_config * nlp_config = capsule->nlp_config;
ocp_nlp_dims * nlp_dims = capsule->nlp_dims;
ocp_nlp_in * nlp_in = capsule->nlp_in;
for (int i = 0; i < N; i++)
{
ocp_nlp_in_set(nlp_config, nlp_dims, nlp_in, i, "Ts", &new_time_steps[i]);
ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "scaling", &new_time_steps[i]);
}
return 0;
}
int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps)
{
int status = 0;
// If N does not match the number of shooting intervals used for code generation, new_time_steps must be given.
if (N != {{ model.name | upper }}_N && !new_time_steps) {
fprintf(stderr, "{{ model.name }}_acados_create_with_discretization: new_time_steps is NULL " \
"but the number of shooting intervals (= %d) differs from the number of " \
"shooting intervals (= %d) during code generation! Please provide a new vector of time_stamps!\n", \
N, {{ model.name | upper }}_N);
return 1;
}
// number of expected runtime parameters
capsule->nlp_np = NP;
@@ -895,28 +933,27 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
{%- endif %}
{%- endfor %}
if (new_time_steps) {
{{ model.name }}_acados_update_time_steps(capsule, N, new_time_steps);
} else {
{%- 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);
}
// 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 %}
for (int i = 0; i < N; i++)
{
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);
// 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 %}
{{ model.name }}_acados_update_time_steps(capsule, N, time_steps);
free(time_steps);
{%- endif %}
}
/**** Dynamics ****/
for (int i = 0; i < N; i++)
@@ -1879,6 +1916,11 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
{%- if solver_options.integrator_type != "DISCRETE" %}
// set collocation type (relevant for implicit integrators)
sim_collocation_type collocation_type = {{ solver_options.collocation_type }};
for (int i = 0; i < N; i++)
ocp_nlp_solver_opts_set_at_stage(nlp_config, capsule->nlp_opts, i, "dynamics_collocation_type", &collocation_type);
// set up sim_method_num_steps
{%- set all_equal = true %}
{%- set val = solver_options.sim_method_num_steps[0] %}
@@ -2113,7 +2155,7 @@ int {{ model.name }}_acados_create(nlp_solver_capsule * capsule)
}
int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *p, int np)
int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *p, int np)
{
int solver_status = 0;
@@ -2125,7 +2167,8 @@ int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stag
}
{%- if dims.np > 0 %}
if (stage < {{ dims.N }} && stage >= 0)
const int N = capsule->nlp_solver_plan->N;
if (stage < N && stage >= 0)
{
{%- if solver_options.integrator_type == "IRK" %}
capsule->impl_dae_fun[stage].set_param(capsule->impl_dae_fun+stage, p);
@@ -2228,7 +2271,7 @@ int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stag
int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule)
int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule)
{
// solve NLP
int solver_status = ocp_nlp_solve(capsule->nlp_solver, capsule->nlp_in, capsule->nlp_out);
@@ -2237,8 +2280,10 @@ int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule)
}
int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule)
{
// before destroying, keep some info
const int N = capsule->nlp_solver_plan->N;
// free memory
ocp_nlp_solver_opts_destroy(capsule->nlp_opts);
ocp_nlp_in_destroy(capsule->nlp_in);
@@ -2251,7 +2296,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
/* free external function */
// dynamics
{%- if solver_options.integrator_type == "IRK" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->impl_dae_fun[i]);
external_function_param_casadi_free(&capsule->impl_dae_fun_jac_x_xdot_z[i]);
@@ -2268,7 +2313,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif solver_options.integrator_type == "LIFTED_IRK" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < 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]);
@@ -2277,7 +2322,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->impl_dae_fun_jac_x_xdot_u);
{%- elif solver_options.integrator_type == "ERK" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->forw_vde_casadi[i]);
external_function_param_casadi_free(&capsule->expl_ode_fun[i]);
@@ -2292,7 +2337,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif solver_options.integrator_type == "GNSF" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->gnsf_phi_fun[i]);
external_function_param_casadi_free(&capsule->gnsf_phi_fun_jac_y[i]);
@@ -2306,7 +2351,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->gnsf_f_lo_jac_x1_x1dot_u_z);
free(capsule->gnsf_get_matrices_fun);
{%- elif solver_options.integrator_type == "DISCRETE" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun[i]);
external_function_param_{{ model.dyn_ext_fun_type }}_free(&capsule->discr_dyn_phi_fun_jac_ut_xt[i]);
@@ -2333,7 +2378,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
external_function_param_{{ cost.cost_ext_fun_type_0 }}_free(&capsule->ext_cost_0_fun_jac_hess);
{%- endif %}
{%- if cost.cost_type == "NONLINEAR_LS" %}
for (int i = 0; i < {{ dims.N }} - 1; i++)
for (int i = 0; i < N - 1; i++)
{
external_function_param_casadi_free(&capsule->cost_y_fun[i]);
external_function_param_casadi_free(&capsule->cost_y_fun_jac_ut_xt[i]);
@@ -2343,7 +2388,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
free(capsule->cost_y_fun_jac_ut_xt);
free(capsule->cost_y_hess);
{%- elif cost.cost_type == "EXTERNAL" %}
for (int i = 0; i < {{ dims.N }} - 1; i++)
for (int i = 0; i < N - 1; i++)
{
external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun[i]);
external_function_param_{{ cost.cost_ext_fun_type }}_free(&capsule->ext_cost_fun_jac[i]);
@@ -2365,13 +2410,13 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
// constraints
{%- if constraints.constr_type == "BGH" and dims.nh > 0 %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac[i]);
external_function_param_casadi_free(&capsule->nl_constr_h_fun[i]);
}
{%- if solver_options.hessian_approx == "EXACT" %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->nl_constr_h_fun_jac_hess[i]);
}
@@ -2383,7 +2428,7 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
{%- endif %}
{%- elif constraints.constr_type == "BGP" and dims.nphi > 0 %}
for (int i = 0; i < {{ dims.N }}; i++)
for (int i = 0; i < N; i++)
{
external_function_param_casadi_free(&capsule->phi_constraint[i]);
}
@@ -2403,16 +2448,16 @@ int {{ model.name }}_acados_free(nlp_solver_capsule * capsule)
return 0;
}
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule) { return capsule->nlp_in; }
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule) { return capsule->nlp_out; }
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule) { return capsule->nlp_solver; }
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule) { return capsule->nlp_config; }
void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule) { return capsule->nlp_dims; }
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_in; }
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_out; }
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver; }
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_config; }
void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_opts; }
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_dims; }
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule) { return capsule->nlp_solver_plan; }
void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule)
void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule)
{
int sqp_iter, stat_m, stat_n, tmp_int;
ocp_nlp_get(capsule->nlp_config, capsule->nlp_solver, "sqp_iter", &sqp_iter);
pyextra/acados_template/c_templates_tera/acados_solver.in.h
+90 -17
View File
@@ -37,12 +37,43 @@
#include "acados_c/ocp_nlp_interface.h"
#include "acados_c/external_function_interface.h"
#define {{ model.name | upper }}_NX {{ dims.nx }}
#define {{ model.name | upper }}_NZ {{ dims.nz }}
#define {{ model.name | upper }}_NU {{ dims.nu }}
#define {{ model.name | upper }}_NP {{ dims.np }}
#define {{ model.name | upper }}_NBX {{ dims.nbx }}
#define {{ model.name | upper }}_NBX0 {{ dims.nbx_0 }}
#define {{ model.name | upper }}_NBU {{ dims.nbu }}
#define {{ model.name | upper }}_NSBX {{ dims.nsbx }}
#define {{ model.name | upper }}_NSBU {{ dims.nsbu }}
#define {{ model.name | upper }}_NSH {{ dims.nsh }}
#define {{ model.name | upper }}_NSG {{ dims.nsg }}
#define {{ model.name | upper }}_NSPHI {{ dims.nsphi }}
#define {{ model.name | upper }}_NSHN {{ dims.nsh_e }}
#define {{ model.name | upper }}_NSGN {{ dims.nsg_e }}
#define {{ model.name | upper }}_NSPHIN {{ dims.nsphi_e }}
#define {{ model.name | upper }}_NSBXN {{ dims.nsbx_e }}
#define {{ model.name | upper }}_NS {{ dims.ns }}
#define {{ model.name | upper }}_NSN {{ dims.ns_e }}
#define {{ model.name | upper }}_NG {{ dims.ng }}
#define {{ model.name | upper }}_NBXN {{ dims.nbx_e }}
#define {{ model.name | upper }}_NGN {{ dims.ng_e }}
#define {{ model.name | upper }}_NY0 {{ dims.ny_0 }}
#define {{ model.name | upper }}_NY {{ dims.ny }}
#define {{ model.name | upper }}_NYN {{ dims.ny_e }}
#define {{ model.name | upper }}_N {{ dims.N }}
#define {{ model.name | upper }}_NH {{ dims.nh }}
#define {{ model.name | upper }}_NPHI {{ dims.nphi }}
#define {{ model.name | upper }}_NHN {{ dims.nh_e }}
#define {{ model.name | upper }}_NPHIN {{ dims.nphi_e }}
#define {{ model.name | upper }}_NR {{ dims.nr }}
#ifdef __cplusplus
extern "C" {
#endif
// ** capsule for solver data **
typedef struct nlp_solver_capsule
typedef struct {{ model.name }}_solver_capsule
{
// acados objects
ocp_nlp_in *nlp_in;
@@ -58,73 +89,115 @@ typedef struct nlp_solver_capsule
/* external functions */
// dynamics
{% if solver_options.integrator_type == "ERK" %}
external_function_param_casadi *forw_vde_casadi;
external_function_param_casadi *expl_ode_fun;
{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *hess_vde_casadi;
{%- endif %}
{% elif solver_options.integrator_type == "IRK" %}
external_function_param_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;
{% if solver_options.hessian_approx == "EXACT" %}
external_function_param_casadi *impl_dae_hess;
{%- endif %}
{% elif solver_options.integrator_type == "LIFTED_IRK" %}
external_function_param_casadi *impl_dae_fun;
external_function_param_casadi *impl_dae_fun_jac_x_xdot_u;
{% elif solver_options.integrator_type == "GNSF" %}
external_function_param_casadi *gnsf_phi_fun;
external_function_param_casadi *gnsf_phi_fun_jac_y;
external_function_param_casadi *gnsf_phi_jac_y_uhat;
external_function_param_casadi *gnsf_f_lo_jac_x1_x1dot_u_z;
external_function_param_casadi *gnsf_get_matrices_fun;
{% elif solver_options.integrator_type == "DISCRETE" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun;
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt;
{%- if solver_options.hessian_approx == "EXACT" %}
external_function_param_{{ model.dyn_ext_fun_type }} *discr_dyn_phi_fun_jac_ut_xt_hess;
{%- endif %}
{%- endif %}
// cost
{% if cost.cost_type == "NONLINEAR_LS" %}
external_function_param_casadi *cost_y_fun;
external_function_param_casadi *cost_y_fun_jac_ut_xt;
external_function_param_casadi *cost_y_hess;
{%- elif cost.cost_type == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type }} *ext_cost_fun_jac_hess;
{% endif %}
{% if cost.cost_type_0 == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_0_fun;
external_function_param_casadi cost_y_0_fun_jac_ut_xt;
external_function_param_casadi cost_y_0_hess;
{% elif cost.cost_type_0 == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_0 }} ext_cost_0_fun_jac_hess;
{%- endif %}
{% if cost.cost_type_e == "NONLINEAR_LS" %}
external_function_param_casadi cost_y_e_fun;
external_function_param_casadi cost_y_e_fun_jac_ut_xt;
external_function_param_casadi cost_y_e_hess;
{% elif cost.cost_type_e == "EXTERNAL" %}
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac;
external_function_param_{{ cost.cost_ext_fun_type_e }} ext_cost_e_fun_jac_hess;
{%- endif %}
// constraints
{%- if constraints.constr_type == "BGP" %}
external_function_param_casadi *phi_constraint;
{% elif constraints.constr_type == "BGH" and dims.nh > 0 %}
external_function_param_casadi *nl_constr_h_fun_jac;
external_function_param_casadi *nl_constr_h_fun;
external_function_param_casadi *nl_constr_h_fun_jac_hess;
{%- endif %}
{% if constraints.constr_type_e == "BGP" %}
external_function_param_casadi phi_e_constraint;
{% elif constraints.constr_type_e == "BGH" and dims.nh_e > 0 %}
external_function_param_casadi nl_constr_h_e_fun_jac;
external_function_param_casadi nl_constr_h_e_fun;
external_function_param_casadi nl_constr_h_e_fun_jac_hess;
} nlp_solver_capsule;
{%- endif %}
nlp_solver_capsule * {{ model.name }}_acados_create_capsule(void);
int {{ model.name }}_acados_free_capsule(nlp_solver_capsule *capsule);
} {{ model.name }}_solver_capsule;
int {{ model.name }}_acados_create(nlp_solver_capsule * capsule);
int {{ model.name }}_acados_update_params(nlp_solver_capsule * capsule, int stage, double *value, int np);
int {{ model.name }}_acados_solve(nlp_solver_capsule * capsule);
int {{ model.name }}_acados_free(nlp_solver_capsule * capsule);
void {{ model.name }}_acados_print_stats(nlp_solver_capsule * capsule);
{{ model.name }}_solver_capsule * {{ model.name }}_acados_create_capsule(void);
int {{ model.name }}_acados_free_capsule({{ model.name }}_solver_capsule *capsule);
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in(nlp_solver_capsule * capsule);
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out(nlp_solver_capsule * capsule);
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver(nlp_solver_capsule * capsule);
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config(nlp_solver_capsule * capsule);
void *{{ model.name }}_acados_get_nlp_opts(nlp_solver_capsule * capsule);
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims(nlp_solver_capsule * capsule);
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan(nlp_solver_capsule * capsule);
int {{ model.name }}_acados_create({{ model.name }}_solver_capsule * capsule);
/**
* Generic version of {{ model.name }}_acados_create which allows to use a different number of shooting intervals than
* the number used for code generation. If new_time_steps=NULL and n_time_steps matches the number used for code
* generation, the time-steps from code generation is used.
*/
int {{ model.name }}_acados_create_with_discretization({{ model.name }}_solver_capsule * capsule, int n_time_steps, double* new_time_steps);
/**
* Update the time step vector. Number N must be identical to the currently set number of shooting nodes in the
* nlp_solver_plan. Returns 0 if no error occurred and a otherwise a value other than 0.
*/
int {{ model.name }}_acados_update_time_steps({{ model.name }}_solver_capsule * capsule, int N, double* new_time_steps);
int {{ model.name }}_acados_update_params({{ model.name }}_solver_capsule * capsule, int stage, double *value, int np);
int {{ model.name }}_acados_solve({{ model.name }}_solver_capsule * capsule);
int {{ model.name }}_acados_free({{ model.name }}_solver_capsule * capsule);
void {{ model.name }}_acados_print_stats({{ model.name }}_solver_capsule * capsule);
ocp_nlp_in *{{ model.name }}_acados_get_nlp_in({{ model.name }}_solver_capsule * capsule);
ocp_nlp_out *{{ model.name }}_acados_get_nlp_out({{ model.name }}_solver_capsule * capsule);
ocp_nlp_solver *{{ model.name }}_acados_get_nlp_solver({{ model.name }}_solver_capsule * capsule);
ocp_nlp_config *{{ model.name }}_acados_get_nlp_config({{ model.name }}_solver_capsule * capsule);
void *{{ model.name }}_acados_get_nlp_opts({{ model.name }}_solver_capsule * capsule);
ocp_nlp_dims *{{ model.name }}_acados_get_nlp_dims({{ model.name }}_solver_capsule * capsule);
ocp_nlp_plan *{{ model.name }}_acados_get_nlp_plan({{ model.name }}_solver_capsule * capsule);
#ifdef __cplusplus
} /* extern "C" */
pyextra/acados_template/c_templates_tera/acados_solver.in.pxd
+22
View File
@@ -0,0 +1,22 @@
cimport acados_solver_common
cdef extern from "acados_solver_{{ model.name }}.h":
ctypedef struct nlp_solver_capsule "{{ model.name }}_solver_capsule":
pass
nlp_solver_capsule * acados_create_capsule "{{ model.name }}_acados_create_capsule"()
int acados_free_capsule "{{ model.name }}_acados_free_capsule"(nlp_solver_capsule *capsule)
int acados_create "{{ model.name }}_acados_create"(nlp_solver_capsule * capsule)
int acados_update_params "{{ model.name }}_acados_update_params"(nlp_solver_capsule * capsule, int stage, double *value, int np_)
int acados_solve "{{ model.name }}_acados_solve"(nlp_solver_capsule * capsule)
int acados_free "{{ model.name }}_acados_free"(nlp_solver_capsule * capsule)
void acados_print_stats "{{ model.name }}_acados_print_stats"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_in *acados_get_nlp_in "{{ model.name }}_acados_get_nlp_in"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_out *acados_get_nlp_out "{{ model.name }}_acados_get_nlp_out"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_solver *acados_get_nlp_solver "{{ model.name }}_acados_get_nlp_solver"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_config *acados_get_nlp_config "{{ model.name }}_acados_get_nlp_config"(nlp_solver_capsule * capsule)
void *acados_get_nlp_opts "{{ model.name }}_acados_get_nlp_opts"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_dims *acados_get_nlp_dims "{{ model.name }}_acados_get_nlp_dims"(nlp_solver_capsule * capsule)
acados_solver_common.ocp_nlp_plan *acados_get_nlp_plan "{{ model.name }}_acados_get_nlp_plan"(nlp_solver_capsule * capsule)
pyextra/acados_template/c_templates_tera/acados_solver_sfun.in.c
+3 -3
View File
@@ -377,7 +377,7 @@ static void mdlInitializeSampleTimes(SimStruct *S)
static void mdlStart(SimStruct *S)
{
nlp_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
{{ model.name }}_solver_capsule *capsule = {{ model.name }}_acados_create_capsule();
{{ model.name }}_acados_create(capsule);
ssSetUserData(S, (void*)capsule);
@@ -386,7 +386,7 @@ static void mdlStart(SimStruct *S)
static void mdlOutputs(SimStruct *S, int_T tid)
{
nlp_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
ocp_nlp_config *nlp_config = {{ model.name }}_acados_get_nlp_config(capsule);
ocp_nlp_dims *nlp_dims = {{ model.name }}_acados_get_nlp_dims(capsule);
ocp_nlp_in *nlp_in = {{ model.name }}_acados_get_nlp_in(capsule);
@@ -747,7 +747,7 @@ static void mdlOutputs(SimStruct *S, int_T tid)
static void mdlTerminate(SimStruct *S)
{
nlp_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_solver_capsule *capsule = ssGetUserData(S);
{{ model.name }}_acados_free(capsule);
{{ model.name }}_acados_free_capsule(capsule);
pyextra/acados_template/c_templates_tera/main.in.c
+52 -18
View File
@@ -42,12 +42,46 @@
#include "acados_c/external_function_interface.h"
#include "acados_solver_{{ model.name }}.h"
#define NX {{ model.name | upper }}_NX
#define NZ {{ model.name | upper }}_NZ
#define NU {{ model.name | upper }}_NU
#define NP {{ model.name | upper }}_NP
#define NBX {{ model.name | upper }}_NBX
#define NBX0 {{ model.name | upper }}_NBX0
#define NBU {{ model.name | upper }}_NBU
#define NSBX {{ model.name | upper }}_NSBX
#define NSBU {{ model.name | upper }}_NSBU
#define NSH {{ model.name | upper }}_NSH
#define NSG {{ model.name | upper }}_NSG
#define NSPHI {{ model.name | upper }}_NSPHI
#define NSHN {{ model.name | upper }}_NSHN
#define NSGN {{ model.name | upper }}_NSGN
#define NSPHIN {{ model.name | upper }}_NSPHIN
#define NSBXN {{ model.name | upper }}_NSBXN
#define NS {{ model.name | upper }}_NS
#define NSN {{ model.name | upper }}_NSN
#define NG {{ model.name | upper }}_NG
#define NBXN {{ model.name | upper }}_NBXN
#define NGN {{ model.name | upper }}_NGN
#define NY0 {{ model.name | upper }}_NY0
#define NY {{ model.name | upper }}_NY
#define NYN {{ model.name | upper }}_NYN
#define NH {{ model.name | upper }}_NH
#define NPHI {{ model.name | upper }}_NPHI
#define NHN {{ model.name | upper }}_NHN
#define NPHIN {{ model.name | upper }}_NPHIN
#define NR {{ model.name | upper }}_NR
int main()
{
nlp_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
int status = {{ model.name }}_acados_create(acados_ocp_capsule);
{{ model.name }}_solver_capsule *acados_ocp_capsule = {{ model.name }}_acados_create_capsule();
// there is an opportunity to change the number of shooting intervals in C without new code generation
int N = {{ model.name | upper }}_N;
// allocate the array and fill it accordingly
double* new_time_steps = NULL;
int status = {{ model.name }}_acados_create_with_discretization(acados_ocp_capsule, N, new_time_steps);
if (status)
{
@@ -63,13 +97,13 @@ int main()
void *nlp_opts = {{ model.name }}_acados_get_nlp_opts(acados_ocp_capsule);
// initial condition
int idxbx0[{{ dims.nbx_0 }}];
int idxbx0[NBX0];
{%- 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 }}];
double lbx0[NBX0];
double ubx0[NBX0];
{%- for i in range(end=dims.nbx_0) %}
lbx0[{{ i }}] = {{ constraints.lbx_0[i] }};
ubx0[{{ i }}] = {{ constraints.ubx_0[i] }};
@@ -80,13 +114,13 @@ int main()
ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", ubx0);
// initialization for state values
double x_init[{{ dims.nx }}];
double x_init[NX];
{%- for i in range(end=dims.nx) %}
x_init[{{ i }}] = 0.0;
{%- endfor %}
// initial value for control input
double u0[{{ dims.nu }}];
double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
@@ -94,14 +128,14 @@ int main()
{%- if dims.np > 0 %}
// set parameters
double p[{{ dims.np }}];
{% for item in parameter_values %}
double p[NP];
{%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
{% endfor %}
{%- endfor %}
for (int ii = 0; ii <= {{ dims.N }}; ii++)
for (int ii = 0; ii <= N; ii++)
{
{{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, {{ dims.np }});
{{ model.name }}_acados_update_params(acados_ocp_capsule, ii, p, NP);
}
{% endif %}{# if np > 0 #}
@@ -112,8 +146,8 @@ int main()
double elapsed_time;
int sqp_iter;
double xtraj[{{ dims.nx }} * ({{ dims.N }}+1)];
double utraj[{{ dims.nu }} * ({{ dims.N }})];
double xtraj[NX * (N+1)];
double utraj[NU * N];
// solve ocp in loop
@@ -135,14 +169,14 @@ int main()
/* print solution and statistics */
for (int ii = 0; ii <= nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*{{ dims.nx }}]);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "x", &xtraj[ii*NX]);
for (int ii = 0; ii < nlp_dims->N; ii++)
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*{{ dims.nu }}]);
ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, ii, "u", &utraj[ii*NU]);
printf("\n--- xtraj ---\n");
d_print_exp_tran_mat( {{ dims.nx }}, {{ dims.N }}+1, xtraj, {{ dims.nx }} );
d_print_exp_tran_mat( NX, N+1, xtraj, NX);
printf("\n--- utraj ---\n");
d_print_exp_tran_mat( {{ dims.nu }}, {{ dims.N }}, utraj, {{ dims.nu }} );
d_print_exp_tran_mat( NU, N, utraj, NU );
// ocp_nlp_out_print(nlp_solver->dims, nlp_out);
printf("\nsolved ocp %d times, solution printed above\n\n", NTIMINGS);
pyextra/acados_template/c_templates_tera/main_sim.in.c
+12 -7
View File
@@ -41,6 +41,11 @@
#include "acados_c/sim_interface.h"
#include "acados_sim_solver_{{ model.name }}.h"
#define NX {{ model.name | upper }}_NX
#define NZ {{ model.name | upper }}_NZ
#define NU {{ model.name | upper }}_NU
#define NP {{ model.name | upper }}_NP
int main()
{
@@ -60,7 +65,7 @@ int main()
void *acados_sim_dims = {{ model.name }}_acados_get_sim_dims(capsule);
// initial condition
double x_current[{{ dims.nx }}];
double x_current[NX];
{%- for i in range(end=dims.nx) %}
x_current[{{ i }}] = 0.0;
{%- endfor %}
@@ -78,19 +83,19 @@ int main()
// initial value for control input
double u0[{{ dims.nu }}];
double u0[NU];
{%- for i in range(end=dims.nu) %}
u0[{{ i }}] = 0.0;
{%- endfor %}
{%- if dims.np > 0 %}
// set parameters
double p[{{ dims.np }}];
{% for item in parameter_values %}
double p[NP];
{%- for item in parameter_values %}
p[{{ loop.index0 }}] = {{ item }};
{% endfor %}
{%- endfor %}
{{ model.name }}_acados_sim_update_params(capsule, p, {{ dims.np }});
{{ model.name }}_acados_sim_update_params(capsule, p, NP);
{% endif %}{# if np > 0 #}
int n_sim_steps = 3;
@@ -110,7 +115,7 @@ int main()
acados_sim_out, "x", x_current);
printf("\nx_current, %d\n", ii);
for (int jj = 0; jj < {{ dims.nx }}; jj++)
for (int jj = 0; jj < NX; jj++)
{
printf("%e\n", x_current[jj]);
}
pyextra/acados_template/generate_c_code_constraint.py
+1 -1
View File
@@ -94,7 +94,7 @@ def generate_c_code_constraint( model, con_name, is_terminal, opts ):
gen_dir = con_name + '_constraints'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = './' + gen_dir
gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# export casadi functions
pyextra/acados_template/generate_c_code_discrete_dynamics.py
+1 -1
View File
@@ -80,7 +80,7 @@ def generate_c_code_discrete_dynamics( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = './' + model_dir
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# set up & generate Functions
pyextra/acados_template/generate_c_code_explicit_ode.py
+1 -1
View File
@@ -105,7 +105,7 @@ def generate_c_code_explicit_ode( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = './' + model_dir
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_expl_ode_fun'
expl_ode_fun.generate(fun_name, casadi_opts)
pyextra/acados_template/generate_c_code_external_cost.py
+6
View File
@@ -58,6 +58,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
custom_hess = model.cost_expr_ext_cost_custom_hess_e
elif stage_type == 'path':
suffix_name = "_cost_ext_cost_fun"
@@ -65,6 +66,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
custom_hess = model.cost_expr_ext_cost_custom_hess
elif stage_type == 'initial':
suffix_name = "_cost_ext_cost_0_fun"
@@ -72,6 +74,7 @@ def generate_c_code_external_cost(model, stage_type, opts):
suffix_name_jac = "_cost_ext_cost_0_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost_0
custom_hess = model.cost_expr_ext_cost_custom_hess_0
# set up functions to be exported
fun_name = model.name + suffix_name
@@ -81,6 +84,9 @@ def generate_c_code_external_cost(model, stage_type, opts):
# generate expression for full gradient and Hessian
full_hess, grad = hessian(ext_cost, vertcat(u, x))
if custom_hess is not None:
full_hess = custom_hess
ext_cost_fun = Function(fun_name, [x, u, p], [ext_cost])
ext_cost_fun_jac_hess = Function(
fun_name_hess, [x, u, p], [ext_cost, grad, full_hess]
pyextra/acados_template/generate_c_code_gnsf.py
+1 -1
View File
@@ -54,7 +54,7 @@ def generate_c_code_gnsf( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = './' + model_dir
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
# obtain gnsf dimensions
pyextra/acados_template/generate_c_code_implicit_ode.py
+1 -1
View File
@@ -114,7 +114,7 @@ def generate_c_code_implicit_ode( model, opts ):
model_dir = model_name + '_model'
if not os.path.exists(model_dir):
os.mkdir(model_dir)
model_dir_location = './' + model_dir
model_dir_location = os.path.join('.', model_dir)
os.chdir(model_dir_location)
fun_name = model_name + '_impl_dae_fun'
pyextra/acados_template/generate_c_code_nls_cost.py
+1 -1
View File
@@ -76,7 +76,7 @@ def generate_c_code_nls_cost( model, cost_name, stage_type, opts ):
gen_dir = cost_name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = './' + gen_dir
gen_dir_location = os.path.join('.', gen_dir)
os.chdir(gen_dir_location)
# set up expressions
pyextra/acados_template/utils.py
+23 -11
View File
@@ -45,7 +45,7 @@ def get_acados_path():
ACADOS_PATH = os.environ.get('ACADOS_SOURCE_DIR')
if not ACADOS_PATH:
acados_template_path = os.path.dirname(os.path.abspath(__file__))
acados_path = os.path.join(acados_template_path, '../../../')
acados_path = os.path.join(acados_template_path, '..','..','..')
ACADOS_PATH = os.path.realpath(acados_path)
msg = 'Warning: Did not find environment variable ACADOS_SOURCE_DIR, '
msg += 'guessed ACADOS_PATH to be {}.\n'.format(ACADOS_PATH)
@@ -54,10 +54,20 @@ def get_acados_path():
return ACADOS_PATH
def get_python_interface_path():
ACADOS_PYTHON_INTERFACE_PATH = os.environ.get('ACADOS_PYTHON_INTERFACE_PATH')
if not ACADOS_PYTHON_INTERFACE_PATH:
acados_path = get_acados_path()
ACADOS_PYTHON_INTERFACE_PATH = os.path.join(acados_path, 'interfaces', 'acados_template', 'acados_template')
return ACADOS_PYTHON_INTERFACE_PATH
def get_tera_exec_path():
TERA_PATH = os.environ.get('TERA_PATH')
if not TERA_PATH:
TERA_PATH = os.path.join(get_acados_path(), 'bin/t_renderer')
TERA_PATH = os.path.join(get_acados_path(), 'bin', 't_renderer')
if os.name == 'nt':
TERA_PATH += '.exe'
return TERA_PATH
@@ -199,9 +209,7 @@ def render_template(in_file, out_file, template_dir, json_path):
# setting up loader and environment
acados_path = os.path.dirname(os.path.abspath(__file__))
template_glob = acados_path + '/c_templates_tera/*'
acados_template_path = acados_path + '/c_templates_tera'
template_glob = os.path.join(acados_path, 'c_templates_tera', '*')
# call tera as system cmd
os_cmd = "{tera_path} '{template_glob}' '{in_file}' '{json_path}' '{out_file}'".format(
@@ -213,7 +221,7 @@ def render_template(in_file, out_file, template_dir, json_path):
)
status = os.system(os_cmd)
if (status != 0):
raise Exception('Rendering of {} failed! Exiting.\n'.format(in_file))
raise Exception('Rendering of {} failed!\n\nAttempted to execute OS command:\n{}\n\nExiting.\n'.format(in_file, os_cmd))
os.chdir(cwd)
@@ -262,6 +270,14 @@ def acados_class2dict(class_instance):
return out
def get_ocp_nlp_layout():
python_interface_path = get_python_interface_path()
abs_path = os.path.join(python_interface_path, 'acados_layout.json')
with open(abs_path, 'r') as f:
ocp_nlp_layout = json.load(f)
return ocp_nlp_layout
def ocp_check_against_layout(ocp_nlp, ocp_dims):
"""
Check dimensions against layout
@@ -273,11 +289,7 @@ def ocp_check_against_layout(ocp_nlp, ocp_dims):
ocp_dims : instance of AcadosOcpDims
"""
# load JSON layout
current_module = sys.modules[__name__]
acados_path = os.path.dirname(current_module.__file__)
with open(acados_path + '/acados_layout.json', 'r') as f:
ocp_nlp_layout = json.load(f)
ocp_nlp_layout = get_ocp_nlp_layout()
ocp_check_against_layout_recursion(ocp_nlp, ocp_dims, ocp_nlp_layout)
return
selfdrive/controls/lib/lateral_mpc_lib/SConscript
+23 -4
View File
@@ -1,4 +1,4 @@
Import('env', 'arch')
Import('env', 'envCython', 'arch', 'common')
gen = "c_generated_code"
@@ -33,6 +33,7 @@ generated_files = [
f'{gen}/main_lat.c',
f'{gen}/acados_solver_lat.h',
f'{gen}/acados_solver.pxd',
f'{gen}/lat_model/lat_expl_vde_adj.c',
@@ -53,6 +54,24 @@ lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CCFLAGS"].append("-Wno-unused")
lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_lat",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
# generate cython stuff
acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx")
acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd")
libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
lenv2 = envCython.Clone()
lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()]
lenv2.Command(libacados_ocp_solver_c,
[acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
f'cython' + \
f' -o {libacados_ocp_solver_c.get_labspath()}' + \
f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
f' {acados_ocp_solver_pyx.get_labspath()}')
lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c])
lenv2.Depends(lib_cython, lib_solver)
selfdrive/controls/lib/lateral_mpc_lib/lat_mpc.py
+18 -13
View File
@@ -5,8 +5,12 @@ import numpy as np
from casadi import SX, vertcat, sin, cos
from selfdrive.controls.lib.drive_helpers import LAT_MPC_N as N
from selfdrive.controls.lib.drive_helpers import T_IDXS
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
if __name__ == '__main__': # generating code
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
# from pyextra.acados_template import AcadosOcpSolverFast
from selfdrive.controls.lib.lateral_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error
LAT_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
EXPORT_DIR = os.path.join(LAT_MPC_DIR, "c_generated_code")
@@ -110,17 +114,16 @@ def gen_lat_mpc_solver():
class LateralMpc():
def __init__(self, x0=np.zeros(X_DIM)):
self.solver = AcadosOcpSolver('lat', N, EXPORT_DIR)
self.solver = AcadosOcpSolverFast('lat', N, EXPORT_DIR)
self.reset(x0)
def reset(self, x0=np.zeros(X_DIM)):
self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N, 1))
self.yref = np.zeros((N+1, 3))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:2])
W = np.eye(3)
self.Ws = np.tile(W[None], reps=(N,1,1))
# Somehow needed for stable init
for i in range(N+1):
@@ -132,12 +135,11 @@ class LateralMpc():
self.cost = 0
def set_weights(self, path_weight, heading_weight, steer_rate_weight):
self.Ws[:,0,0] = path_weight
self.Ws[:,1,1] = heading_weight
self.Ws[:,2,2] = steer_rate_weight
self.solver.cost_set_slice(0, N, 'W', self.Ws, api='old')
W = np.asfortranarray(np.diag([path_weight, heading_weight, steer_rate_weight]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
#TODO hacky weights to keep behavior the same
self.solver.cost_set(N, 'W', (3/20.)*self.Ws[0,:2,:2])
self.solver.cost_set(N, 'W', (3/20.)*W[:2,:2])
def run(self, x0, v_ego, car_rotation_radius, y_pts, heading_pts):
x0_cp = np.copy(x0)
@@ -145,12 +147,15 @@ class LateralMpc():
self.solver.constraints_set(0, "ubx", x0_cp)
self.yref[:,0] = y_pts
self.yref[:,1] = heading_pts*(v_ego+5.0)
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:2])
self.solution_status = self.solver.solve()
self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
for i in range(N+1):
self.x_sol[i] = self.solver.get(i, 'x')
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.cost = self.solver.get_cost()
selfdrive/controls/lib/longitudinal_mpc_lib/SConscript
+23 -4
View File
@@ -1,4 +1,4 @@
Import('env', 'arch')
Import('env', 'envCython', 'arch', 'common')
gen = "c_generated_code"
@@ -41,6 +41,7 @@ generated_files = [
f'{gen}/main_long.c',
f'{gen}/acados_solver_long.h',
f'{gen}/acados_solver.pxd',
f'{gen}/long_model/long_expl_vde_adj.c',
@@ -63,6 +64,24 @@ lenv["CFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CXXFLAGS"].append("-DACADOS_WITH_QPOASES")
lenv["CCFLAGS"].append("-Wno-unused")
lenv["LINKFLAGS"].append("-Wl,--disable-new-dtags")
lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
lib_solver = lenv.SharedLibrary(f"{gen}/acados_ocp_solver_long",
build_files,
LIBS=['m', 'acados', 'hpipm', 'blasfeo', 'qpOASES_e'])
# generate cython stuff
acados_ocp_solver_pyx = File("#pyextra/acados_template/acados_ocp_solver_pyx.pyx")
acados_ocp_solver_common = File("#pyextra/acados_template/acados_solver_common.pxd")
libacados_ocp_solver_pxd = File(f'{gen}/acados_solver.pxd')
libacados_ocp_solver_c = File(f'{gen}/acados_ocp_solver_pyx.c')
lenv2 = envCython.Clone()
lenv2["LINKFLAGS"] += [lib_solver[0].get_labspath()]
lenv2.Command(libacados_ocp_solver_c,
[acados_ocp_solver_pyx, acados_ocp_solver_common, libacados_ocp_solver_pxd],
f'cython' + \
f' -o {libacados_ocp_solver_c.get_labspath()}' + \
f' -I {libacados_ocp_solver_pxd.get_dir().get_labspath()}' + \
f' -I {acados_ocp_solver_common.get_dir().get_labspath()}' + \
f' {acados_ocp_solver_pyx.get_labspath()}')
lib_cython = lenv2.Program(f'{gen}/acados_ocp_solver_pyx.so', [libacados_ocp_solver_c])
lenv2.Depends(lib_cython, lib_solver)
selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+28 -19
View File
@@ -8,7 +8,12 @@ from selfdrive.swaglog import cloudlog
from selfdrive.modeld.constants import index_function
from selfdrive.controls.lib.radar_helpers import _LEAD_ACCEL_TAU
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
if __name__ == '__main__': # generating code
from pyextra.acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
else:
# from pyextra.acados_template import AcadosOcpSolver as AcadosOcpSolverFast
from selfdrive.controls.lib.longitudinal_mpc_lib.c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverFast # pylint: disable=no-name-in-module, import-error
from casadi import SX, vertcat
LONG_MPC_DIR = os.path.dirname(os.path.abspath(__file__))
@@ -190,13 +195,14 @@ class LongitudinalMpc():
self.source = SOURCES[2]
def reset(self):
self.solver = AcadosOcpSolver('long', N, EXPORT_DIR)
self.solver = AcadosOcpSolverFast('long', N, EXPORT_DIR)
self.v_solution = [0.0 for i in range(N+1)]
self.a_solution = [0.0 for i in range(N+1)]
self.j_solution = [0.0 for i in range(N)]
self.yref = np.zeros((N+1, COST_DIM))
self.solver.cost_set_slice(0, N, "yref", self.yref[:N])
self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
self.x_sol = np.zeros((N+1, X_DIM))
self.u_sol = np.zeros((N,1))
self.params = np.zeros((N+1,3))
@@ -216,30 +222,30 @@ class LongitudinalMpc():
self.set_weights_for_lead_policy()
def set_weights_for_lead_policy(self):
W = np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST])
Ws = np.tile(W[None], reps=(N,1,1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
W = np.asfortranarray(np.diag([X_EGO_OBSTACLE_COST, X_EGO_COST, V_EGO_COST, A_EGO_COST, J_EGO_COST]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Set L2 slack cost on lower bound constraints
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, DANGER_ZONE_COST])
Zls = np.tile(Zl[None], reps=(N+1,1,1))
self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
W = np.diag([0., 10., 1., 10., 1.])
Ws = np.tile(W[None], reps=(N,1,1))
self.solver.cost_set_slice(0, N, 'W', Ws, api='old')
W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
# causing issues with the C interface.
self.solver.cost_set(N, 'W', np.copy(W[:COST_E_DIM, :COST_E_DIM]))
# Set L2 slack cost on lower bound constraints
Zl = np.array([LIMIT_COST, LIMIT_COST, LIMIT_COST, 0.0])
Zls = np.tile(Zl[None], reps=(N+1,1,1))
self.solver.cost_set_slice(0, N+1, 'Zl', Zls, api='old')
for i in range(N):
self.solver.cost_set(i, 'Zl', Zl)
def set_cur_state(self, v, a):
if abs(self.x0[1] - v) > 1.:
@@ -326,8 +332,9 @@ class LongitudinalMpc():
self.yref[:,1] = x
self.yref[:,2] = v
self.yref[:,3] = a
self.solver.cost_set_slice(0, N, "yref", self.yref[:N], api='old')
self.solver.set(N, "yref", self.yref[N][:COST_E_DIM])
for i in range(N):
self.solver.cost_set(i, "yref", self.yref[i])
self.solver.cost_set(N, "yref", self.yref[N][:COST_E_DIM])
self.accel_limit_arr[:,0] = -10.
self.accel_limit_arr[:,1] = 10.
x_obstacle = 1e5*np.ones((N+1))
@@ -338,12 +345,14 @@ class LongitudinalMpc():
def run(self):
for i in range(N+1):
self.solver.set_param(i, self.params[i])
self.solver.set(i, 'p', self.params[i])
self.solver.constraints_set(0, "lbx", self.x0)
self.solver.constraints_set(0, "ubx", self.x0)
self.solution_status = self.solver.solve()
self.solver.fill_in_slice(0, N+1, 'x', self.x_sol)
self.solver.fill_in_slice(0, N, 'u', self.u_sol)
for i in range(N+1):
self.x_sol[i] = self.solver.get(i, 'x')
for i in range(N):
self.u_sol[i] = self.solver.get(i, 'u')
self.v_solution = self.x_sol[:,1]
self.a_solution = self.x_sol[:,2]
third_party/acados/aarch64/lib/libacados.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/aarch64/lib/libblasfeo.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/aarch64/lib/libhpipm.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/build.sh
Vendored
+1 -1
View File
@@ -18,7 +18,7 @@ if [ ! -d acados_repo/ ]; then
fi
cd acados_repo
git fetch
git checkout 43ba28e95062f9ac9b48facd3b45698d57666fa3
git checkout 79e9e3e76f2751198858adf382c97837833ad31f
git submodule update --recursive --init
# build
third_party/acados/include/acados/ocp_nlp/ocp_nlp_common.h
Vendored
+7
View File
@@ -135,6 +135,8 @@ typedef struct ocp_nlp_dims
int *nz; // number of algebraic variables
int *ns; // number of slack variables
int N; // number of shooting nodes
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_dims;
//
@@ -203,6 +205,9 @@ typedef struct ocp_nlp_in
/// Pointers to constraints functions (TBC).
void **constraints;
/// Pointer to allocated memory, to be used for freeing.
void *raw_memory;
} ocp_nlp_in;
//
@@ -235,6 +240,8 @@ typedef struct ocp_nlp_out
double inf_norm_res;
double total_time;
void *raw_memory; // Pointer to allocated memory, to be used for freeing
} ocp_nlp_out;
//
third_party/acados/include/acados/sim/sim_collocation_utils.h
Vendored
+38 -29
View File
@@ -43,44 +43,53 @@ extern "C" {
enum Newton_type_collocation
// enum Newton_type_collocation
// {
// exact = 0,
// simplified_in,
// simplified_inis
// };
// typedef struct
// {
// enum Newton_type_collocation type;
// double *eig;
// double *low_tria;
// bool single;
// bool freeze;
// double *transf1;
// double *transf2;
// double *transf1_T;
// double *transf2_T;
// } Newton_scheme;
typedef enum
{
exact = 0,
simplified_in,
simplified_inis
};
typedef struct
{
enum Newton_type_collocation type;
double *eig;
double *low_tria;
bool single;
bool freeze;
double *transf1;
double *transf2;
double *transf1_T;
double *transf2_T;
} Newton_scheme;
GAUSS_LEGENDRE,
GAUSS_RADAU_IIA,
} sim_collocation_type;
//
acados_size_t gauss_nodes_work_calculate_size(int ns);
// acados_size_t gauss_legendre_nodes_work_calculate_size(int ns);
//
void gauss_nodes(int ns, double *nodes, void *raw_memory);
// void gauss_legendre_nodes(int ns, double *nodes, void *raw_memory);
//
acados_size_t gauss_simplified_work_calculate_size(int ns);
// acados_size_t gauss_simplified_work_calculate_size(int ns);
// //
// void gauss_simplified(int ns, Newton_scheme *scheme, void *work);
//
void gauss_simplified(int ns, Newton_scheme *scheme, void *work);
acados_size_t butcher_tableau_work_calculate_size(int ns);
//
acados_size_t butcher_table_work_calculate_size(int ns);
// void calculate_butcher_tableau_from_nodes(int ns, double *nodes, double *b, double *A, void *work);
//
void butcher_table(int ns, double *nodes, double *b, double *A, void *work);
void calculate_butcher_tableau(int ns, sim_collocation_type collocation_type, double *c_vec,
double *b_vec, double *A_mat, void *work);
third_party/acados/include/acados/sim/sim_common.h
Vendored
+2 -1
View File
@@ -141,12 +141,13 @@ typedef struct
bool output_z; // 1 -- if zn should be computed
bool sens_algebraic; // 1 -- if S_algebraic should be computed
bool exact_z_output; // 1 -- if z, S_algebraic should be computed exactly, extra Newton iterations
sim_collocation_type collocation_type;
// for explicit integrators: newton_iter == 0 && scheme == NULL
// && jac_reuse=false
int newton_iter;
bool jac_reuse;
Newton_scheme *scheme;
// Newton_scheme *scheme;
// workspace
void *work;
third_party/acados/include/acados/utils/math.h
Vendored
+1 -1
View File
@@ -40,7 +40,7 @@ extern "C" {
#include "acados/utils/types.h"
#if defined(__DSPACE__)
#if defined(__MABX2__)
double fmax(double a, double b);
#endif
third_party/acados/include/acados/utils/timing.h
Vendored
+1 -1
View File
@@ -67,7 +67,7 @@ typedef struct acados_timer_
mach_timebase_info_data_t tinfo;
} acados_timer;
#elif defined(__DSPACE__)
#elif defined(__MABX2__)
#include <brtenv.h>
third_party/acados/include/acados_c/ocp_nlp_interface.h
Vendored
+5 -8
View File
@@ -227,10 +227,8 @@ 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 start_stage, const char *field, void *value);
int stage, const char *field, void *value);
/// Sets the function pointers to the constraints functions for the given stage.
@@ -241,8 +239,6 @@ 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);
@@ -283,9 +279,6 @@ 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);
@@ -300,6 +293,8 @@ void ocp_nlp_constraint_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims
void ocp_nlp_cost_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out);
void ocp_nlp_dynamics_dims_get_from_attr(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_out *out,
int stage, const char *field, int *dims_out);
/* opts */
@@ -374,6 +369,8 @@ int ocp_nlp_precompute(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *
/// \param nlp_out The output struct.
void ocp_nlp_eval_cost(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
//
void ocp_nlp_eval_residuals(ocp_nlp_solver *solver, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out);
//
void ocp_nlp_eval_param_sens(ocp_nlp_solver *solver, char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
third_party/acados/include/acados_c/sim_interface.h
Vendored
+5 -5
View File
@@ -45,11 +45,11 @@ extern "C" {
typedef enum
{
ERK,
IRK,
GNSF,
LIFTED_IRK,
INVALID_SIM_SOLVER,
ERK,
IRK,
GNSF,
LIFTED_IRK,
INVALID_SIM_SOLVER,
} sim_solver_t;
third_party/acados/include/blasfeo/include/blasfeo_block_size.h
Vendored
+22 -1
View File
@@ -43,7 +43,28 @@
#if defined( TARGET_X64_INTEL_HASWELL )
#if defined( TARGET_X64_INTEL_SKYLAKE_X )
// common
#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes
#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way
#define L2_CACHE_SIZE (256*1024) //(1024*1024) // L2 data cache size: 1 MB ; DTLB1 64*4 kB = 256 kB
#define LLC_CACHE_SIZE (6*1024*1024) //(8*1024*1024) // LLC cache size: 8 MB ; TLB 1536*4 kB = 6 MB
// double
#define D_PS 8 // panel size
#define D_PLD 8 // 4 // GCD of panel length
#define D_M_KERNEL 24 // max kernel size
#define D_KC 128 //256 // 192
#define D_NC 144 //72 //96 //72 // 120 // 512
#define D_MC 2400 // 6000
// single
#define S_PS 16 // panel size
#define S_PLD 4 // GCD of panel length TODO probably 16 when writing assebly
#define S_M_KERNEL 32 // max kernel size
#define S_KC 128 //256
#define S_NC 128 //144
#define S_MC 3000
#elif defined( TARGET_X64_INTEL_HASWELL )
// common
#define CACHE_LINE_SIZE 64 // data cache size: 64 bytes
#define L1_CACHE_SIZE (32*1024) // L1 data cache size: 32 kB, 8-way
third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_api.h
Vendored
+8 -8
View File
@@ -101,18 +101,18 @@ void blasfeo_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct bl
void blasfeo_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= inv( A^T ) * x, A (m)x(m) upper, transposed, not_unit
void blasfeo_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_dtrmv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_dtrmv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A^T * x ; A lower triangular
void blasfeo_dtrmv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A^T * x ; A lower triangular, unit diagonal
void blasfeo_dtrmv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= beta * y + alpha * A * x ; A upper triangular
void blasfeo_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A^T * x ; A upper triangular
void blasfeo_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A^T * x ; A lower triangular
void blasfeo_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A^T * x ; A lower triangular, unit diagonal
void blasfeo_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z_n <= beta_n * y_n + alpha_n * A * x_n
// z_t <= beta_t * y_t + alpha_t * A^T * x_t
void blasfeo_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t);
third_party/acados/include/blasfeo/include/blasfeo_d_blasfeo_ref_api.h
Vendored
+10 -9
View File
@@ -101,23 +101,24 @@ void blasfeo_ref_dtrsv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struc
void blasfeo_ref_dtrsv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
void blasfeo_ref_dtrsv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_ref_dtrmv_lnn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_ref_dtrmv_lnu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_ref_dtrmv_ltn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A' * x ; A lower triangular, unit diagonal
void blasfeo_ref_dtrmv_ltu(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= beta * y + alpha * A * x ; A upper triangular
void blasfeo_ref_dtrmv_unn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A' * x ; A upper triangular
void blasfeo_ref_dtrmv_utn(int m, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_ref_dtrmv_lnn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_ref_dtrmv_ltn(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_ref_dtrmv_lnu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z <= A' * x ; A lower triangular, unit diagonal
void blasfeo_ref_dtrmv_ltu(int m, int n, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, struct blasfeo_dvec *sz, int zi);
// z_n <= beta_n * y_n + alpha_n * A * x_n
// z_t <= beta_t * y_t + alpha_t * A' * x_t
void blasfeo_ref_dgemv_nt(int m, int n, double alpha_n, double alpha_t, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx_n, int xi_n, struct blasfeo_dvec *sx_t, int xi_t, double beta_n, double beta_t, struct blasfeo_dvec *sy_n, int yi_n, struct blasfeo_dvec *sy_t, int yi_t, struct blasfeo_dvec *sz_n, int zi_n, struct blasfeo_dvec *sz_t, int zi_t);
// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
void blasfeo_ref_dsymv_l(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
void blasfeo_ref_dsymv_l(int m, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
void blasfeo_ref_dsymv_l_mn(int m, int n, double alpha, struct blasfeo_dmat *sA, int ai, int aj, struct blasfeo_dvec *sx, int xi, double beta, struct blasfeo_dvec *sy, int yi, struct blasfeo_dvec *sz, int zi);
// diagonal
third_party/acados/include/blasfeo/include/blasfeo_d_kernel.h
Vendored
+205 -6
View File
@@ -54,6 +54,158 @@ void blasfeo_align_4096_byte(void *ptr, void **ptr_align);
void blasfeo_align_64_byte(void *ptr, void **ptr_align);
//
// lib8
//
// 24x8
void kernel_dgemm_nt_24x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
void kernel_dgemm_nt_24x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
void kernel_dtrsm_nt_rl_inv_24x8_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); //
void kernel_dpotrf_nt_l_24x8_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dpotrf_nt_l_24x8_vs_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1);
void kernel_dtrsm_nt_rl_inv_24x8_vs_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); //
void kernel_dgemm_dtrsm_nt_rl_inv_24x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_24x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1);
void kernel_dsyrk_dpotrf_nt_l_24x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_24x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1);
void kernel_dlarfb8_rn_24_lib8(int kmax, double *pV, double *pT, double *pD, int sdd);
// 16x8
void kernel_dgemm_nt_16x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
void kernel_dgemm_nt_16x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
void kernel_dgemm_nt_16x8_gen_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dgemm_nn_16x8_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd); //
void kernel_dgemm_nn_16x8_vs_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
void kernel_dgemm_nn_16x8_gen_lib8(int k, double *alpha, double *A, int sda, int offB, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dsyrk_nt_l_16x8_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd); //
void kernel_dsyrk_nt_l_16x8_vs_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, int m1, int n1); //
void kernel_dsyrk_nt_l_16x8_gen_lib8(int k, double *alpha, double *A, int sda, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dtrmm_nn_rl_16x8_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd);
void kernel_dtrmm_nn_rl_16x8_vs_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, double *D, int sdd, int m1, int n1);
void kernel_dtrmm_nn_rl_16x8_gen_lib8(int k, double *alpha, double *A, int sda, int offsetB, double *B, int sdb, int offD, double *D, int sdd, int m0, int m1, int n0, int n1);
void kernel_dtrsm_nt_rl_inv_16x8_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E); //
void kernel_dtrsm_nt_rl_inv_16x8_vs_lib8(int k, double *A, int sda, double *B, double *beta, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1); //
void kernel_dpotrf_nt_l_16x8_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dpotrf_nt_l_16x8_vs_lib8(int k, double *A, int sda, double *B, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1);
void kernel_dgemm_dtrsm_nt_rl_inv_16x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_16x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int m1, int n1);
void kernel_dsyrk_dpotrf_nt_l_16x8_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_16x8_vs_lib8(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int m1, int n1);
void kernel_dlarfb8_rn_16_lib8(int kmax, double *pV, double *pT, double *pD, int sdd);
void kernel_dlarfb8_rn_la_16_lib8(int n1, double *pVA, double *pT, double *pD, int sdd, double *pA, int sda);
void kernel_dlarfb8_rn_lla_16_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, int sdd, double *pL, int sdl, double *pA, int sda);
// 8x16
void kernel_dgemm_tt_8x16_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D); //
void kernel_dgemm_tt_8x16_vs_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
void kernel_dgemm_tt_8x16_gen_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dgemm_nt_8x16_lib8(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D); //
void kernel_dgemm_nt_8x16_vs_lib8(int k, double *alpha, double *A, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
// 8x8
void kernel_dgemm_nt_8x8_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
void kernel_dgemm_nt_8x8_vs_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); //
void kernel_dgemm_nt_8x8_gen_lib8(int k, double *alpha, double *A, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dgemm_nn_8x8_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, double *C, double *D); //
void kernel_dgemm_nn_8x8_vs_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, double *C, double *D, int m1, int n1); //
void kernel_dgemm_nn_8x8_gen_lib8(int k, double *alpha, double *A, int offB, double *B, int sdb, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dgemm_tt_8x8_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, double *C, double *D); //
void kernel_dgemm_tt_8x8_vs_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, double *C, double *D, int m1, int n1); //
void kernel_dgemm_tt_8x8_gen_lib8(int k, double *alpha, int offA, double *A, int sda, double *B, double *beta, int offc, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dsyrk_nt_l_8x8_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D); //
void kernel_dsyrk_nt_l_8x8_vs_lib8(int k, double *alpha, double *A, double *B, double *beta, double *C, double *D, int m1, int n1); //
void kernel_dsyrk_nt_l_8x8_gen_lib8(int k, double *alpha, double *A, double *B, double *beta, int offC, double *C, int sdc, int offD, double *D, int sdd, int m0, int m1, int n0, int n1); //
void kernel_dtrmm_nn_rl_8x8_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D);
void kernel_dtrmm_nn_rl_8x8_vs_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, double *D, int m1, int n1);
void kernel_dtrmm_nn_rl_8x8_gen_lib8(int k, double *alpha, double *A, int offsetB, double *B, int sdb, int offD, double *D, int sdd, int m0, int m1, int n0, int n1);
void kernel_dtrsm_nt_rl_inv_8x8_lib8(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E);
void kernel_dtrsm_nt_rl_inv_8x8_vs_lib8(int k, double *A, double *B, double *beta, double *C, double *D, double *E, double *inv_diag_E, int m1, int n1);
void kernel_dpotrf_nt_l_8x8_lib8(int k, double *A, double *B, double *C, double *D, double *inv_diag_D);
void kernel_dpotrf_nt_l_8x8_vs_lib8(int k, double *A, double *B, double *C, double *D, double *inv_diag_D, int m1, int n1);
void kernel_dgemm_dtrsm_nt_rl_inv_8x8_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_8x8_vs_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int m1, int n1);
void kernel_dsyrk_dpotrf_nt_l_8x8_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib8(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int m1, int n1);
void kernel_dgelqf_vs_lib8(int m, int n, int k, int offD, double *pD, int sdd, double *dD);
void kernel_dgelqf_pd_vs_lib8(int m, int n, int k, int offD, double *pD, int sdd, double *dD);
void kernel_dgelqf_8_lib8(int kmax, double *pD, double *dD);
void kernel_dgelqf_pd_8_lib8(int kmax, double *pD, double *dD);
void kernel_dlarft_8_lib8(int kmax, double *pD, double *dD, double *pT);
void kernel_dlarfb8_rn_8_lib8(int kmax, double *pV, double *pT, double *pD);
void kernel_dlarfb8_rn_8_vs_lib8(int kmax, double *pV, double *pT, double *pD, int m1);
void kernel_dlarfb8_rn_1_lib8(int kmax, double *pV, double *pT, double *pD);
void kernel_dgelqf_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pT);
void kernel_dgelqf_pd_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pT);
void kernel_dgelqf_pd_la_vs_lib8(int m, int n1, int k, int offD, double *pD, int sdd, double *dD, int offA, double *pA, int sda);
void kernel_dgelqf_pd_la_dlarft8_8_lib8(int kmax, double *pD, double *dD, double *pA, double *pT);
void kernel_dlarft_la_8_lib8(int n1, double *dD, double *pA, double *pT);
void kernel_dlarfb8_rn_la_8_lib8(int n1, double *pVA, double *pT, double *pD, double *pA);
void kernel_dlarfb8_rn_la_8_vs_lib8(int n1, double *pVA, double *pT, double *pD, double *pA, int m1);
void kernel_dlarfb8_rn_la_1_lib8(int n1, double *pVA, double *pT, double *pD, double *pA);
void kernel_dgelqf_pd_lla_vs_lib8(int m, int n0, int n1, int k, int offD, double *pD, int sdd, double *dD, int offL, double *pL, int sdl, int offA, double *pA, int sda);
void kernel_dgelqf_pd_lla_dlarft8_8_lib8(int n0, int n1, double *pD, double *dD, double *pL, double *pA, double *pT);
void kernel_dlarft_lla_8_lib8(int n0, int n1, double *dD, double *pL, double *pA, double *pT);
void kernel_dlarfb8_rn_lla_8_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA);
void kernel_dlarfb8_rn_lla_8_vs_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA, int m1);
void kernel_dlarfb8_rn_lla_1_lib8(int n0, int n1, double *pVL, double *pVA, double *pT, double *pD, double *pL, double *pA);
// panel copy / pack
// 24
void kernel_dpack_nn_24_lib8(int kmax, double *A, int lda, double *C, int sdc);
void kernel_dpack_nn_24_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1);
// 16
void kernel_dpacp_nn_16_lib8(int kmax, int offsetA, double *A, int sda, double *B, int sdb);
void kernel_dpacp_nn_16_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int sdb, int m1);
void kernel_dpack_nn_16_lib8(int kmax, double *A, int lda, double *C, int sdc);
void kernel_dpack_nn_16_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1);
// 8
void kernel_dpacp_nn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B);
void kernel_dpacp_nn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1);
void kernel_dpacp_tn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B);
void kernel_dpacp_tn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1);
void kernel_dpacp_l_nn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B);
void kernel_dpacp_l_nn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1);
void kernel_dpacp_l_tn_8_lib8(int kmax, int offsetA, double *A, int sda, double *B);
void kernel_dpacp_l_tn_8_vs_lib8(int kmax, int offsetA, double *A, int sda, double *B, int m1);
void kernel_dpaad_nn_8_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *B);
void kernel_dpaad_nn_8_vs_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *B, int m1);
void kernel_dpack_nn_8_lib8(int kmax, double *A, int lda, double *C);
void kernel_dpack_nn_8_vs_lib8(int kmax, double *A, int lda, double *C, int m1);
void kernel_dpack_tn_8_lib8(int kmax, double *A, int lda, double *C);
void kernel_dpack_tn_8_vs_lib8(int kmax, double *A, int lda, double *C, int m1);
// 4
void kernel_dpack_tt_4_lib8(int kmax, double *A, int lda, double *C, int sdc); // TODO offsetC
void kernel_dpack_tt_4_vs_lib8(int kmax, double *A, int lda, double *C, int sdc, int m1); // TODO offsetC
// level 2 BLAS
// 16
void kernel_dgemv_n_16_lib8(int k, double *alpha, double *A, int sda, double *x, double *beta, double *y, double *z);
// 8
void kernel_dgemv_n_8_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z);
void kernel_dgemv_n_8_vs_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int m1);
//void kernel_dgemv_n_8_gen_lib8(int k, double *alpha, double *A, double *x, double *beta, double *y, double *z, int m0, int m1);
void kernel_dgemv_n_8_gen_lib8(int k, double *alpha, int offsetA, double *A, double *x, double *beta, double *y, double *z, int m1);
void kernel_dgemv_t_8_lib8(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z);
void kernel_dgemv_t_8_vs_lib8(int k, double *alpha, int offsetA, double *A, int sda, double *x, double *beta, double *y, double *z, int n1);
void kernel_dgemv_nt_8_lib8(int kmax, double *alpha_n, double *alpha_t, int offsetA, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t);
void kernel_dgemv_nt_8_vs_lib8(int kmax, double *alpha_n, double *alpha_t, int offsetA, double *A, int sda, double *x_n, double *x_t, double *beta_t, double *y_t, double *z_n, double *z_t, int n1);
void kernel_dsymv_l_8_lib8(int kmax, double *alpha, double *A, int sda, double *x, double *z);
void kernel_dsymv_l_8_vs_lib8(int kmax, double *alpha, double *A, int sda, double *x, double *z, int n1);
void kernel_dsymv_l_8_gen_lib8(int kmax, double *alpha, int offsetA, double *A, int sda, double *x, double *z, int n1);
void kernel_dtrmv_n_ln_8_lib8(int k, double *A, double *x, double *z);
void kernel_dtrmv_n_ln_8_vs_lib8(int k, double *A, double *x, double *z, int m1);
void kernel_dtrmv_n_ln_8_gen_lib8(int k, int offsetA, double *A, double *x, double *z, int m1);
void kernel_dtrmv_t_ln_8_lib8(int k, double *A, int sda, double *x, double *z);
void kernel_dtrmv_t_ln_8_vs_lib8(int k, double *A, int sda, double *x, double *z, int n1);
void kernel_dtrmv_t_ln_8_gen_lib8(int k, int offsetA, double *A, int sda, double *x, double *z, int n1);
void kernel_dtrsv_n_l_inv_8_lib8(int k, double *A, double *inv_diag_A, double *x, double *z);
void kernel_dtrsv_n_l_inv_8_vs_lib8(int k, double *A, double *inv_diag_A, double *x, double *z, int m1, int n1);
void kernel_dtrsv_t_l_inv_8_lib8(int k, double *A, int sda, double *inv_diag_A, double *x, double *z);
void kernel_dtrsv_t_l_inv_8_vs_lib8(int k, double *A, int sda, double *inv_diag_A, double *x, double *z, int m1, int n1);
//
// lib4
//
// level 2 BLAS
// 12
@@ -413,10 +565,10 @@ void kernel_drowsw_lib4(int kmax, double *pA, double *pC);
// merged routines
// 12x4
void kernel_dgemm_dtrsm_nt_rl_inv_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
void kernel_dgemm_dtrsm_nt_rl_inv_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
void kernel_dsyrk_dpotrf_nt_l_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
void kernel_dgemm_dtrsm_nt_rl_inv_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_12x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_12x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
// 4x12
void kernel_dgemm_dtrsm_nt_rl_inv_4x12_vs_lib4(int kp, double *Ap, double *Bp, int sdbp, int km_, double *Am, double *Bm, int sdbm, double *C, double *D, double *E, int sde, double *inv_diag_E, int km, int kn);
// 8x8
@@ -425,8 +577,8 @@ void kernel_dsyrk_dpotrf_nt_l_8x8_vs_lib4(int kp, double *Ap, int sdap, double *
void kernel_dgemm_dtrsm_nt_rl_inv_8x8l_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
void kernel_dgemm_dtrsm_nt_rl_inv_8x8u_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int sdb, int km_, double *Am, int sdam, double *Bm, int sdbm, double *C, int sdc, double *D, int sdd, double *E, int sde, double *inv_diag_E, int km, int kn);
// 8x4
void kernel_dgemm_dtrsm_nt_rl_inv_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
void kernel_dgemm_dtrsm_nt_rl_inv_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *E, double *inv_diag_E, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_8x4_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_8x4_vs_lib4(int kp, double *Ap, int sdap, double *Bp, int km_, double *Am, int sdam, double *Bm, double *C, int sdc, double *D, int sdd, double *inv_diag_D, int km, int kn);
// 4x8
@@ -434,16 +586,16 @@ void kernel_dgemm_dtrsm_nt_rl_inv_4x8_vs_lib4(int kp, double *Ap, double *Bp, in
// 4x4
void kernel_dgemm_dtrsm_nt_rl_inv_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_4x4_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_4x4_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
// 4x2
void kernel_dgemm_dtrsm_nt_rl_inv_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E);
void kernel_dgemm_dtrsm_nt_rl_inv_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *E, double *inv_diag_E, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_4x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_4x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
// 2x2
void kernel_dsyrk_dpotrf_nt_l_2x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
void kernel_dsyrk_dpotrf_nt_l_2x2_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D);
void kernel_dsyrk_dpotrf_nt_l_2x2_vs_lib4(int kp, double *Ap, double *Bp, int km_, double *Am, double *Bm, double *C, double *D, double *inv_diag_D, int km, int kn);
/*
*
@@ -1034,6 +1186,53 @@ void kernel_dgemm_nt_8xn_p0_lib44cc(int n, int k, double *alpha, double *A, int
// A, B panel-major bs=8; C, D column-major
// 24x8
void kernel_dgemm_nt_24x8_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_24x8_vs_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 16x8
void kernel_dgemm_nt_16x8_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_16x8_vs_lib88cc(int kmax, double *alpha, double *A, int sda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 8x8
void kernel_dgemm_nt_8x8_lib88cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_8x8_vs_lib88cc(int kmax, double *alpha, double *A, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// A, panel-major bs=8; B, C, D column-major
// 24x8
void kernel_dgemm_nt_24x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_24x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_nn_24x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nn_24x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 16x8
void kernel_dgemm_nt_16x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_16x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_nn_16x8_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nn_16x8_vs_lib8ccc(int kmax, double *alpha, double *A, int sda, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 8x8
void kernel_dgemm_nt_8x8_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_8x8_vs_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_nn_8x8_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nn_8x8_vs_lib8ccc(int kmax, double *alpha, double *A, double *B, int ldb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// B, panel-major bs=8; A, C, D column-major
// 8x24
void kernel_dgemm_nt_8x24_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_8x24_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_tt_8x24_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_tt_8x24_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 8x16
void kernel_dgemm_nt_8x16_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_8x16_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_tt_8x16_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_tt_8x16_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, int sdb, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// 8x8
void kernel_dgemm_nt_8x8_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_nt_8x8_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
void kernel_dgemm_tt_8x8_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd);
void kernel_dgemm_tt_8x8_vs_libc8cc(int kmax, double *alpha, double *A, int lda, double *B, double *beta, double *C, int ldc, double *D, int ldd, int m1, int n1);
// aux
void kernel_dvecld_inc1(int kmax, double *x);
void kernel_dveccp_inc1(int kmax, double *x, double *y);
third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_api.h
Vendored
+8 -4
View File
@@ -97,14 +97,18 @@ void blasfeo_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct bl
void blasfeo_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
void blasfeo_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_strmv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_strmv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_strmv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular, unit diagonal
void blasfeo_strmv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= beta * y + alpha * A * x ; A upper triangular
void blasfeo_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A upper triangular
void blasfeo_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z_n <= beta_n * y_n + alpha_n * A * x_n
// z_t <= beta_t * y_t + alpha_t * A' * x_t
void blasfeo_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t);
third_party/acados/include/blasfeo/include/blasfeo_s_blasfeo_ref_api.h
Vendored
+10 -5
View File
@@ -97,19 +97,24 @@ void blasfeo_ref_strsv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struc
void blasfeo_ref_strsv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= inv( A' ) * x, A (m)x(m) upper, transposed, not_unit
void blasfeo_ref_strsv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_ref_strmv_lnn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular, unit diagonal
void blasfeo_ref_strmv_lnu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_ref_strmv_ltn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular, unit diagonal
void blasfeo_ref_strmv_ltu(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= beta * y + alpha * A * x ; A upper triangular
void blasfeo_ref_strmv_unn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A upper triangular
void blasfeo_ref_strmv_utn(int m, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A * x ; A lower triangular
void blasfeo_ref_strmv_lnn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z <= A' * x ; A lower triangular
void blasfeo_ref_strmv_ltn(int m, int n, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, struct blasfeo_svec *sz, int zi);
// z_n <= beta_n * y_n + alpha_n * A * x_n
// z_t <= beta_t * y_t + alpha_t * A' * x_t
void blasfeo_ref_sgemv_nt(int m, int n, float alpha_n, float alpha_t, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx_n, int xi_n, struct blasfeo_svec *sx_t, int xi_t, float beta_n, float beta_t, struct blasfeo_svec *sy_n, int yi_n, struct blasfeo_svec *sy_t, int yi_t, struct blasfeo_svec *sz_n, int zi_n, struct blasfeo_svec *sz_t, int zi_t);
// z <= beta * y + alpha * A * x, where A is symmetric and only the lower triangular patr of A is accessed
void blasfeo_ref_ssymv_l(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
void blasfeo_ref_ssymv_l(int m, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
void blasfeo_ref_ssymv_l_mn(int m, int n, float alpha, struct blasfeo_smat *sA, int ai, int aj, struct blasfeo_svec *sx, int xi, float beta, struct blasfeo_svec *sy, int yi, struct blasfeo_svec *sz, int zi);
// diagonal
third_party/acados/include/blasfeo/include/blasfeo_s_kernel.h
Vendored
+10
View File
@@ -550,6 +550,16 @@ void kernel_spotrf_nt_l_4x4_lib44cc(int kmax, float *A, float *B, float *C, int
void kernel_spotrf_nt_l_4x4_vs_lib44cc(int kmax, float *A, float *B, float *C, int ldc, float *D, int ldd, float *dD, int m1, int n1);
// B panel-major bs=8; A, C, D column-major
// 4x24
void kernel_sgemm_nt_4x24_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
void kernel_sgemm_nt_4x24_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
void kernel_sgemm_tt_4x24_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
void kernel_sgemm_tt_4x24_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
// 4x16
void kernel_sgemm_nt_4x16_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
void kernel_sgemm_nt_4x16_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
void kernel_sgemm_tt_4x16_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd);
void kernel_sgemm_tt_4x16_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, int sdb, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
// 8x8
void kernel_sgemm_nt_8x8_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd);
void kernel_sgemm_nt_8x8_vs_libc8cc(int kmax, float *alpha, float *A, int lda, float *B, float *beta, float *C, int ldc, float *D, int ldd, int m1, int n1);
third_party/acados/larch64/lib/libacados.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/larch64/lib/libblasfeo.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/larch64/lib/libhpipm.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/x86_64/lib/libacados.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/x86_64/lib/libblasfeo.so
Vendored
BIN
View File
Binary file not shown.
third_party/acados/x86_64/lib/libhpipm.so
Vendored
BIN
View File
Binary file not shown.