Example #1
0
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")
Example #2
0
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
Example #3
0
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
Example #6
0
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