ACADOS學習（2）

這一篇将介紹如果在C/C++環境下配置和使用ACADOS。官網的方法是使用MATLAB來配置和生成C代碼，因為本人不是很喜歡用MATLAB，是以這裡展示如何使用Python生成的代碼後用C++來實作。這個可能不是最佳的實作方法，歡迎大家讨論。

代碼介紹

首先，我們仍然需要Python作為前端生成c代碼，具體内容詳見ACADOS學習（1）。在執行Python代碼後，會自動生成c代碼，預設會在一個c_generated_code的檔案夾中。在我的GitHub中，提供了一個簡易的c++實作，代碼詳見下面解釋

int main()
{
    // 讀取非線性優化器
    nlp_solver_capsule *acados_ocp_capsule = mobile_robot_acados_create_capsule();

    // 載入優化器
    status = mobile_robot_acados_create(acados_ocp_capsule);
	// 确認是否載入成功
    if (status)
    {
        printf("mobile_robot_acados_create() returned status %d. Exiting.\n", status);
        exit(1);
    }
	// 使用仿真器
    status = mobile_robot_acados_sim_create();

    if (status)
    {
        printf("acados_create() simulator returned status %d. Exiting.\n", status);
        exit(1);
    }

    // 獲得一些NLP相關結構體
    ocp_nlp_config *nlp_config = mobile_robot_acados_get_nlp_config(acados_ocp_capsule);
    ocp_nlp_dims *nlp_dims = mobile_robot_acados_get_nlp_dims(acados_ocp_capsule);
    ocp_nlp_in *nlp_in = mobile_robot_acados_get_nlp_in(acados_ocp_capsule);
    ocp_nlp_out *nlp_out = mobile_robot_acados_get_nlp_out(acados_ocp_capsule);

    // 一些相關變量
    N = nlp_dims->N;
    nx = *nlp_dims->nx;
    nu = *nlp_dims->nu;
    printf("time horizion is %d, with state %d and input %d \n", N, nx, nu);

    // 這裡我使用Eigen來存儲資料，這個不是ACADOS必須的
    Eigen::MatrixXd simX((N+1), nx);
    Eigen::MatrixXd simU(N, nu);
    Eigen::VectorXd time_record(N);

    x_current[0] = 0.0;
    x_current[1] = 0.0;
    x_current[2] = 0.0;

    x_target[0] = 2.0;
    x_target[1] = 2.0;
    x_target[2] = 1.0;

    x_state[0] = 2.0;
    x_state[1] = 2.0;
    x_state[2] = 1.0;
    x_state[3] = 0.0;
    x_state[4] = 0.0;

    // 設定N期望目标狀态
    ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, N, "yref", x_target);
	// 設定0-N-1期望狀态
    for(int i=0; i<nx; i++)
        simX(0, i)  = x_current[i];

    for (int i=0; i<N; i++)
        ocp_nlp_cost_model_set(nlp_config, nlp_dims, nlp_in, i, "yref", x_state);

    // 閉環仿真
    for (int ii=0; ii<N; ii++)
    {
        // 計時器
        auto t_start = std::chrono::high_resolution_clock::now();
        // 設定x0
        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "lbx", x_current);
        ocp_nlp_constraints_model_set(nlp_config, nlp_dims, nlp_in, 0, "ubx", x_current);
		// 求解
        status = mobile_robot_acados_solve(acados_ocp_capsule);
        
        // 得到u0
        ocp_nlp_out_get(nlp_config, nlp_dims, nlp_out, 0, "u", &u_current);
        // 隻是為了記錄
        for (int i=0; i<nu; i++)
            simU(ii, i) = u_current[i];
		// 計算運作時間
        auto t_end = std::chrono::high_resolution_clock::now();
        double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end-t_start).count();
        time_record(ii) = elapsed_time_ms;

        // 通過仿真器計算下一個可能的狀态
        sim_in_set(mobile_robot_sim_config, mobile_robot_sim_dims,
                   mobile_robot_sim_in, "u", u_current);
        sim_in_set(mobile_robot_sim_config, mobile_robot_sim_dims,
                   mobile_robot_sim_in, "x", x_current);

        status = mobile_robot_acados_sim_solve();
        if (status != ACADOS_SUCCESS)
        {
            printf("acados_solve() failed with status %d.\n", status);
        }
		// 獲得更新後機器人位置
        sim_out_get(mobile_robot_sim_config, mobile_robot_sim_dims,
                    mobile_robot_sim_out, "x", x_current);

        for (int i=0; i<nx; i++)
            simX(ii+1, i) = x_current[i];



    }

    // 計算運作時間
    for (int i=0; i<N+1; i++)
        printf("Final result index %d %f, %f, %f \n", i, simX(i, 0), simX(i, 1), simX(i, 2));

    printf("average estimation time %f ms \n", time_record.mean());
    printf("max estimation time %f ms \n", time_record.maxCoeff());
    printf("min estimation time %f ms \n", time_record.minCoeff());
    return status;
}
           

