diff --git a/interfaces/daqp-eigen/CMakeLists.txt b/interfaces/daqp-eigen/CMakeLists.txt index f7497aae..756b4713 100644 --- a/interfaces/daqp-eigen/CMakeLists.txt +++ b/interfaces/daqp-eigen/CMakeLists.txt @@ -17,6 +17,7 @@ add_executable(02_class tests/02_class.cpp) add_executable(03_update tests/03_update.cpp) add_executable(04_slack_sign tests/04_slack_sign.cpp) add_executable(05_warmstart tests/05_warmstart.cpp) +add_executable(06_general_hessian tests/06_general_hessian.cpp) set(TARGETS 00_basic_qp @@ -25,6 +26,7 @@ set(TARGETS 03_update 04_slack_sign 05_warmstart + 06_general_hessian ) foreach(TARGET ${TARGETS}) diff --git a/interfaces/daqp-eigen/daqp.cpp b/interfaces/daqp-eigen/daqp.cpp index 7d44368c..bd13a518 100644 --- a/interfaces/daqp-eigen/daqp.cpp +++ b/interfaces/daqp-eigen/daqp.cpp @@ -141,12 +141,25 @@ DAQP::DAQP(int max_variables, int max_constraints, int max_constraints_in_level) , max_constraints_{max_constraints} , max_constraints_in_level_{max_constraints_in_level} { allocate_daqp_workspace(&work_, max_variables, max_constraints_in_level); - allocate_daqp_ldp(&work_, max_variables, max_constraints, 0, 0, 0); + allocate_daqp_ldp(&work_, max_variables, max_constraints, 0, 1, 1); + // Save pre-allocated buffers but start in LDP (identity Hessian) mode, + // where Rinv must be NULL so the solver skips the Hessian back-transformation. + rinv_buf_ = work_.Rinv; + work_.Rinv = nullptr; + v_buf_ = work_.v; + work_.v = nullptr; daqp_default_settings(&settings_); work_.settings = &settings_; } DAQP::~DAQP() { + // Restore pre-allocated buffers so free_daqp_ldp can free them correctly. + // After a QP solve the buffer may be in RinvD (diagonal H) or Rinv (dense H); + // restore only when neither pointer holds the buffer (i.e. we are in LDP mode). + if (work_.Rinv == nullptr && work_.RinvD == nullptr) + work_.Rinv = rinv_buf_; + if (work_.v == nullptr) + work_.v = v_buf_; work_.settings = nullptr; free_daqp_workspace(&work_); free_daqp_ldp(&work_); @@ -217,8 +230,35 @@ int DAQP::update(Eigen::MatrixXd const& H, assert(resize_status == 0); if (update_mask < 0) { - // Assume that everythig should be updated - update_mask = DAQP_UPDATE_Rinv + DAQP_UPDATE_M + DAQP_UPDATE_v + DAQP_UPDATE_d + DAQP_UPDATE_sense + DAQP_UPDATE_hierarchy; + // Assume that everything should be updated, but only include Rinv/v + // updates when H/f are actually provided (avoids null dereference in + // daqp_update_Rinv when H is nullptr). + update_mask = DAQP_UPDATE_M + DAQP_UPDATE_d + DAQP_UPDATE_sense + DAQP_UPDATE_hierarchy; + if (H_ptr != nullptr) update_mask += DAQP_UPDATE_Rinv; + if (f_ptr != nullptr) update_mask += DAQP_UPDATE_v; + } + + // Manage the pre-allocated Rinv buffer: + // - When H is provided, make the buffer visible to daqp_update_Rinv. + // - When H is not provided, hide the buffer so the solver treats the + // Hessian as identity (Rinv == nullptr means R = I in daqp.c). + if (H_ptr != nullptr) { + if (work_.Rinv == nullptr && work_.RinvD == nullptr) + work_.Rinv = rinv_buf_; + } else { + if (work_.Rinv != nullptr) rinv_buf_ = work_.Rinv; + else if (work_.RinvD != nullptr) rinv_buf_ = work_.RinvD; + work_.Rinv = nullptr; + work_.RinvD = nullptr; + } + + // Manage the pre-allocated v buffer similarly. + if (f_ptr != nullptr) { + if (work_.v == nullptr) + work_.v = v_buf_; + } else { + if (work_.v != nullptr) v_buf_ = work_.v; + work_.v = nullptr; } if (warm_start_){ @@ -360,6 +400,11 @@ void DAQP::set_refactor_tol(double val) { is_solved_ = false; } +void DAQP::set_time_limit(double val) { + settings_.time_limit = val; + is_solved_ = false; +} + Eigen::VectorXd DAQP::get_primal() { if (!is_solved_) { diff --git a/interfaces/daqp-eigen/daqp.hpp b/interfaces/daqp-eigen/daqp.hpp index 61a1f7cd..9579eddb 100644 --- a/interfaces/daqp-eigen/daqp.hpp +++ b/interfaces/daqp-eigen/daqp.hpp @@ -67,6 +67,8 @@ class DAQP { EigenDAQPResult result_; DAQPProblem qp_; int resize_result(int n, int m, Eigen::VectorXi const& break_points); + c_float* rinv_buf_; + c_float* v_buf_; public: DAQP(int max_variables, int max_constraints, int max_constraints_in_level); @@ -103,6 +105,7 @@ class DAQP { void set_abs_subopt(double val); void set_sing_tol(double val); void set_refactor_tol(double val); + void set_time_limit(double val); // Getters for result Eigen::VectorXd get_primal(); diff --git a/interfaces/daqp-eigen/tests/06_general_hessian.cpp b/interfaces/daqp-eigen/tests/06_general_hessian.cpp new file mode 100644 index 00000000..10c634cb --- /dev/null +++ b/interfaces/daqp-eigen/tests/06_general_hessian.cpp @@ -0,0 +1,43 @@ +#include +#include +#include + +int main() { + double precision = 1e-5; + + // Solve: min_x 0.5*x'*H*x + f'*x + // s.t bl <= A*x <= bu + // with a non-identity Hessian using the workspace class + Eigen::MatrixXd H = (Eigen::MatrixXd(2, 2) << 2, 0, 0, 2).finished(); + Eigen::VectorXd f = (Eigen::VectorXd(2) << 1, 1).finished(); + + Eigen::Matrix A = + (Eigen::MatrixXd(2, 2) << 1, 2, 1, -1).finished(); + Eigen::VectorXd bu = (Eigen::VectorXd(4) << 1, 2, 3, 4).finished(); + Eigen::VectorXd bl = (Eigen::VectorXd(4) << -1, -2, -3, -4).finished(); + + Eigen::VectorXi sense = Eigen::VectorXi::Zero(4); + Eigen::VectorXi break_points = Eigen::VectorXi::Zero(0); + + // Use the workspace class (DAQP) with a general Hessian + DAQP solver(2, 10, 10); + solver.set_time_limit(10.0); + int status = solver.update(H, f, A, bu, bl, sense, break_points); + if (status < 0) { + std::cerr << "update() failed with status " << status << std::endl; + return 1; + } + solver.solve(); + + std::cout << "General-Hessian QP solution: \n"; + std::cout << solver.get_primal().transpose() << std::endl; + std::cout << "Status: " << solver.get_status() << std::endl; + std::cout << "Solve time: " << solver.get_solve_time() << " seconds" << std::endl; + + // Expected solution: same as the static daqp_solve result with this H and f + EigenDAQPResult ref = daqp_solve(H, f, A, bu, bl, sense, break_points); + std::cout << "Reference solution: \n"; + std::cout << ref.get_primal().transpose() << std::endl; + + return solver.get_primal().isApprox(ref.get_primal(), precision) ? 0 : 1; +}