├── .clangd ├── .gitignore ├── configure.sh ├── CMakeLists.txt ├── cmake └── ACADOSCppConfig.cmake ├── test ├── CMakeLists.txt ├── generate_solver.py └── test.cpp ├── test.sh ├── README.md ├── flake.nix ├── flake.lock └── include └── ACADOSCpp ├── Capsule.hpp └── Capsule.ipp /.clangd: -------------------------------------------------------------------------------- 1 | Diagnostics: 2 | Suppress: "pp_including_mainfile_in_preamble" -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | ./lib/ 3 | .cache/ 4 | .vscode/ 5 | Testing/ 6 | codegen/ -------------------------------------------------------------------------------- /configure.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | cmake -S . -B build -DACADOS_CPP_BUILD_TESTS=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(ACADOSCpp) 4 | 5 | option(ACADOS_CPP_BUILD_TESTS "Build tests." OFF) 6 | 7 | include_directories(${CMAKE_CURRENT_LIST_DIR}/include) 8 | 9 | find_package(acados REQUIRED) 10 | 11 | if(ACADOS_CPP_BUILD_TESTS) 12 | add_subdirectory(test/) 13 | endif() 14 | -------------------------------------------------------------------------------- /cmake/ACADOSCppConfig.cmake: -------------------------------------------------------------------------------- 1 | 2 | message(STATUS "Found ACADOSCpp. ${CMAKE_CURRENT_LIST_DIR}") 3 | 4 | set(ACADOSCPP_FOUND TRUE) 5 | 6 | find_package(acados REQUIRED) 7 | 8 | 9 | add_library(ACADOSCpp INTERFACE) 10 | target_include_directories(ACADOSCpp INTERFACE ${CMAKE_CURRENT_SOURCE_DIR}/include) 11 | target_link_libraries(ACADOSCpp INTERFACE acados dl) -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | 2 | set(CMAKE_BUILD_TYPE Debug) 3 | 4 | list(APPEND CMAKE_PREFIX_PATH ${CMAKE_CURRENT_LIST_DIR}/../) 5 | 6 | find_package(ACADOSCpp REQUIRED) 7 | find_package(GTest REQUIRED) 8 | 9 | enable_testing() 10 | 11 | add_executable( 12 | testACADOSCpp 13 | test.cpp 14 | ) 15 | 16 | target_link_libraries( 17 | testACADOSCpp 18 | GTest::gtest_main 19 | ACADOSCpp 20 | ) 21 | 22 | include(GoogleTest) 23 | 24 | gtest_discover_tests(testACADOSCpp) 25 | -------------------------------------------------------------------------------- /test.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | cmake -S . -B build -DACADOS_CPP_BUILD_TESTS=ON 4 | cmake --build build 5 | 6 | 7 | # Check if codegen dir exists 8 | if [ ! -d "./codegen/test_model/build" ]; then 9 | echo "Generating test model" 10 | python ./test/generate_solver.py 11 | fi 12 | 13 | 14 | # Add test lib dir to LD_LIBRARY_PATH 15 | script_dir=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) 16 | 17 | # ACADOS seems to install the library at somewhat arbitrary locations. 18 | test_model_install_dir=$(dirname $(find $script_dir -name "libacados_ocp_solver_test_model.so" | head -n 1)) 19 | 20 | export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$test_model_install_dir 21 | 22 | ./build/test/testACADOSCpp 23 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ACADOSCpp 2 | 3 | `C++` wrapper to dynamically run `C` code generated by [ACADOS](https://github.com/acados/acados). Allows run-time linking against generated solvers. 4 | 5 | ## Usage 6 | 7 | ```cmake 8 | find_package(ACADOSCpp REQUIRED) 9 | 10 | # ACADOSCpp is header-only but for portability, you may link against it. 11 | target_link_libraries(my_target ACADOSCpp) 12 | ``` 13 | 14 | See test cases for code examples. 15 | 16 | ## Why? 17 | 18 | ### Building 19 | 20 | Currently, the best way to use ACADOS' generated code within `C++` projects is to create a custom build target, directly `#include` the generated headers, link/add source files, and use the code directly. This adds unnecessary complexity and makes it incredibly difficult to re-use the same code around different generated solvers without the use of macros and re-compilation. 21 | 22 | ### Interface & Usability 23 | 24 | Even if you are fine with having the code generator as a compile-time dependency, the interface is meant for `C`, not `C++`. I assume that, if you are reading this, you have used the `C` interface, and you know what I mean. 25 | -------------------------------------------------------------------------------- /test/generate_solver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | ''' 5 | 6 | import os 7 | 8 | import casadi as ca 9 | 10 | import numpy as np 11 | 12 | from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel 13 | from acados_template.builders import CMakeBuilder 14 | 15 | nu = 3 16 | nx = 2*nu 17 | 18 | model = AcadosModel() 19 | 20 | 21 | model.name = 'test_model' 22 | 23 | x1 = ca.MX.sym('x1', nu) 24 | x2 = ca.MX.sym('x2', nu) 25 | 26 | model.x = ca.vertcat(x1, x2) 27 | model.u = ca.MX.sym('u', nu) 28 | 29 | model.f_expl_expr = ca.vertcat(x2, model.u) 30 | 31 | ocp = AcadosOcp() 32 | 33 | ocp.model = model 34 | 35 | ocp.solver_options.tf = 1.0 36 | ocp.dims.N = 20 37 | 38 | ocp.cost.cost_type = 'NONLINEAR_LS' 39 | 40 | ocp.model.cost_y_expr = ocp.model.x 41 | ocp.model.cost_y_expr_e = ocp.model.x 42 | 43 | ocp.cost.yref = np.zeros(ocp.model.cost_y_expr.rows()) 44 | ocp.cost.W = np.eye(ocp.model.cost_y_expr.rows(), ocp.model.cost_y_expr.rows()) 45 | 46 | ocp.cost.cost_type_e = 'NONLINEAR_LS' 47 | ocp.cost.W_e = np.eye(ocp.model.cost_y_expr_e.rows(), ocp.model.cost_y_expr_e.rows()) 48 | ocp.cost.yref_e = np.zeros(ocp.model.cost_y_expr_e.rows()) 49 | 50 | ocp.constraints.lbu = -np.ones(nu) 51 | ocp.constraints.ubu = np.ones(nu) 52 | ocp.constraints.idxbu = np.arange(nu) 53 | 54 | ocp.solver_options.integrator_type = 'ERK' 55 | ocp.solver_options.qp_solver_cond_N = 1 # Condensing steps 56 | ocp.solver_options.nlp_solver_type = 'SQP_RTI' 57 | 58 | code_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../codegen/test_model')) 59 | install_dir = os.path.abspath(os.path.join(os.path.dirname(__file__), '../install')) 60 | 61 | os.makedirs(install_dir, exist_ok=True) 62 | os.makedirs(code_dir, exist_ok=True) 63 | os.chdir(code_dir) 64 | 65 | ocp.code_export_directory = code_dir 66 | 67 | builder = CMakeBuilder() 68 | 69 | builder.options_on = [ 70 | 'BUILD_ACADOS_OCP_SOLVER_LIB', 71 | ] 72 | 73 | builder.options_off = [ 74 | 'BUILD_EXAMPLE' 75 | ] 76 | 77 | ocp_solver = AcadosOcpSolver( 78 | ocp, 79 | json_file='acados_ocp.json', 80 | build=True, 81 | generate=True, 82 | cmake_builder=builder 83 | ) 84 | 85 | -------------------------------------------------------------------------------- /flake.nix: -------------------------------------------------------------------------------- 1 | { 2 | description = "C++ wrapper for ACADOS."; 3 | 4 | inputs = { 5 | nixpkgs.url = "github:NixOS/nixpkgs"; 6 | 7 | acados-overlay.url = "github:adrian-kriegel/acados-nix"; 8 | }; 9 | 10 | outputs = { self, nixpkgs, acados-overlay }: 11 | let 12 | pkgs = import nixpkgs { 13 | system = "x86_64-linux"; 14 | overlays = [ acados-overlay.overlays.default ]; 15 | }; 16 | in { 17 | packages.x86_64-linux.acados-cpp = pkgs.stdenv.mkDerivation rec { 18 | pname = "acados-cpp"; 19 | version = "0.1.0"; 20 | 21 | src = ./.; 22 | 23 | # Intentionally not adding ACADOS as a dependency. ACADOSCpp is header-only 24 | # so nothing links to ACADOS and no build step is required. 25 | # Users can just use their own ACADOS installation. 26 | buildInputs = []; 27 | 28 | nativeBuildInputs = []; 29 | 30 | installPhase = '' 31 | mkdir -p $out/include 32 | cp -r ${src}/include/* $out/include 33 | 34 | mkdir -p $out/cmake 35 | cp ${src}/cmake/* $out/cmake 36 | ''; 37 | 38 | meta = with pkgs.lib; { 39 | description = "C++ wrapper for ACADOS."; 40 | homepage = "https://github.com/adrian-kriegel/ACADOSCpp"; 41 | license = licenses.mit; 42 | maintainers = [ maintainers.adrian-kriegel ]; 43 | }; 44 | }; 45 | 46 | overlays.default = final: prev: { 47 | acados-cpp = self.packages.${prev.system}.acados-cpp; 48 | }; 49 | 50 | devShells.x86_64-linux.default = pkgs.mkShell { 51 | 52 | nativeBuildInputs = with pkgs; [ 53 | gcc 54 | cmake 55 | ]; 56 | 57 | buildInputs = with pkgs; [ 58 | acados 59 | (pkgs.python312.withPackages (ps: with ps; [ 60 | numpy 61 | acados_template 62 | ])) 63 | gtest 64 | ]; 65 | 66 | shellHook = with pkgs; '' 67 | export ACADOS_DIR=${acados} 68 | export CPLUS_INCLUDE_PATH=$CMAKE_INCLUDE_PATH:$(pwd)/include 69 | ''; 70 | }; 71 | }; 72 | } -------------------------------------------------------------------------------- /flake.lock: -------------------------------------------------------------------------------- 1 | { 2 | "nodes": { 3 | "acados-overlay": { 4 | "inputs": { 5 | "nixpkgs": "nixpkgs", 6 | "systems": "systems" 7 | }, 8 | "locked": { 9 | "lastModified": 1742717421, 10 | "narHash": "sha256-fEl1mhIzFZ6hewtbU1Byis4Hc3ash+HeVhaM4ukFUUk=", 11 | "owner": "adrian-kriegel", 12 | "repo": "acados-nix", 13 | "rev": "85b2b3f9a5c983768fc519fa11616cba24ab0d2c", 14 | "type": "github" 15 | }, 16 | "original": { 17 | "owner": "adrian-kriegel", 18 | "repo": "acados-nix", 19 | "type": "github" 20 | } 21 | }, 22 | "nixpkgs": { 23 | "locked": { 24 | "lastModified": 1742709858, 25 | "narHash": "sha256-Z3b9Exe17A8YqypZHs/Q29hp9InCz+uAyUksXMZR61s=", 26 | "owner": "NixOS", 27 | "repo": "nixpkgs", 28 | "rev": "f9ec383a105dfa6ea11dc7527563aa6e929503bf", 29 | "type": "github" 30 | }, 31 | "original": { 32 | "owner": "NixOS", 33 | "repo": "nixpkgs", 34 | "type": "github" 35 | } 36 | }, 37 | "nixpkgs_2": { 38 | "locked": { 39 | "lastModified": 1731157057, 40 | "narHash": "sha256-9syOK3Q59P86NIeU9uIxODq0eFgNjdcP+k9dI32clI0=", 41 | "owner": "NixOS", 42 | "repo": "nixpkgs", 43 | "rev": "3b91a81423cd1bff1bb173d3d45718bd38177fea", 44 | "type": "github" 45 | }, 46 | "original": { 47 | "owner": "NixOS", 48 | "repo": "nixpkgs", 49 | "type": "github" 50 | } 51 | }, 52 | "root": { 53 | "inputs": { 54 | "acados-overlay": "acados-overlay", 55 | "nixpkgs": "nixpkgs_2" 56 | } 57 | }, 58 | "systems": { 59 | "locked": { 60 | "lastModified": 1681028828, 61 | "narHash": "sha256-Vy1rq5AaRuLzOxct8nz4T6wlgyUR7zLU309k9mBC768=", 62 | "owner": "nix-systems", 63 | "repo": "default", 64 | "rev": "da67096a3b9bf56a91d16901293e51ba5b49a27e", 65 | "type": "github" 66 | }, 67 | "original": { 68 | "owner": "nix-systems", 69 | "repo": "default", 70 | "type": "github" 71 | } 72 | } 73 | }, 74 | "root": "root", 75 | "version": 7 76 | } 77 | -------------------------------------------------------------------------------- /test/test.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | #include 5 | 6 | using namespace acados::solver; 7 | 8 | void test_capsule(Capsule &ca) { 9 | 10 | int print_level = 0; 11 | ca.set_solver_option("print_level", &print_level); 12 | 13 | // check that dims matches the exported values TODO: assert entire dims 14 | // struct 15 | EXPECT_EQ(ca.dims().N, 20); 16 | 17 | for (uint i = 0; i < ca.dims().N; ++i) { 18 | EXPECT_EQ(ca.dims().nx[i], 6); 19 | EXPECT_EQ(ca.dims().nu[i], 3); 20 | } 21 | 22 | const int n = ca.dims().N; 23 | const int nx = ca.dims().nx[0]; 24 | const int nu = ca.dims().nu[0]; 25 | const int num_lbx0 = ca.get_constraint_dims(0, "lbx"); 26 | const int num_ubx0 = ca.get_constraint_dims(0, "ubx"); 27 | 28 | EXPECT_EQ(num_ubx0, 0); 29 | EXPECT_EQ(num_lbx0, 0); 30 | 31 | // initial conditions 32 | std::vector lbx0(num_lbx0, 0.0); 33 | std::vector ubx0(num_lbx0, 0.0); 34 | // vector::data() is nullptr if capacity is zero 35 | // set_constraints_for_stage complains if we pass nullptr 36 | lbx0.reserve(lbx0.size() + 1); 37 | ubx0.reserve(ubx0.size() + 1); 38 | 39 | // initialization for state values (repeated ) 40 | std::vector x_init(nx, 0.0); 41 | 42 | // initial value for control input 43 | std::vector u0(nu, 0.0); 44 | 45 | // container for the output state trajectory 46 | std::vector xtraj(nx * (n + 1), 0.0); 47 | // container for the output controls trajectory 48 | std::vector utraj(nu * n, 0.0); 49 | 50 | ca.set_constraints_for_stage(0, "lbx", lbx0.data()); 51 | ca.set_constraints_for_stage(0, "ubx", ubx0.data()); 52 | 53 | // prepare evaluation 54 | int NTIMINGS = 10; 55 | double min_time = 1e12; 56 | double kkt_norm_inf; 57 | double elapsed_time; 58 | int sqp_iter; 59 | // solve ocp in loop 60 | int rti_phase = 0; 61 | 62 | for (int ii = 0; ii < NTIMINGS; ii++) { 63 | // initialize solution 64 | for (int i = 0; i < n; i++) { 65 | ca.set_output(i, "x", x_init.data()); 66 | ca.set_output(i, "u", u0.data()); 67 | } 68 | 69 | ca.set_output(n, "x", x_init.data()); 70 | 71 | ca.set_option("rti_phase", &rti_phase); 72 | 73 | ca.solve(); 74 | 75 | ca.get(&elapsed_time, "time_tot"); 76 | 77 | min_time = std::min(elapsed_time, min_time); 78 | } 79 | 80 | /* print solution and statistics */ 81 | for (int ii = 0; ii <= ca.dims().N; ii++) { 82 | ca.get_output(&xtraj[ii * nx], ii, "x"); 83 | } 84 | 85 | for (int ii = 0; ii < ca.dims().N; ii++) { 86 | ca.get_output(&utraj[ii * nu], ii, "u"); 87 | } 88 | 89 | // just checking a couple of hard coded values takeen from the example 90 | // generated by ACADOS I'm assuming that if those are correct, the entire 91 | // solution is correct, as there is no reason to assume that the CPP wrapper 92 | // messed up the solver that bad. 93 | EXPECT_NEAR(xtraj[0], 0.0, 1e-5); 94 | EXPECT_NEAR(xtraj.back(), 0.0, 1e-5); 95 | EXPECT_NEAR(utraj[0], 0.0, 1e-5); 96 | EXPECT_NEAR(utraj.back(), 0.0, 1e-5); 97 | } 98 | 99 | /** 100 | * This is a C++ified version of the example program that can be optionally 101 | * generated with ACADOS with added assertions. 102 | */ 103 | TEST(TestACADOSCpp, ExampleProgram) { 104 | 105 | Capsule ca("libacados_ocp_solver_test_model.so", "test_model"); 106 | 107 | test_capsule(ca); 108 | } 109 | 110 | TEST(TestACADOSCpp, MoveCTor) { 111 | 112 | Capsule ca1("libacados_ocp_solver_test_model.so", "test_model"); 113 | 114 | Capsule ca2 = std::move(ca1); 115 | 116 | EXPECT_EQ(ca1.get_dl_handle(), nullptr); 117 | EXPECT_EQ(ca1.get_prefix(), ""); 118 | EXPECT_EQ(ca1.get_capsule_ptr(), nullptr); 119 | 120 | test_capsule(ca2); 121 | } 122 | -------------------------------------------------------------------------------- /include/ACADOSCpp/Capsule.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _ACADOS_CPP_SOLVER_HPP_ 3 | #define _ACADOS_CPP_SOLVER_HPP_ 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | namespace acados::solver { 13 | 14 | // yes. 15 | using capsule_ptr = void *; 16 | /** acados_solve(capsule_) */ 17 | using solve_t = int(capsule_ptr); 18 | using print_stats_t = void(capsule_ptr); 19 | using reset_t = void(capsule_ptr, int); 20 | 21 | enum RTIPhase { 22 | // See https://discourse.acados.org/t/rti-phases-in-python-interface/190 23 | // (1) preparation, (2) feedback, (0) both. 24 | BOTH = 0, 25 | PREPARATION = 1, 26 | FEEDBACK = 2 27 | }; 28 | 29 | enum class SolveResult { 30 | SUCCESS = return_values::ACADOS_SUCCESS, 31 | NAN_DETECTED = ACADOS_NAN_DETECTED, 32 | MAXITER = ACADOS_MAXITER, 33 | MINSTEP = ACADOS_MINSTEP, 34 | QP_FAILURE = ACADOS_QP_FAILURE, 35 | READY = ACADOS_READY, 36 | UNBOUNDED = ACADOS_UNBOUNDED, 37 | }; // enum class SolveResult 38 | 39 | /** 40 | * This struct contains all ACADOS related pointers. 41 | * I bunched them all together to make the move ctor of Capsule simpler. 42 | */ 43 | class ACADOSData { 44 | 45 | public: 46 | const ocp_nlp_dims &dims() const { return *dims_; } 47 | 48 | public: 49 | ocp_nlp_config *config_ = nullptr; 50 | 51 | ocp_nlp_in *in_ = nullptr; 52 | ocp_nlp_out *out_ = nullptr; 53 | ocp_nlp_solver *solver_ = nullptr; 54 | void *opts_ = nullptr; 55 | 56 | ocp_nlp_dims *dims_{}; 57 | 58 | // ptr to acados solve function 59 | solve_t *solve_; 60 | 61 | // ptr to acados print_stats function 62 | print_stats_t *print_stats_; 63 | 64 | reset_t *reset_; 65 | }; 66 | 67 | class Capsule : public ACADOSData { 68 | public: 69 | /** 70 | * Create a solver capsule. 71 | * @param lib Name or path of OCP solver .so file. 72 | * @param prefix Prefix prepended to acados functions within the .so file. 73 | * TODO: This constructor may throw. I would prefer a factory instead 74 | */ 75 | Capsule(const std::string &lib, const std::string &prefix); 76 | 77 | /** 78 | * Construct a capsule from an existing dynamic library handle. 79 | * IMPORTANT: The capsule will take ownership of the handle and free it on 80 | * destruction. 81 | * @param dl_handle Dynamic library handle. 82 | * @param prefix Prefix prepended to acados functions within the .so file. 83 | */ 84 | Capsule(void *&&dl_handle, const std::string &prefix); 85 | 86 | Capsule() = default; 87 | 88 | Capsule(Capsule &&other); 89 | 90 | const std::string &get_prefix() const { return prefix_; } 91 | 92 | /** 93 | * @throws runtime_error 94 | * @deprecated use solve_noexcept instead 95 | */ 96 | void solve(); 97 | 98 | /** 99 | * @returns SolveResult::SUCCESS on success, other values on failure. 100 | */ 101 | [[nodiscard]] SolveResult solve_noexcept() noexcept; 102 | 103 | void set_rti_phase(RTIPhase phase); 104 | 105 | void print_stats() const; 106 | 107 | void reset(bool reset_qp_mem = true); 108 | 109 | void set_constraints_for_stage(uint stage, const std::string &field, 110 | const double *values); 111 | 112 | /** 113 | * Sets fields in the output struct of an nlp solver, used to initialize the 114 | * solver. 115 | */ 116 | void set_output(uint stage, const std::string &field, const double *values); 117 | 118 | /** 119 | * Sets an option on the opts_ struct. 120 | */ 121 | void set_option(const std::string &field, const void *value); 122 | 123 | void set_solver_option(const std::string &field, const void *value); 124 | 125 | void get_output(void *out, uint stage, const std::string &field) const; 126 | 127 | void set_cost_model(uint stage, const std::string &field, const void *value); 128 | 129 | /** 130 | * Wrapper for ocp_nlp_get which has *zero* documentation and idk what it does 131 | * tbh. 132 | */ 133 | void get(void *out, const std::string &field) const; 134 | 135 | int get_constraint_dims(uint stage, const std::string &field) const; 136 | 137 | /** ocp_nlp_eval_cost */ 138 | void eval_cost(); 139 | 140 | /** ocp_nlp_eval_residuals */ 141 | void eval_residuals(); 142 | 143 | /** @returns ptr to the dynamic library handle. */ 144 | void *get_dl_handle() const { return dl_handle_; } 145 | 146 | /** @returns ptr to the internal ACADOS capsule. */ 147 | capsule_ptr get_capsule_ptr() const { return capsule_; } 148 | 149 | ~Capsule(); 150 | 151 | protected: 152 | /** 153 | * Get symbol from the dl prefixed by _acado_ 154 | */ 155 | void *get_symbol(const std::string &name) const; 156 | 157 | /** 158 | * Shorthand for T target = _acado_nlp_get_(capsule_); 159 | */ 160 | template void get_nlp(T &target, const std::string &name) const { 161 | typedef T (*getter_t)(decltype(capsule_)); 162 | 163 | getter_t getter = (getter_t)get_symbol("get_nlp_" + name); 164 | 165 | target = getter(capsule_); 166 | } 167 | 168 | protected: 169 | // prefix before every symbol prepended by the codegen 170 | std::string prefix_; 171 | 172 | protected: 173 | capsule_ptr capsule_ = nullptr; 174 | 175 | // dynamic library handle 176 | void *dl_handle_ = nullptr; 177 | 178 | private: 179 | // making copy ctor private ensures ownership of capsule_ and dl_handle_ 180 | Capsule(const Capsule &) = delete; 181 | 182 | }; // class Capsule 183 | 184 | } // namespace acados::solver 185 | 186 | #include "ACADOSCpp/Capsule.ipp" 187 | 188 | #endif // _ACADOS_CPP_SOLVER_HPP_ 189 | -------------------------------------------------------------------------------- /include/ACADOSCpp/Capsule.ipp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef _ACADOS_CPP_SOLVER_CAPSULE_IPP_ 3 | #define _ACADOS_CPP_SOLVER_CAPSULE_IPP_ 4 | 5 | #include "ACADOSCpp/Capsule.hpp" 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | namespace acados::solver { 12 | 13 | inline Capsule::Capsule(Capsule &&other) 14 | : ACADOSData(other), prefix_(std::move(other.prefix_)), 15 | capsule_(std::move(other.capsule_)), 16 | dl_handle_(std::move(other.dl_handle_)) { 17 | other.capsule_ = nullptr; 18 | other.dl_handle_ = nullptr; 19 | } 20 | 21 | inline Capsule::Capsule(void *&&dl_handle, const std::string &prefix) 22 | : prefix_(prefix), dl_handle_(std::move(dl_handle)) { 23 | 24 | // 25 | if (!dl_handle_) { 26 | throw std::runtime_error("Failed to load solver library: " + 27 | std::string(dlerror())); 28 | } 29 | // reset error state 30 | dlerror(); 31 | 32 | typedef void *(*create_capsule_t)(void); 33 | typedef int (*create_t)(void *); 34 | 35 | auto create_capsule = (create_capsule_t)get_symbol("create_capsule"); 36 | auto create = (create_t)get_symbol("create"); 37 | 38 | capsule_ = create_capsule(); 39 | 40 | int status = create(capsule_); 41 | 42 | if (status != ACADOS_SUCCESS) { 43 | throw std::runtime_error("create(capsule) returned status " + 44 | std::to_string(status)); 45 | } 46 | 47 | solve_ = (solve_t *)get_symbol("solve"); 48 | print_stats_ = (print_stats_t *)get_symbol("print_stats"); 49 | reset_ = (reset_t *)get_symbol("reset"); 50 | 51 | get_nlp(config_, "config"); 52 | get_nlp(dims_, "dims"); 53 | get_nlp(in_, "in"); 54 | get_nlp(out_, "out"); 55 | get_nlp(solver_, "solver"); 56 | get_nlp(opts_, "opts"); 57 | } 58 | 59 | inline Capsule::Capsule(const std::string &lib, const std::string &prefix) 60 | : Capsule(dlopen(lib.c_str(), RTLD_NOW), prefix) {} 61 | 62 | inline Capsule::~Capsule() { 63 | 64 | if (capsule_) { 65 | using free_capsule_t = int(void *); 66 | 67 | // TODO: this may throw, causing dl_handle_ to leak 68 | auto free_capsule = (free_capsule_t *)get_symbol("free_capsule"); 69 | 70 | free_capsule(capsule_); 71 | } 72 | 73 | if (dl_handle_) { 74 | dlclose(dl_handle_); 75 | } 76 | } 77 | 78 | inline void *Capsule::get_symbol(const std::string &name) const { 79 | void *symbol = dlsym(dl_handle_, (prefix_ + "_acados_" + name).c_str()); 80 | 81 | const char *dlsym_error = dlerror(); 82 | 83 | if (dlsym_error) { 84 | throw std::runtime_error("Could not find symbol " + name + ": " + 85 | std::string(dlsym_error)); 86 | } 87 | 88 | return symbol; 89 | } 90 | 91 | inline void Capsule::solve() { 92 | 93 | SolveResult status = solve_noexcept(); 94 | 95 | if (status != SolveResult::SUCCESS) { 96 | throw std::runtime_error("ACADOS solve() returned status " + 97 | std::to_string((int)status) + "."); 98 | } 99 | } 100 | 101 | inline SolveResult Capsule::solve_noexcept() noexcept { 102 | return (SolveResult)solve_(capsule_); 103 | } 104 | 105 | inline void Capsule::set_rti_phase(RTIPhase phase) { 106 | set_option("rti_phase", &phase); 107 | } 108 | 109 | inline void Capsule::print_stats() const { print_stats_(capsule_); } 110 | 111 | inline void Capsule::reset(bool reset_qp_mem) { 112 | reset_(capsule_, (int)reset_qp_mem); 113 | } 114 | 115 | inline void Capsule::set_constraints_for_stage(uint stage, 116 | const std::string &field, 117 | const double *values) { 118 | // contents of value are always copied during the function call. ( 119 | // See https://discourse.acados.org/t/life-time-of-pointers/1795 120 | // So we can use const_cast. 121 | ocp_nlp_constraints_model_set(config_, dims_, in_, stage, field.c_str(), 122 | const_cast(values)); 123 | } 124 | 125 | inline void Capsule::set_output(uint stage, const std::string &field, 126 | const double *values) { 127 | ocp_nlp_out_set(config_, dims_, out_, stage, field.c_str(), 128 | const_cast(values)); 129 | } 130 | 131 | inline void Capsule::set_option(const std::string &field, const void *value) { 132 | 133 | ocp_nlp_solver_opts_set(config_, opts_, field.c_str(), 134 | const_cast(value)); 135 | } 136 | 137 | inline void Capsule::get_output(void *out, uint stage, 138 | const std::string &field) const { 139 | ocp_nlp_out_get(config_, dims_, out_, stage, field.c_str(), out); 140 | } 141 | 142 | namespace internal { 143 | 144 | /** 145 | * Compatibility helper for ocp_nlp_get from acados 0.3 to 0.4. 146 | */ 147 | template 148 | auto ocp_nlp_get_compat(Config config, Solver solver, const char *field, 149 | void *return_value) 150 | -> decltype(ocp_nlp_get(config, solver, field, return_value)) { 151 | ocp_nlp_get(config, solver, field, return_value); 152 | } 153 | 154 | template 155 | auto ocp_nlp_get_compat(Config config, Solver solver, const char *field, 156 | void *return_value) 157 | -> decltype(ocp_nlp_get(solver, field, return_value)) { 158 | ocp_nlp_get(solver, field, return_value); 159 | } 160 | 161 | } // namespace internal 162 | 163 | inline void Capsule::get(void *out, const std::string &field) const { 164 | 165 | internal::ocp_nlp_get_compat(config_, solver_, field.c_str(), out); 166 | } 167 | 168 | inline void Capsule::eval_cost() { ocp_nlp_eval_cost(solver_, in_, out_); } 169 | 170 | inline void Capsule::eval_residuals() { 171 | ocp_nlp_eval_residuals(solver_, in_, out_); 172 | } 173 | 174 | inline int Capsule::get_constraint_dims(uint stage, 175 | const std::string &field) const { 176 | return ocp_nlp_dims_get_from_attr(config_, dims_, out_, stage, field.c_str()); 177 | } 178 | 179 | inline void Capsule::set_solver_option(const std::string &field, 180 | const void *value) { 181 | ocp_nlp_solver_opts_set(config_, opts_, field.c_str(), 182 | const_cast(value)); 183 | } 184 | 185 | inline void Capsule::set_cost_model(uint stage, const std::string &field, 186 | const void *value) { 187 | ocp_nlp_cost_model_set(config_, dims_, in_, stage, field.c_str(), 188 | const_cast(value)); 189 | } 190 | 191 | } // namespace acados::solver 192 | 193 | #endif // _ACADOS_CPP_SOLVER_CAPSULE_IPP_ --------------------------------------------------------------------------------