代碼的結構和Python部分是基本一緻的。運作時間上看C++代碼并沒有對于Python代碼有明顯優勢，提升隻是ms級别。究其原因，Python其實後端運作也是c代碼。

接下來稍微解釋一下CMakeList.txt的設定

cmake_minimum_required(VERSION 3.1)
project(acados_test LANGUAGES C CXX)

# for macOS 這個隻是為了macOS，Linux下會自動忽略
set(CMAKE_MACOSX_RPATH 1)

# 設定編譯器标準
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall")
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99 -DEXT_DEP -fdiagnostics-show-option")

# 使用Eigen3（非必須）
find_package(PkgConfig)
pkg_search_module(Eigen3 REQUIRED eigen3)
include_directories(${Eigen3_INCLUDE_DIRS})

# 設定引用變量，注意在安裝時候配置好$ENV{ACADOS_SOURCE_DIR}變量
include_directories($ENV{ACADOS_SOURCE_DIR}/include)
include_directories($ENV{ACADOS_SOURCE_DIR}/include/blasfeo/include)
include_directories($ENV{ACADOS_SOURCE_DIR}/include/hpipm/include)
include_directories($ENV{ACADOS_SOURCE_DIR}/include/acados)
include_directories(../python/c_generated_code)

# 載入ACADOS預設庫
link_directories($ENV{ACADOS_SOURCE_DIR}/lib)

file(GLOB ocp_solver
        ../python/c_generated_code/acados_solver_mobile_robot.c
        )

file(GLOB casadi_fun
        ../python/c_generated_code/mobile_robot_model/mobile_robot_expl_ode_fun.c
        ../python/c_generated_code/mobile_robot_model/mobile_robot_expl_vde_forw.c)

file(GLOB sim_solver
        ../python/c_generated_code/acados_sim_solver_mobile_robot.c
        )

# 将原來C代碼設定為庫
add_library(ocp_shared_lib SHARED ${ocp_solver} ${casadi_fun} )
target_link_libraries(ocp_shared_lib acados hpipm blasfeo)
## 這裡使用到仿真器，如果在實際實作的時候這部分就不需要了
add_library(sim_shared_lib SHARED ${sim_solver} ${casadi_fun})
target_link_libraries(sim_shared_lib acados hpipm blasfeo)

# 編譯自己的例子
add_executable(mobile_robot_app src/mobile_robot_app.cpp )
target_link_libraries(mobile_robot_app ocp_shared_lib sim_shared_lib)
           

總結

C++代碼比Python代碼的優勢不算明顯，好處是不需要每次都編譯一次模型，而是直接載入編譯好的檔案（暫時我還不知道如何用ACADOS直接讀取編譯好的動态連結庫而跳過編譯），計算時間在本例中大概比Python快1ms左右。缺點是Debug和展示資料比Python來說麻煩很多，請大家自行取舍。