Warm start in Eigen interface

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
 }

interfaces/daqp-eigen/CMakeLists.txt

@@ -16,13 +16,15 @@ 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
     01_static_func
     02_class
     03_update
     04_slack_sign
+    05_warmstart
 )
 
 foreach(TARGET ${TARGETS})

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<double, Eigen::Dynamic, Eigen::Dynamic,
 DAQP::DAQP(int max_variables, int max_constraints, int max_constraints_in_level)
   : is_solved_{false}
   , is_slack_computed_{false}
+  , warm_start_{false}
   , max_variables_{max_variables}
   , max_constraints_{max_constraints}
   , max_constraints_in_level_{max_constraints_in_level} {
@@ -215,13 +216,39 @@ int DAQP::update(Eigen::MatrixXd const& H,
     int resize_status = resize_result(n, m, break_points);
     assert(resize_status == 0);
 
-    qp_ = {n, m, ms, H_ptr, f_ptr, A_ptr, bu_ptr, bl_ptr, sense_ptr, bp_ptr, n_tasks};
-
     if (update_mask < 0) {
         // Assume that everythig should be updated
         update_mask = UPDATE_Rinv + UPDATE_M + UPDATE_v + UPDATE_d + UPDATE_sense + UPDATE_hierarchy;
     }
 
+    if (warm_start_){
+        if(sense_ptr == nullptr)
+            sense_ptr = work_.sense; // Directly work with sense of workspace
+        for(int i = 0; i < m; i++){
+            if(result_.lam[i] > 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<double, Eigen::Dynamic, Eigen::
 }
 
 
+void DAQP::set_warm_start() {
+    warm_start_ = true;
+}
+
+void DAQP::set_cold_start() {
+    warm_start_ = false;
+}
+
 void DAQP::set_primal_tol(double val) {
     settings_.primal_tol = val;
     is_solved_ = false;

interfaces/daqp-eigen/daqp.hpp

@@ -58,6 +58,7 @@ class DAQP {
   private:
     bool is_solved_;
     bool is_slack_computed_;
+    bool warm_start_;
     int max_variables_;
     int max_constraints_;
     int max_constraints_in_level_;
@@ -83,7 +84,10 @@ class DAQP {
                                  Eigen::VectorXd const& bu,
                                  Eigen::VectorXd const& bl,
                                  Eigen::VectorXi const& break_points);
+
     // Setters for settings
+    void set_warm_start();
+    void set_cold_start();
     void set_primal_tol(double val);
     void set_dual_tol(double val);
     void set_zero_tol(double val);

interfaces/daqp-eigen/tests/03_update.cpp

@@ -41,7 +41,8 @@ int main() {
 
     std::cout << "Solution 1: \n";
     std::cout << solver.get_primal().transpose() << std::endl;
-    std::cout << "Solve time: " << solver.get_solve_time() << " seconds" << 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;
     }

interfaces/daqp-eigen/tests/05_warmstart.cpp

@@ -0,0 +1,66 @@
+#include <iostream>
+#include <Eigen/Dense>
+#include <daqp.hpp>
+
+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;
+}

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<n;j++) Ax += A[disp++]*x[j];
+        if(Ax > bu[i]+tol || Ax < bl[i]-tol) return i;
+    }
+    return m; // No constraint is violating
+}