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/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/daqp.cpp b/interfaces/daqp-eigen/daqp.cpp index 55ab12a..ee87376 100644 --- a/interfaces/daqp-eigen/daqp.cpp +++ b/interfaces/daqp-eigen/daqp.cpp @@ -15,12 +15,12 @@ EigenDAQPResult::EigenDAQPResult(int n, int m) } 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); } @@ -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 + + // 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}; + int status = update_ldp(update_mask, &work_, &qp_); is_solved_ = is_slack_computed_ = false; return status; @@ -250,6 +277,14 @@ EigenDAQPResult const& DAQP::solve(Eigen::Matrix +#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; +} 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 +}