Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions interfaces/daqp-eigen/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -25,6 +26,7 @@ set(TARGETS
03_update
04_slack_sign
05_warmstart
06_general_hessian
)

foreach(TARGET ${TARGETS})
Expand Down
51 changes: 48 additions & 3 deletions interfaces/daqp-eigen/daqp.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_);
Expand Down Expand Up @@ -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_){
Expand Down Expand Up @@ -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_) {
Expand Down
3 changes: 3 additions & 0 deletions interfaces/daqp-eigen/daqp.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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();
Expand Down
43 changes: 43 additions & 0 deletions interfaces/daqp-eigen/tests/06_general_hessian.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,43 @@
#include <iostream>
#include <Eigen/Dense>
#include <daqp.hpp>

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<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> 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;
}
Loading