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