From 8297098b8dda64bb36275fd75b5d072fd68c90fc Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= Date: Sun, 9 Nov 2025 22:59:08 +0100 Subject: [PATCH 1/8] Warm start in Eigen interface --- interfaces/daqp-eigen/daqp.cpp | 32 ++++++++++++++++++++--- interfaces/daqp-eigen/daqp.hpp | 4 +++ interfaces/daqp-eigen/tests/03_update.cpp | 10 ++++++- 3 files changed, 42 insertions(+), 4 deletions(-) diff --git a/interfaces/daqp-eigen/daqp.cpp b/interfaces/daqp-eigen/daqp.cpp index 55ab12a..042a8f0 100644 --- a/interfaces/daqp-eigen/daqp.cpp +++ b/interfaces/daqp-eigen/daqp.cpp @@ -8,7 +8,7 @@ EigenDAQPResult::EigenDAQPResult() EigenDAQPResult::EigenDAQPResult(int n, int m) : x_{Eigen::VectorXd(n)} - , lam_{Eigen::VectorXd(m)} + , lam_{Eigen::VectorXd::Zero(m)} , slack_{Eigen::VectorXd(m)} { x = x_.data(); lam = lam_.data(); @@ -136,6 +136,7 @@ EigenDAQPResult daqp_solve(Eigen::Matrix 0) + sense_ptr[i] |= 1; // Active + Upper + else if(result_.lam[i] < 0) + sense_ptr[i] |= 3; // Active + Lower + else + sense_ptr[i] = 0; + } + update_mask &= ~UPDATE_sense; // Ensure sense is update + } + + qp_ = {n, m, ms, H_ptr, f_ptr, A_ptr, bu_ptr, bl_ptr, sense_ptr, bp_ptr, n_tasks}; + int status = update_ldp(update_mask, &work_, &qp_); is_solved_ = is_slack_computed_ = false; return status; @@ -230,6 +245,9 @@ int DAQP::update(Eigen::MatrixXd const& H, // Solve the LDP that is in the workspace EigenDAQPResult const& DAQP::solve() { if (!is_solved_) { + if(true){ + + } daqp_solve(&result_, &work_); is_solved_ = true; } @@ -250,6 +268,14 @@ EigenDAQPResult const& DAQP::solve(Eigen::Matrix Date: Mon, 10 Nov 2025 19:37:20 +0100 Subject: [PATCH 2/8] Add function to find first violating constraint --- include/api.h | 1 + src/api.c | 16 ++++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/include/api.h b/include/api.h index 81727b1..f20ad1b 100644 --- a/include/api.h +++ b/include/api.h @@ -42,6 +42,7 @@ void free_daqp_bnb(DAQPWorkspace* work); void daqp_extract_result(DAQPResult* res, DAQPWorkspace* work); void daqp_default_settings(DAQPSettings *settings); void daqp_minrep(int* is_redundant, c_float* A, c_float* b, int n, int m, int ms); +int daqp_first_violating(c_float* x, c_float* A, c_float* bu, c_float* bl, int n, int m, int ms, c_float tol); # ifdef __cplusplus } diff --git a/src/api.c b/src/api.c index 47d1962..d828cf0 100644 --- a/src/api.c +++ b/src/api.c @@ -423,3 +423,19 @@ void daqp_minrep(int* is_redundant, c_float* A, c_float* b, int n, int m, int ms free(work.dlower); free(work.sense); } + + +// Find which constraints the point x first violates +int daqp_first_violating(c_float* x, c_float* A, c_float* bu, c_float* bl, int n, int m, int ms, c_float tol){ + int i,j,disp; + c_float Ax; + for(i=0; i < ms; i++) + if(x[i] > bu[i]+tol || x[i] < bl[i]-tol) return i; + + for(disp=0; i < m; i++){ + Ax = 0; + for(j=0;j bu[i]+tol || Ax < bl[i]-tol) return i; + } + return m; // No constraint is violating +} From 6c562a7bb61f6f6ee87e81ef06c20828f46e0ada Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= Date: Mon, 10 Nov 2025 19:38:12 +0100 Subject: [PATCH 3/8] Move check if hierarchical mode inside activate_constraints --- src/auxiliary.c | 3 ++- src/utils.c | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/auxiliary.c b/src/auxiliary.c index 96e7125..acd450b 100644 --- a/src/auxiliary.c +++ b/src/auxiliary.c @@ -366,7 +366,8 @@ void pivot_last(DAQPWorkspace *work){ int activate_constraints(DAQPWorkspace *work){ //TODO prioritize inequalities? int i; - for(i =0;inh == 0 ? N_CONSTR : work->break_points[0]; + for(i =0;inh < 2){ + if(do_activate == 1){ reset_daqp_workspace(work); error_flag = activate_constraints(work); if(error_flag<0) From 95b7eb8e0df3302cc0284a9b01d0cbaae9a925d9 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= Date: Mon, 10 Nov 2025 19:39:05 +0100 Subject: [PATCH 4/8] Use previous primal solution to skip hierarchies --- interfaces/daqp-eigen/daqp.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/interfaces/daqp-eigen/daqp.cpp b/interfaces/daqp-eigen/daqp.cpp index 042a8f0..ee87376 100644 --- a/interfaces/daqp-eigen/daqp.cpp +++ b/interfaces/daqp-eigen/daqp.cpp @@ -8,19 +8,19 @@ EigenDAQPResult::EigenDAQPResult() EigenDAQPResult::EigenDAQPResult(int n, int m) : x_{Eigen::VectorXd(n)} - , lam_{Eigen::VectorXd::Zero(m)} + , lam_{Eigen::VectorXd(m)} , slack_{Eigen::VectorXd(m)} { x = x_.data(); lam = lam_.data(); } void EigenDAQPResult::resize_primal(int n) { - x_.resize(n); + x_.conservativeResizeLike(Eigen::VectorXd::Zero(n)); x = x_.data(); } void EigenDAQPResult::resize_dual(int m) { - lam_.resize(m); + lam_.conservativeResizeLike(Eigen::VectorXd::Zero(m)); lam = lam_.data(); slack_.resize(m); } @@ -232,7 +232,19 @@ int DAQP::update(Eigen::MatrixXd const& H, else sense_ptr[i] = 0; } - update_mask &= ~UPDATE_sense; // Ensure sense is update + update_mask |= UPDATE_sense; // Ensure sense is update + + // Adjust break points based on feasibility of previous solution + if(A_ptr != nullptr && n_tasks > 1){ + int first_violating = daqp_first_violating(result_.x,A_ptr,bu_ptr,bl_ptr,n,m,ms,settings_.primal_tol); + for(int i = 0; i < n_tasks; i++){ + if(bp_ptr[i] >= first_violating){ + n_tasks = n_tasks - i; + bp_ptr += i; + break; + } + } + } } qp_ = {n, m, ms, H_ptr, f_ptr, A_ptr, bu_ptr, bl_ptr, sense_ptr, bp_ptr, n_tasks}; @@ -245,9 +257,6 @@ int DAQP::update(Eigen::MatrixXd const& H, // Solve the LDP that is in the workspace EigenDAQPResult const& DAQP::solve() { if (!is_solved_) { - if(true){ - - } daqp_solve(&result_, &work_); is_solved_ = true; } From 6f84dc5bd5313f34d5da93756a1aa67d26c92ea7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= Date: Mon, 10 Nov 2025 19:41:23 +0100 Subject: [PATCH 5/8] Add test for warm start in Eigen interface --- interfaces/daqp-eigen/CMakeLists.txt | 2 + interfaces/daqp-eigen/tests/03_update.cpp | 7 --- interfaces/daqp-eigen/tests/05_warmstart.cpp | 66 ++++++++++++++++++++ 3 files changed, 68 insertions(+), 7 deletions(-) create mode 100644 interfaces/daqp-eigen/tests/05_warmstart.cpp diff --git a/interfaces/daqp-eigen/CMakeLists.txt b/interfaces/daqp-eigen/CMakeLists.txt index cc9690c..f7497aa 100644 --- a/interfaces/daqp-eigen/CMakeLists.txt +++ b/interfaces/daqp-eigen/CMakeLists.txt @@ -16,6 +16,7 @@ add_executable(01_static_func tests/01_static_func.cpp) 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) set(TARGETS 00_basic_qp @@ -23,6 +24,7 @@ set(TARGETS 02_class 03_update 04_slack_sign + 05_warmstart ) foreach(TARGET ${TARGETS}) diff --git a/interfaces/daqp-eigen/tests/03_update.cpp b/interfaces/daqp-eigen/tests/03_update.cpp index 6ecefda..5304871 100644 --- a/interfaces/daqp-eigen/tests/03_update.cpp +++ b/interfaces/daqp-eigen/tests/03_update.cpp @@ -46,13 +46,6 @@ int main() { if (!solver.get_primal().isApprox((Eigen::VectorXd(3) << 1, 0.5, -1).finished(),precision)) { return 1; } - solver.set_warm_start(); - solver.solve(A, bu, bl, break_points); - std::cout << "Solution 1 (warm start): \n"; - std::cout << solver.get_primal().transpose() << std::endl; - std::cout << "Solve time: " << solver.get_solve_time() << " seconds"; - std::cout << " | Iterations: " << solver.get_iterations() << std::endl; - solver.set_cold_start(); // ==== Second setup: remove some tasks ==== A = (Eigen::MatrixXd(4, 3) << A0, A3).finished(); diff --git a/interfaces/daqp-eigen/tests/05_warmstart.cpp b/interfaces/daqp-eigen/tests/05_warmstart.cpp new file mode 100644 index 0000000..b0de2f6 --- /dev/null +++ b/interfaces/daqp-eigen/tests/05_warmstart.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +int main() { + // + double precision = 1e-5; + Eigen::MatrixXd A; + Eigen::VectorXd bu, bl; + Eigen::VectorXi break_points; + + // Setup solver + DAQP solver(3, 50, 5); + solver.set_warm_start(); + + // Task 1: -1 <= x <= 1 + Eigen::MatrixXd A0 = Eigen::MatrixXd::Identity(3, 3); + Eigen::VectorXd bu0 = Eigen::VectorXd::Ones(3); + Eigen::VectorXd bl0 = -Eigen::VectorXd::Ones(3); + + // Task 2: x1+x2+x3 <= 1 + Eigen::MatrixXd A1 = (Eigen::MatrixXd(1, 3) << 1, 1, 1).finished(); + Eigen::VectorXd bu1 = Eigen::VectorXd::Ones(1); + Eigen::VectorXd bl1 = Eigen::VectorXd::Constant(1, -DAQP_INF); + + // Task 3: x1 - x2 == 0.5 + Eigen::MatrixXd A2 = (Eigen::MatrixXd(1, 3) << 1, -1, 0).finished(); + Eigen::VectorXd bu2 = 0.5 * Eigen::VectorXd::Ones(1); + Eigen::VectorXd bl2 = 0.5 * Eigen::VectorXd::Ones(1); + + // Task 4: 10 <= 3*x1+x2-x3 <= 20 + Eigen::MatrixXd A3 = (Eigen::MatrixXd(1, 3) << 3, 1, -1).finished(); + Eigen::VectorXd bu3 = 20 * Eigen::VectorXd::Ones(1); + Eigen::VectorXd bl3 = 10 * Eigen::VectorXd::Ones(1); + + // ==== First setup (stack all tasks) ==== + A = (Eigen::MatrixXd(6, 3) << A0, A1, A2, A3).finished(); + bu = (Eigen::VectorXd(6) << bu0, bu1, bu2, bu3).finished(); + bl = (Eigen::VectorXd(6) << bl0, bl1, bl2, bl3).finished(); + break_points = (Eigen::VectorXi(4) << 3, 4, 5, 6).finished(); + solver.solve(A, bu, bl, break_points); + int cold_iter = solver.get_iterations(); + + std::cout << "Solution cold start: \n"; + std::cout << solver.get_primal().transpose() << std::endl; + std::cout << "Solve time: " << solver.get_solve_time() << " seconds"; + std::cout << " | Iterations: " << solver.get_iterations() << std::endl; + if (!solver.get_primal().isApprox((Eigen::VectorXd(3) << 1, 0.5, -1).finished(),precision)) { + return 1; + } + solver.set_warm_start(); + solver.solve(A, bu, bl, break_points); + std::cout << "Solution warm start: \n"; + std::cout << solver.get_primal().transpose() << std::endl; + std::cout << "Solve time: " << solver.get_solve_time() << " seconds"; + std::cout << " | Iterations: " << solver.get_iterations() << std::endl; + if (!solver.get_primal().isApprox((Eigen::VectorXd(3) << 1, 0.5, -1).finished(),precision)) { + return 1; + } + + // Ensure cold start reduced number of iterations + if(cold_iter <= solver.get_iterations()) return 1; + + + return 0; +} From 6eccbc5d5a71dc6487126e38ab50280f1b6c306b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= <55484604+darnstrom@users.noreply.github.com> Date: Thu, 13 Nov 2025 22:39:48 +0100 Subject: [PATCH 6/8] Improve numerics in hdaqp for weakly active constraints (#96) * Improve numerics in hdaqp for weakly active constraints. * Ensure constraints that cannot be activated are are mutable --- src/hierarchical.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/hierarchical.c b/src/hierarchical.c index c011c2f..9d6bdb0 100644 --- a/src/hierarchical.c +++ b/src/hierarchical.c @@ -52,9 +52,9 @@ int daqp_hiqp(DAQPWorkspace *work, c_float *lambda){ id=work->WS[j]; if(IS_SOFT(id)){ w = work->lam_star[j]*work->settings->rho_soft; - if(IS_LOWER(id)) + if(w < -work->settings->primal_tol) work->dlower[id]+=w; - else + else if(w > work->settings->primal_tol) work->dupper[id]+=w; if(lambda != NULL){ w += IS_LOWER(id) ? -1e-14 : 1e-14; // For weakly active @@ -73,7 +73,9 @@ int daqp_hiqp(DAQPWorkspace *work, c_float *lambda){ // reactive constraint in level current (to addresss soft->hard) // TODO: can factorization be directly reused? int n_active_old = (work->n_active < work->n) ? work->n_active : work->n; - for(int jj=n_active_old; jj < work->n_active ;jj++) SET_INACTIVE(work->WS[jj]); + for(int jj=n_active_old; jj < work->n_active ;jj++) { + work->sense[work->WS[jj]]&=~(ACTIVE+IMMUTABLE); // ensure inactive + mutable + } work->n_active =j; work->reuse_ind=j; work->sing_ind = EMPTY_IND; @@ -83,6 +85,7 @@ int daqp_hiqp(DAQPWorkspace *work, c_float *lambda){ if(work->sing_ind != EMPTY_IND){ remove_constraint(work,j); work->sing_ind = EMPTY_IND; + SET_MUTABLE(work->WS[j]); } else{ if(IS_IMMUTABLE(work->WS[j])) nfree--; From 3003e3bf98b048d29c050125fbfcb6d4962f35cd Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Daniel=20Arnstr=C3=B6m?= Date: Sun, 7 Dec 2025 14:48:47 +0100 Subject: [PATCH 7/8] Revert activate_constraint --- src/auxiliary.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/auxiliary.c b/src/auxiliary.c index 22c5716..e282acf 100644 --- a/src/auxiliary.c +++ b/src/auxiliary.c @@ -365,8 +365,7 @@ void pivot_last(DAQPWorkspace *work){ int activate_constraints(DAQPWorkspace *work){ //TODO prioritize inequalities? int i; - const int end = work->nh == 0 ? N_CONSTR : work->break_points[0]; - for(i =0;i Date: Sun, 7 Dec 2025 18:22:34 +0100 Subject: [PATCH 8/8] Fix indentation --- src/auxiliary.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/auxiliary.c b/src/auxiliary.c index e282acf..990cd9d 100644 --- a/src/auxiliary.c +++ b/src/auxiliary.c @@ -365,7 +365,7 @@ void pivot_last(DAQPWorkspace *work){ int activate_constraints(DAQPWorkspace *work){ //TODO prioritize inequalities? int i; - for(i =0;i