Upstreamed and update acados (#23927)
* change acados version to finalized PR https://github.com/acados/acados/pull/791 * acados: update include and x86_64 build * acados: update pyextra * adapt mpc files to updated acados * acados buid: clear acados_template and include dir before updating * update acados again * acados MPC: only generate solver, building is done via scons * update acados to latest acados/master * c3 libs * c2 libs * acados scons files: add acados build script as a dependency * acados scons files: add libacados.so and ocp_nlp_interface.h, build script not in files_common * update acados * adapts long & lat MPC file * update acados reference commit to acados/master Co-authored-by: Comma Device <device@comma.ai>pull/23948/head
Jonathan Frey
3 years ago
committed by
GitHub
parent
107156cd58
commit
d653dd843a
51 changed files with 1588 additions and 1595 deletions
@ -1,402 +0,0 @@ |
||||
import sys |
||||
import os |
||||
import json |
||||
import numpy as np |
||||
from datetime import datetime |
||||
|
||||
from ctypes import POINTER, CDLL, c_void_p, c_int, cast, c_double, c_char_p |
||||
|
||||
from copy import deepcopy |
||||
|
||||
from .generate_c_code_explicit_ode import generate_c_code_explicit_ode |
||||
from .generate_c_code_implicit_ode import generate_c_code_implicit_ode |
||||
from .generate_c_code_gnsf import generate_c_code_gnsf |
||||
from .generate_c_code_discrete_dynamics import generate_c_code_discrete_dynamics |
||||
from .generate_c_code_constraint import generate_c_code_constraint |
||||
from .generate_c_code_nls_cost import generate_c_code_nls_cost |
||||
from .generate_c_code_external_cost import generate_c_code_external_cost |
||||
from .acados_ocp import AcadosOcp |
||||
from .acados_model import acados_model_strip_casadi_symbolics |
||||
from .utils import is_column, is_empty, casadi_length, render_template, acados_class2dict,\ |
||||
format_class_dict, ocp_check_against_layout, np_array_to_list, make_model_consistent,\ |
||||
set_up_imported_gnsf_model, get_acados_path |
||||
|
||||
|
||||
class AcadosOcpSolverFast: |
||||
dlclose = CDLL(None).dlclose |
||||
dlclose.argtypes = [c_void_p] |
||||
|
||||
def __init__(self, model_name, N, code_export_dir): |
||||
|
||||
self.solver_created = False |
||||
self.N = N |
||||
self.model_name = model_name |
||||
|
||||
self.shared_lib_name = f'{code_export_dir}/libacados_ocp_solver_{model_name}.so' |
||||
|
||||
# get shared_lib |
||||
self.shared_lib = CDLL(self.shared_lib_name) |
||||
|
||||
# create capsule |
||||
getattr(self.shared_lib, f"{model_name}_acados_create_capsule").restype = c_void_p |
||||
self.capsule = getattr(self.shared_lib, f"{model_name}_acados_create_capsule")() |
||||
|
||||
# create solver |
||||
getattr(self.shared_lib, f"{model_name}_acados_create").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_create").restype = c_int |
||||
assert getattr(self.shared_lib, f"{model_name}_acados_create")(self.capsule)==0 |
||||
self.solver_created = True |
||||
|
||||
# get pointers solver |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts").restype = c_void_p |
||||
self.nlp_opts = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_opts")(self.capsule) |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims").restype = c_void_p |
||||
self.nlp_dims = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_dims")(self.capsule) |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config").restype = c_void_p |
||||
self.nlp_config = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_config")(self.capsule) |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out").restype = c_void_p |
||||
self.nlp_out = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_out")(self.capsule) |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in").restype = c_void_p |
||||
self.nlp_in = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_in")(self.capsule) |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver").restype = c_void_p |
||||
self.nlp_solver = getattr(self.shared_lib, f"{model_name}_acados_get_nlp_solver")(self.capsule) |
||||
|
||||
|
||||
def solve(self): |
||||
""" |
||||
Solve the ocp with current input. |
||||
""" |
||||
model_name = self.model_name |
||||
|
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").argtypes = [c_void_p] |
||||
getattr(self.shared_lib, f"{model_name}_acados_solve").restype = c_int |
||||
status = getattr(self.shared_lib, f"{model_name}_acados_solve")(self.capsule) |
||||
return status |
||||
|
||||
def cost_set(self, start_stage_, field_, value_, api='warn'): |
||||
self.cost_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') |
||||
return |
||||
|
||||
def cost_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): |
||||
""" |
||||
Set numerical data in the cost module of the solver. |
||||
|
||||
:param stage: integer corresponding to shooting node |
||||
:param field: string, e.g. 'yref', 'W', 'ext_cost_num_hess' |
||||
:param value: of appropriate size |
||||
""" |
||||
# cast value_ to avoid conversion issues |
||||
if isinstance(value_, (float, int)): |
||||
value_ = np.array([value_]) |
||||
value_ = np.ascontiguousarray(np.copy(value_), dtype=np.float64) |
||||
field = field_ |
||||
field = field.encode('utf-8') |
||||
dim = np.product(value_.shape[1:]) |
||||
|
||||
start_stage = c_int(start_stage_) |
||||
end_stage = c_int(end_stage_) |
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] |
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr.restype = c_int |
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) |
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int)) |
||||
|
||||
self.shared_lib.ocp_nlp_cost_dims_get_from_attr(self.nlp_config, |
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) |
||||
|
||||
value_shape = value_.shape |
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) |
||||
if len(value_shape) == 2: |
||||
value_shape = (value_shape[0], value_shape[1], 0) |
||||
|
||||
elif len(value_shape) == 3: |
||||
if api=='old': |
||||
pass |
||||
elif api=='warn': |
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): |
||||
raise Exception("Ambiguity in API detected.\n" |
||||
"Are you making an acados model from scrach? Add api='new' to cost_set and carry on.\n" |
||||
"Are you seeing this error suddenly in previously running code? Read on.\n" |
||||
" You are relying on a now-fixed bug in cost_set for field '{}'.\n".format(field_) + |
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" + |
||||
" Two options to fix this error: \n" + |
||||
" * Add api='old' to cost_set to restore old incorrect behaviour\n" + |
||||
" * Add api='new' to cost_set and remove any unnatural manipulation of the value argument " + |
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + |
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.") |
||||
# Get elements in column major order |
||||
value_ = np.ravel(value_, order='F') |
||||
elif api=='new': |
||||
# Get elements in column major order |
||||
value_ = np.ravel(value_, order='F') |
||||
else: |
||||
raise Exception("Unknown api: '{}'".format(api)) |
||||
|
||||
if value_shape != expected_shape: |
||||
raise Exception('AcadosOcpSolver.cost_set(): mismatching dimension', |
||||
' for field "{}" with dimension {} (you have {})'.format( |
||||
field_, expected_shape, value_shape)) |
||||
|
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double)) |
||||
value_data_p = cast((value_data), c_void_p) |
||||
|
||||
self.shared_lib.ocp_nlp_cost_model_set_slice.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] |
||||
self.shared_lib.ocp_nlp_cost_model_set_slice(self.nlp_config, |
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) |
||||
return |
||||
|
||||
def constraints_set(self, start_stage_, field_, value_, api='warn'): |
||||
self.constraints_set_slice(start_stage_, start_stage_+1, field_, value_[None], api='warn') |
||||
return |
||||
|
||||
def constraints_set_slice(self, start_stage_, end_stage_, field_, value_, api='warn'): |
||||
""" |
||||
Set numerical data in the constraint module of the solver. |
||||
|
||||
:param stage: integer corresponding to shooting node |
||||
:param field: string in ['lbx', 'ubx', 'lbu', 'ubu', 'lg', 'ug', 'lh', 'uh', 'uphi'] |
||||
:param value: of appropriate size |
||||
""" |
||||
# cast value_ to avoid conversion issues |
||||
if isinstance(value_, (float, int)): |
||||
value_ = np.array([value_]) |
||||
value_ = value_.astype(float) |
||||
|
||||
field = field_ |
||||
field = field.encode('utf-8') |
||||
dim = np.product(value_.shape[1:]) |
||||
|
||||
start_stage = c_int(start_stage_) |
||||
end_stage = c_int(end_stage_) |
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, POINTER(c_int)] |
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr.restype = c_int |
||||
|
||||
dims = np.ascontiguousarray(np.zeros((2,)), dtype=np.intc) |
||||
dims_data = cast(dims.ctypes.data, POINTER(c_int)) |
||||
|
||||
self.shared_lib.ocp_nlp_constraint_dims_get_from_attr(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_out, start_stage_, field, dims_data) |
||||
|
||||
value_shape = value_.shape |
||||
expected_shape = tuple(np.concatenate([np.array([end_stage_ - start_stage_]), dims])) |
||||
if len(value_shape) == 2: |
||||
value_shape = (value_shape[0], value_shape[1], 0) |
||||
elif len(value_shape) == 3: |
||||
if api=='old': |
||||
pass |
||||
elif api=='warn': |
||||
if not np.all(np.ravel(value_, order='F')==np.ravel(value_, order='K')): |
||||
raise Exception("Ambiguity in API detected.\n" |
||||
"Are you making an acados model from scrach? Add api='new' to constraints_set and carry on.\n" |
||||
"Are you seeing this error suddenly in previously running code? Read on.\n" |
||||
" You are relying on a now-fixed bug in constraints_set for field '{}'.\n".format(field_) + |
||||
" acados_template now correctly passes on any matrices to acados in column major format.\n" + |
||||
" Two options to fix this error: \n" + |
||||
" * Add api='old' to constraints_set to restore old incorrect behaviour\n" + |
||||
" * Add api='new' to constraints_set and remove any unnatural manipulation of the value argument " + |
||||
"such as non-mathematical transposes, reshaping, casting to fortran order, etc... " + |
||||
"If there is no such manipulation, then you have probably been getting an incorrect solution before.") |
||||
# Get elements in column major order |
||||
value_ = np.ravel(value_, order='F') |
||||
elif api=='new': |
||||
# Get elements in column major order |
||||
value_ = np.ravel(value_, order='F') |
||||
else: |
||||
raise Exception("Unknown api: '{}'".format(api)) |
||||
if value_shape != expected_shape: |
||||
raise Exception('AcadosOcpSolver.constraints_set(): mismatching dimension' \ |
||||
' for field "{}" with dimension {} (you have {})'.format(field_, expected_shape, value_shape)) |
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double)) |
||||
value_data_p = cast((value_data), c_void_p) |
||||
|
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p, c_int] |
||||
self.shared_lib.ocp_nlp_constraints_model_set_slice(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_in, start_stage, end_stage, field, value_data_p, dim) |
||||
return |
||||
|
||||
# Note: this function should not be used anymore, better use cost_set, constraints_set |
||||
def set(self, stage_, field_, value_): |
||||
""" |
||||
Set numerical data inside the solver. |
||||
|
||||
:param stage: integer corresponding to shooting node |
||||
:param field: string in ['x', 'u', 'pi', 'lam', 't', 'p'] |
||||
|
||||
.. note:: regarding lam, t: \n |
||||
the inequalities are internally organized in the following order: \n |
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n |
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] |
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n |
||||
lam: multipliers for inequalities \n |
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n |
||||
sl: slack variables of soft lower inequality constraints \n |
||||
su: slack variables of soft upper inequality constraints \n |
||||
""" |
||||
cost_fields = ['y_ref', 'yref'] |
||||
constraints_fields = ['lbx', 'ubx', 'lbu', 'ubu'] |
||||
out_fields = ['x', 'u', 'pi', 'lam', 't', 'z'] |
||||
mem_fields = ['sl', 'su'] |
||||
|
||||
# cast value_ to avoid conversion issues |
||||
if isinstance(value_, (float, int)): |
||||
value_ = np.array([value_]) |
||||
value_ = value_.astype(float) |
||||
|
||||
model_name = self.model_name |
||||
|
||||
field = field_ |
||||
field = field.encode('utf-8') |
||||
|
||||
stage = c_int(stage_) |
||||
|
||||
# treat parameters separately |
||||
if field_ == 'p': |
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").argtypes = [c_void_p, c_int, POINTER(c_double)] |
||||
getattr(self.shared_lib, f"{model_name}_acados_update_params").restype = c_int |
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double)) |
||||
|
||||
assert getattr(self.shared_lib, f"{model_name}_acados_update_params")(self.capsule, stage, value_data, value_.shape[0])==0 |
||||
else: |
||||
if field_ not in constraints_fields + cost_fields + out_fields + mem_fields: |
||||
raise Exception("AcadosOcpSolver.set(): {} is not a valid argument.\ |
||||
\nPossible values are {}. Exiting.".format(field, \ |
||||
constraints_fields + cost_fields + out_fields + ['p'])) |
||||
|
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p] |
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int |
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_out, stage_, field) |
||||
|
||||
if value_.shape[0] != dims: |
||||
msg = 'AcadosOcpSolver.set(): mismatching dimension for field "{}" '.format(field_) |
||||
msg += 'with dimension {} (you have {})'.format(dims, value_.shape) |
||||
raise Exception(msg) |
||||
|
||||
value_data = cast(value_.ctypes.data, POINTER(c_double)) |
||||
value_data_p = cast((value_data), c_void_p) |
||||
|
||||
if field_ in constraints_fields: |
||||
self.shared_lib.ocp_nlp_constraints_model_set.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_constraints_model_set(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p) |
||||
elif field_ in cost_fields: |
||||
self.shared_lib.ocp_nlp_cost_model_set.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_cost_model_set(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_in, stage, field, value_data_p) |
||||
elif field_ in out_fields: |
||||
self.shared_lib.ocp_nlp_out_set.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_out_set(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_out, stage, field, value_data_p) |
||||
elif field_ in mem_fields: |
||||
self.shared_lib.ocp_nlp_set.argtypes = \ |
||||
[c_void_p, c_void_p, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_set(self.nlp_config, \ |
||||
self.nlp_solver, stage, field, value_data_p) |
||||
return |
||||
|
||||
|
||||
def get_slice(self, start_stage_, end_stage_, field_): |
||||
""" |
||||
Get the last solution of the solver: |
||||
|
||||
:param start_stage: integer corresponding to shooting node that indicates start of slice |
||||
:param end_stage: integer corresponding to shooting node that indicates end of slice |
||||
:param field: string in ['x', 'u', 'z', 'pi', 'lam', 't', 'sl', 'su',] |
||||
|
||||
.. note:: regarding lam, t: \n |
||||
the inequalities are internally organized in the following order: \n |
||||
[ lbu lbx lg lh lphi ubu ubx ug uh uphi; \n |
||||
lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi] |
||||
|
||||
.. note:: pi: multipliers for dynamics equality constraints \n |
||||
lam: multipliers for inequalities \n |
||||
t: slack variables corresponding to evaluation of all inequalities (at the solution) \n |
||||
sl: slack variables of soft lower inequality constraints \n |
||||
su: slack variables of soft upper inequality constraints \n |
||||
""" |
||||
out_fields = ['x', 'u', 'z', 'pi', 'lam', 't'] |
||||
mem_fields = ['sl', 'su'] |
||||
field = field_ |
||||
field = field.encode('utf-8') |
||||
|
||||
if (field_ not in out_fields + mem_fields): |
||||
raise Exception('AcadosOcpSolver.get_slice(): {} is an invalid argument.\ |
||||
\n Possible values are {}. Exiting.'.format(field_, out_fields)) |
||||
|
||||
if not isinstance(start_stage_, int): |
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') |
||||
|
||||
if not isinstance(end_stage_, int): |
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be Integer.') |
||||
|
||||
if start_stage_ >= end_stage_: |
||||
raise Exception('AcadosOcpSolver.get_slice(): end stage index must be larger than start stage index') |
||||
|
||||
if start_stage_ < 0 or end_stage_ > self.N + 1: |
||||
raise Exception('AcadosOcpSolver.get_slice(): stage index must be in [0, N], got: {}.'.format(self.N)) |
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p] |
||||
self.shared_lib.ocp_nlp_dims_get_from_attr.restype = c_int |
||||
|
||||
dims = self.shared_lib.ocp_nlp_dims_get_from_attr(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_out, start_stage_, field) |
||||
|
||||
out = np.ascontiguousarray(np.zeros((end_stage_ - start_stage_, dims)), dtype=np.float64) |
||||
out_data = cast(out.ctypes.data, POINTER(c_double)) |
||||
|
||||
if (field_ in out_fields): |
||||
self.shared_lib.ocp_nlp_out_get_slice.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_out_get_slice(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_out, start_stage_, end_stage_, field, out_data) |
||||
elif field_ in mem_fields: |
||||
self.shared_lib.ocp_nlp_get_at_stage.argtypes = \ |
||||
[c_void_p, c_void_p, c_void_p, c_int, c_char_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_get_at_stage(self.nlp_config, \ |
||||
self.nlp_dims, self.nlp_solver, start_stage_, end_stage_, field, out_data) |
||||
|
||||
return out |
||||
|
||||
def get_cost(self): |
||||
""" |
||||
Returns the cost value of the current solution. |
||||
""" |
||||
# compute cost internally |
||||
self.shared_lib.ocp_nlp_eval_cost.argtypes = [c_void_p, c_void_p, c_void_p] |
||||
self.shared_lib.ocp_nlp_eval_cost(self.nlp_solver, self.nlp_in, self.nlp_out) |
||||
|
||||
# create output array |
||||
out = np.ascontiguousarray(np.zeros((1,)), dtype=np.float64) |
||||
out_data = cast(out.ctypes.data, POINTER(c_double)) |
||||
|
||||
# call getter |
||||
self.shared_lib.ocp_nlp_get.argtypes = [c_void_p, c_void_p, c_char_p, c_void_p] |
||||
|
||||
field = "cost_value".encode('utf-8') |
||||
self.shared_lib.ocp_nlp_get(self.nlp_config, self.nlp_solver, field, out_data) |
||||
|
||||
return out[0] |
||||
File diff suppressed because it is too large
Load Diff
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Loading…
Reference in new issue