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来说麻烦很多，请大家自行取舍。