diff --git a/interfaces/daqp-eigen/CMakeLists.txt b/interfaces/daqp-eigen/CMakeLists.txt index db9e189..76344cb 100644 --- a/interfaces/daqp-eigen/CMakeLists.txt +++ b/interfaces/daqp-eigen/CMakeLists.txt @@ -19,6 +19,7 @@ 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) add_executable(07_symmetrize_hessian tests/07_symmetrize_hessian.cpp) +add_executable(08_unconstrained_check tests/08_unconstrained_check.cpp) set(TARGETS 00_basic_qp @@ -29,6 +30,7 @@ set(TARGETS 05_warmstart 06_general_hessian 07_symmetrize_hessian + 08_unconstrained_check ) foreach(TARGET ${TARGETS}) diff --git a/interfaces/daqp-eigen/tests/08_unconstrained_check.cpp b/interfaces/daqp-eigen/tests/08_unconstrained_check.cpp new file mode 100644 index 0000000..c762da0 --- /dev/null +++ b/interfaces/daqp-eigen/tests/08_unconstrained_check.cpp @@ -0,0 +1,130 @@ +#include +#include +#include + +// Test that the unconstrained-feasibility shortcut is correctly bypassed for +// LP objectives and for problems with equality constraints. + +int main() { + double precision = 1e-5; + bool all_pass = true; + + // ----------------------------------------------------------------------- + // Test 1: LP (no Hessian) where x_unc = -f satisfies the simple bounds + // but is NOT the LP optimal. + // + // min x1 + x2 (f = [1, 1], H = empty -> LP) + // s.t. -10 <= x1 <= 10 + // -10 <= x2 <= 10 + // + // Unconstrained "optimum": x_unc = -f = [-1, -1] (would be reported + // by the buggy shortcut as optimal, yielding fval = -2). + // True LP optimum: x* = [-10, -10], fval = -20. + // ----------------------------------------------------------------------- + { + int n = 2, m = 2; + Eigen::MatrixXd H(0, 0); // Empty Hessian signals an LP to DAQP (H_ptr == nullptr) + Eigen::VectorXd f = (Eigen::VectorXd(2) << 1, 1).finished(); + Eigen::Matrix A(0, n); // no general constraints + Eigen::VectorXd bu = (Eigen::VectorXd(2) << 10, 10).finished(); + Eigen::VectorXd bl = (Eigen::VectorXd(2) << -10, -10).finished(); + Eigen::VectorXi sense = Eigen::VectorXi::Zero(m); + Eigen::VectorXi break_points = Eigen::VectorXi::Zero(0); + + EigenDAQPResult result = daqp_solve(H, f, A, bu, bl, sense, break_points); + + Eigen::VectorXd expected = (Eigen::VectorXd(2) << -10, -10).finished(); + bool pass = (result.exitflag == DAQP_EXIT_OPTIMAL) && + result.get_primal().isApprox(expected, precision); + std::cout << "Test 1 (LP optimal, not unconstrained): " + << (pass ? "PASS" : "FAIL") << std::endl; + if (!pass) { + std::cout << " Expected: " << expected.transpose() << std::endl; + std::cout << " Got: " << result.get_primal().transpose() << std::endl; + std::cout << " exitflag: " << result.exitflag << std::endl; + } + all_pass = all_pass && pass; + } + + // ----------------------------------------------------------------------- + // Test 2: QP with a general equality constraint (sense = ACTIVE+IMMUTABLE) + // where the unconstrained optimum x_unc happens to satisfy the equality. + // The shortcut must not be taken; the solver must run and produce the + // correct primal AND report the equality as active. + // + // min 0.5*(x1^2+x2^2) - x1 + x2 (H=I, f=[-1,1]) + // s.t. x1 - x2 = 2 (general equality, sense = 5) + // + // x_unc = -f = [1, -1]; check: 1 - (-1) = 2 == rhs => equality satisfied. + // KKT: x + f + A'*lam = 0 => [x1-1; x2+1] + [1;-1]*lam = 0 + // x1 = 1-lam, x2 = -1+lam, x1-x2 = 2-2*lam = 2 => lam = 0. + // True optimum: x* = [1, -1], lam = 0. + // ----------------------------------------------------------------------- + { + int n = 2, m = 3; // 2 simple bounds + 1 general equality + Eigen::MatrixXd H = Eigen::MatrixXd::Identity(2, 2); + Eigen::VectorXd f = (Eigen::VectorXd(2) << -1, 1).finished(); + Eigen::Matrix A = + (Eigen::MatrixXd(1, 2) << 1, -1).finished(); + // [simple bound x1, simple bound x2, equality x1-x2] + Eigen::VectorXd bu = (Eigen::VectorXd(3) << 5, 5, 2).finished(); + Eigen::VectorXd bl = (Eigen::VectorXd(3) << -5, -5, 2).finished(); + // Equality constraint has sense = DAQP_ACTIVE (1) + DAQP_IMMUTABLE (4) = 5 + Eigen::VectorXi sense = (Eigen::VectorXi(3) << 0, 0, 5).finished(); + Eigen::VectorXi break_points = Eigen::VectorXi::Zero(0); + + EigenDAQPResult result = daqp_solve(H, f, A, bu, bl, sense, break_points); + + Eigen::VectorXd expected = (Eigen::VectorXd(2) << 1, -1).finished(); + bool pass = (result.exitflag == DAQP_EXIT_OPTIMAL) && + result.get_primal().isApprox(expected, precision); + std::cout << "Test 2 (QP with equality, x_unc on constraint): " + << (pass ? "PASS" : "FAIL") << std::endl; + if (!pass) { + std::cout << " Expected: " << expected.transpose() << std::endl; + std::cout << " Got: " << result.get_primal().transpose() << std::endl; + std::cout << " exitflag: " << result.exitflag << std::endl; + } + all_pass = all_pass && pass; + } + + // ----------------------------------------------------------------------- + // Test 3: QP where blower == bupper (unmarked equality, no explicit sense + // flag) and the unconstrained optimum satisfies it. The solver must + // still run normally and return the correct primal. + // + // min 0.5*(x1^2+x2^2) - x1 + x2 (H=I, f=[-1,1]) + // s.t. x1 - x2 = 2 (general equality via equal bounds) + // -5 <= x1 <= 5 (simple bounds) + // -5 <= x2 <= 5 + // + // Same as Test 2 but without the explicit IMMUTABLE sense flag. + // ----------------------------------------------------------------------- + { + int n = 2, m = 3; + Eigen::MatrixXd H = Eigen::MatrixXd::Identity(2, 2); + Eigen::VectorXd f = (Eigen::VectorXd(2) << -1, 1).finished(); + Eigen::Matrix A = + (Eigen::MatrixXd(1, 2) << 1, -1).finished(); + Eigen::VectorXd bu = (Eigen::VectorXd(3) << 5, 5, 2).finished(); + Eigen::VectorXd bl = (Eigen::VectorXd(3) << -5, -5, 2).finished(); // bl==bu for equality + Eigen::VectorXi sense = Eigen::VectorXi::Zero(3); // no explicit flags + Eigen::VectorXi break_points = Eigen::VectorXi::Zero(0); + + EigenDAQPResult result = daqp_solve(H, f, A, bu, bl, sense, break_points); + + Eigen::VectorXd expected = (Eigen::VectorXd(2) << 1, -1).finished(); + bool pass = (result.exitflag == DAQP_EXIT_OPTIMAL) && + result.get_primal().isApprox(expected, precision); + std::cout << "Test 3 (QP with unmarked equality via equal bounds): " + << (pass ? "PASS" : "FAIL") << std::endl; + if (!pass) { + std::cout << " Expected: " << expected.transpose() << std::endl; + std::cout << " Got: " << result.get_primal().transpose() << std::endl; + std::cout << " exitflag: " << result.exitflag << std::endl; + } + all_pass = all_pass && pass; + } + + return all_pass ? 0 : 1; +} diff --git a/src/utils.c b/src/utils.c index 7a3b515..4b6119b 100644 --- a/src/utils.c +++ b/src/utils.c @@ -29,6 +29,13 @@ int daqp_update_ldp(const int mask, DAQPWorkspace *work, DAQPProblem* qp){ } } + // Check bounds early + if(mask&DAQP_UPDATE_Rinv||mask&DAQP_UPDATE_M||mask&DAQP_UPDATE_v||mask&DAQP_UPDATE_d){ + error_flag = daqp_check_bounds(work,qp->bupper,qp->blower); + if(error_flag<0) return error_flag; + if(error_flag==1) do_activate = 1; + } + /** Update Rinv **/ if(mask&DAQP_UPDATE_Rinv){ if(work->avi == NULL) @@ -46,15 +53,27 @@ int daqp_update_ldp(const int mask, DAQPWorkspace *work, DAQPProblem* qp){ daqp_update_v(qp->f,work,mask); } - const int check_unconstrained = ((mask&DAQP_UPDATE_Rinv||mask&DAQP_UPDATE_M|| - mask&DAQP_UPDATE_v||mask&DAQP_UPDATE_d) && - work->bnb == NULL && work->nh <= 1 && work->avi == NULL) ? 1 : 0; + // The unconstrained shortcut is invalid when: + // - The problem is an LP (H == NULL, f != NULL): Rinv and RinvD are both NULL + // - Equality constraints are present + const int has_hessian = (work->Rinv != NULL || work->RinvD != NULL || work->v == NULL); + const int check_unconstrained_base = + (mask&DAQP_UPDATE_Rinv||mask&DAQP_UPDATE_M||mask&DAQP_UPDATE_v||mask&DAQP_UPDATE_d) && + work->bnb == NULL && work->nh <= 1 && work->avi == NULL; + int has_equalities = 0; + if(check_unconstrained_base && has_hessian){ + for(i = 0; i < work->m; i++) + if(work->sense[i]&(DAQP_ACTIVE + DAQP_IMMUTABLE)){ has_equalities = 1; break; } + } + + const int check_unconstrained = check_unconstrained_base && has_hessian && !has_equalities; /** Check if unconstrained optimum is primal feasible. * Compute x_unc = Rinv * (-v) (using the raw, un-normalized Rinv) and * verify dupper_unnorm = bupper - A*x_unc >= 0 and * dlower_unnorm = blower - A*x_unc <= 0 for every constraint. * If so, x_unc is optimal and we can skip the expensive M = A*Rinv step. - * Only applicable for standard QPs (no BnB, no hierarchy, no AVI, no prox). **/ + * Only applicable for standard QPs (no BnB, no hierarchy, no AVI, no prox, + * no equality constraints, and with a positive-definite Hessian). **/ if(check_unconstrained){ int j, disp; c_float sum; @@ -121,10 +140,6 @@ int daqp_update_ldp(const int mask, DAQPWorkspace *work, DAQPProblem* qp){ /** Update d **/ if(mask&DAQP_UPDATE_Rinv||mask&DAQP_UPDATE_M||mask&DAQP_UPDATE_v||mask&DAQP_UPDATE_d){ - error_flag = daqp_check_bounds(work,qp->bupper,qp->blower); - if(error_flag<0) return error_flag; - if(error_flag==1) do_activate = 1; - if(check_unconstrained){ // Already computed d, just need to normalize if(work->scaling != NULL){ for(i = 0; i < work->m; i++){