def acados_dae_model_json_dump(model):
# load model
x = model.x
xdot = model.xdot
u = model.u
z = model.z
p = model.p
f_impl = model.f_impl_expr
model_name = model.name
# create struct with impl_dae_fun, casadi_version
fun_name = model_name + '_impl_dae_fun'
impl_dae_fun = Function(fun_name, [x, xdot, u, z, p], [f_impl])
casadi_version = CasadiMeta.version()
str_impl_dae_fun = impl_dae_fun.serialize()
dae_dict = {
"str_impl_dae_fun": str_impl_dae_fun,
"casadi_version": casadi_version
}
# dump
json_file = model_name + '_acados_dae.json'
with open(json_file, 'w') as f:
json.dump(dae_dict,
f,
default=np_array_to_list,
indent=4,
sort_keys=True)
print("dumped ", model_name, " dae to file:", json_file, "\n")
def set_up_imported_gnsf_model(acados_formulation):
gnsf = acados_formulation.gnsf_model
# check CasADi version
dump_casadi_version = gnsf['casadi_version']
casadi_version = CasadiMeta.version()
if not casadi_version == dump_casadi_version:
print(
"WARNING: GNSF model was dumped with another CasADi version.\n" +
"This might yield errors. Please use the same version for compatibility, serialize version: "
+ dump_casadi_version + " current Python CasADi verison: " +
casadi_version)
input("Press any key to attempt to continue...")
# load model
phi_fun = Function.deserialize(gnsf['phi_fun'])
phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
# obtain gnsf dimensions
size_gnsf_A = get_matrices_fun.size_out(0)
acados_formulation.dims.gnsf_nx1 = size_gnsf_A[1]
acados_formulation.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
acados_formulation.dims.gnsf_nuhat = max(phi_fun.size_in(1))
acados_formulation.dims.gnsf_ny = max(phi_fun.size_in(0))
acados_formulation.dims.gnsf_nout = max(phi_fun.size_out(0))
# save gnsf functions in model
acados_formulation.model.phi_fun = phi_fun
acados_formulation.model.phi_fun_jac_y = phi_fun_jac_y
acados_formulation.model.phi_jac_y_uhat = phi_jac_y_uhat
acados_formulation.model.get_matrices_fun = get_matrices_fun
if "f_lo_fun_jac_x1k1uz" in gnsf:
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
acados_formulation.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
else:
dummy_var_x1 = SX.sym('dummy_var_x1', acados_formulation.dims.gnsf_nx1)
dummy_var_x1dot = SX.sym('dummy_var_x1dot',
acados_formulation.dims.gnsf_nx1)
dummy_var_z1 = SX.sym('dummy_var_z1', acados_formulation.dims.gnsf_nz1)
dummy_var_u = SX.sym('dummy_var_z1', acados_formulation.dims.nu)
dummy_var_p = SX.sym('dummy_var_z1', acados_formulation.dims.np)
empty_var = SX.sym('empty_var', 0, 0)
empty_fun = Function('empty_fun', \
[dummy_var_x1, dummy_var_x1dot, dummy_var_z1, dummy_var_u, dummy_var_p],
[empty_var])
acados_formulation.model.f_lo_fun_jac_x1k1uz = empty_fun
del acados_formulation.gnsf_model
def set_up_imported_gnsf_model(acados_ocp):
gnsf = acados_ocp.gnsf_model
# check CasADi version
dump_casadi_version = gnsf['casadi_version']
casadi_version = CasadiMeta.version()
if not casadi_version == dump_casadi_version:
raise Exception("GNSF model was dumped with another CasADi version.\n"
+ "Please use the same version for compatibility, serialize version:"
+ dump_casadi_version + " current Python CasADi verison: " + casadi_version)
# load model
phi_fun = Function.deserialize(gnsf['phi_fun'])
phi_fun_jac_y = Function.deserialize(gnsf['phi_fun_jac_y'])
phi_jac_y_uhat = Function.deserialize(gnsf['phi_jac_y_uhat'])
f_lo_fun_jac_x1k1uz = Function.deserialize(gnsf['f_lo_fun_jac_x1k1uz'])
get_matrices_fun = Function.deserialize(gnsf['get_matrices_fun'])
# obtain gnsf dimensions
size_gnsf_A = get_matrices_fun.size_out(0)
acados_ocp.dims.gnsf_nx1 = size_gnsf_A[1]
acados_ocp.dims.gnsf_nz1 = size_gnsf_A[0] - size_gnsf_A[1]
acados_ocp.dims.gnsf_nuhat = max(phi_fun.size_in(1))
acados_ocp.dims.gnsf_ny = max(phi_fun.size_in(0))
acados_ocp.dims.gnsf_nout = max(phi_fun.size_out(0))
# save gnsf functions in model
acados_ocp.model.phi_fun = phi_fun
acados_ocp.model.phi_fun_jac_y = phi_fun_jac_y
acados_ocp.model.phi_jac_y_uhat = phi_jac_y_uhat
acados_ocp.model.f_lo_fun_jac_x1k1uz = f_lo_fun_jac_x1k1uz
acados_ocp.model.get_matrices_fun = get_matrices_fun
del acados_ocp.gnsf_model
def generate_c_code_external_cost(model, is_terminal):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
if is_terminal:
suffix_name = "_cost_ext_cost_e_fun"
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_e_fun_jac"
u = symbol("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
else:
suffix_name = "_cost_ext_cost_fun"
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
suffix_name_jac = "_cost_ext_cost_fun_jac"
u = model.u
ext_cost = model.cost_expr_ext_cost
# set up functions to be exported
fun_name = model.name + suffix_name
fun_name_hess = model.name + suffix_name_hess
fun_name_jac = model.name + suffix_name_jac
# generate expression for full gradient and Hessian
full_hess, grad = hessian(ext_cost, vertcat(u, x))
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]
)
ext_cost_fun_jac = Function(
fun_name_jac, [x, u, p], [ext_cost, grad]
)
# generate C code
if not os.path.exists("c_generated_code"):
os.mkdir("c_generated_code")
os.chdir("c_generated_code")
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir("../..")
return
def generate_c_code_external_cost(model, is_terminal):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
msg = "Please download and install CasADi {} ".format(
" or ".join(ALLOWED_CASADI_VERSIONS))
msg += "to ensure compatibility with acados.\n"
msg += "Version {} currently in use.".format(casadi_version)
raise Exception(msg)
if is_terminal:
suffix_name = "_ext_cost_e_fun"
suffix_name_hess = "_ext_cost_e_fun_jac_hess"
u = SX.sym("u", 0, 0)
ext_cost = model.cost_expr_ext_cost_e
else:
suffix_name = "_ext_cost_fun"
suffix_name_hess = "_ext_cost_fun_jac_hess"
u = model.u
ext_cost = model.cost_expr_ext_cost
x = model.x
p = model.p
# set up functions to be exported
fun_name = model.name + suffix_name
fun_name_hess = model.name + suffix_name_hess
# generate jacobians
jac_x = jacobian(ext_cost, x)
jac_u = jacobian(ext_cost, u)
# generate hessians
hess_uu = jacobian(jac_u.T, u)
hess_xu = jacobian(jac_u.T, x)
hess_ux = jacobian(jac_x.T, u)
hess_xx = jacobian(jac_x.T, x)
full_hess = vertcat(horzcat(hess_uu, hess_xu), horzcat(hess_ux, hess_xx))
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, vertcat(jac_u.T, jac_x.T), full_hess])
# generate C code
if not os.path.exists("c_generated_code"):
os.mkdir("c_generated_code")
os.chdir("c_generated_code")
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
os.chdir("../..")
return
def generate_c_code_external_cost(model, stage_type, opts):
casadi_version = CasadiMeta.version()
casadi_opts = dict(mex=False, casadi_int="int", casadi_real="double")
if casadi_version not in (ALLOWED_CASADI_VERSIONS):
casadi_version_warning(casadi_version)
x = model.x
p = model.p
if isinstance(x, MX):
symbol = MX.sym
else:
symbol = SX.sym
if stage_type == 'terminal':
suffix_name = "_cost_ext_cost_e_fun"
suffix_name_hess = "_cost_ext_cost_e_fun_jac_hess"
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"
suffix_name_hess = "_cost_ext_cost_fun_jac_hess"
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"
suffix_name_hess = "_cost_ext_cost_0_fun_jac_hess"
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
fun_name_hess = model.name + suffix_name_hess
fun_name_jac = model.name + suffix_name_jac
# 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])
ext_cost_fun_jac = Function(fun_name_jac, [x, u, p], [ext_cost, grad])
# generate C code
code_export_dir = opts["code_export_directory"]
if not os.path.exists(code_export_dir):
os.makedirs(code_export_dir)
cwd = os.getcwd()
os.chdir(code_export_dir)
gen_dir = model.name + '_cost'
if not os.path.exists(gen_dir):
os.mkdir(gen_dir)
gen_dir_location = "./" + gen_dir
os.chdir(gen_dir_location)
ext_cost_fun.generate(fun_name, casadi_opts)
ext_cost_fun_jac_hess.generate(fun_name_hess, casadi_opts)
ext_cost_fun_jac.generate(fun_name_jac, casadi_opts)
os.chdir(cwd)
return
