diff --git a/include/jrl-qp/GoldfarbIdnaniSolver.h b/include/jrl-qp/GoldfarbIdnaniSolver.h index 1ff0d38..fb3b3e4 100644 --- a/include/jrl-qp/GoldfarbIdnaniSolver.h +++ b/include/jrl-qp/GoldfarbIdnaniSolver.h @@ -74,10 +74,17 @@ class JRLQP_DLLAPI GoldfarbIdnaniSolver : public DualSolver void addInitialConstraint(const internal::SelectedConstraint & sc); + internal::TerminationType processMatrixG(); + internal::TerminationType processInitialActiveSetWithEqualityOnly(); + internal::TerminationType initializeComputationData(); + internal::TerminationType initializePrimalDualPoints(); + mutable internal::Workspace<> work_d_; internal::Workspace<> work_J_; internal::Workspace<> work_R_; internal::Workspace<> work_tmp_; + internal::Workspace<> work_hCoeffs_; // for initial QR decomposition + internal::Workspace<> work_bact_; Problem pb_; }; diff --git a/include/jrl-qp/SolverOptions.h b/include/jrl-qp/SolverOptions.h index 8563dd7..50dc396 100644 --- a/include/jrl-qp/SolverOptions.h +++ b/include/jrl-qp/SolverOptions.h @@ -29,6 +29,7 @@ struct JRLQP_DLLAPI SolverOptions int maxIter_ = 500; double bigBnd_ = 1e100; bool warmStart_ = false; + bool equalityFirst_ = false; //True if all equality constraints are given first in the constraint matrix GFactorization gFactorization_ = GFactorization::NONE; std::uint32_t logFlags_ = 0; std::ostream * logStream_ = &defaultStream_; @@ -90,6 +91,16 @@ struct JRLQP_DLLAPI SolverOptions return *this; } + bool equalityFirst() const + { + return equalityFirst_; + } + SolverOptions & equalityFirst(bool eqFirst) + { + equalityFirst_ = eqFirst; + return *this; + } + GFactorization gFactorization() const { return gFactorization_; diff --git a/src/GoldfarbIdnaniSolver.cpp b/src/GoldfarbIdnaniSolver.cpp index b768334..2e7eb6d 100644 --- a/src/GoldfarbIdnaniSolver.cpp +++ b/src/GoldfarbIdnaniSolver.cpp @@ -60,49 +60,23 @@ internal::InitTermination GoldfarbIdnaniSolver::init_() if(ret >= 0) return TerminationStatus::NON_POS_HESSIAN; - auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_); - - // J = L^-t - switch(options_.gFactorization_) + if (options_.equalityFirst()) { - case GFactorization::NONE: - [[fallthrough]]; - case GFactorization::L: - { - auto L = pb_.G.template triangularView(); - J.setIdentity(); - L.transpose().solveInPlace(J); - } - break; - case GFactorization::L_INV: - { - auto invL = pb_.G.template triangularView(); - J = invL.transpose(); - } - break; - case GFactorization::L_TINV: - { - auto invLT = pb_.G.template triangularView(); - J = invLT; - } - break; - default: - assert(false); + processInitialActiveSetWithEqualityOnly(); + initializeComputationData(); + initializePrimalDualPoints(); } + else + { + processMatrixG(); + initializePrimalDualPoints(); - // x = -G^-1 * a - auto x = work_x_.asVector(nbVar_); - auto tmp = work_tmp_.asVector(nbVar_); - tmp.noalias() = J.template triangularView().transpose() * pb_.a; - x.noalias() = J.template triangularView() * tmp; - x = -x; - f_ = 0.5 * pb_.a.dot(x); - - A_.reset(); - JRLQP_DEBUG_ONLY(work_R_.setZero()); + A_.reset(); + JRLQP_DEBUG_ONLY(work_R_.setZero()); - // Adding equality constraints - initActiveSet(); + // Adding equality constraints + initActiveSet(); + } return TerminationStatus::SUCCESS; } @@ -289,6 +263,8 @@ void GoldfarbIdnaniSolver::resize_(int nbVar, int /*nbCstr*/, bool /*useBounds*/ work_J_.resize(nbVar, nbVar); work_R_.resize(nbVar, nbVar); work_tmp_.resize(nbVar); + work_hCoeffs_.resize(nbVar); + work_bact_.resize(nbVar); } } @@ -363,4 +339,154 @@ void GoldfarbIdnaniSolver::addInitialConstraint(const internal::SelectedConstrai // return TerminationStatus::LINEAR_DEPENDENCY_DETECTED; } } + +internal::TerminationType GoldfarbIdnaniSolver::processMatrixG() +{ + auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_); + + // J = L^-t + switch(options_.gFactorization_) + { + case GFactorization::NONE: + [[fallthrough]]; + case GFactorization::L: + { + auto L = pb_.G.template triangularView(); + J.setIdentity(); + L.transpose().solveInPlace(J); + } + break; + case GFactorization::L_INV: + { + auto invL = pb_.G.template triangularView(); + J = invL.transpose(); + } + break; + case GFactorization::L_TINV: + { + auto invLT = pb_.G.template triangularView(); + J = invLT; + } + break; + default: + assert(false); + } + + return TerminationStatus::SUCCESS; +} + +internal::TerminationType GoldfarbIdnaniSolver::processInitialActiveSetWithEqualityOnly() +{ + A_.reset(); + + // This considers that all equality constraints come first + // We stop as soon as we have an inequality constraint. + for(int i = 0; i < A_.nbCstr(); ++i) + { + if(pb_.bl[i] == pb_.bu[i]) + { + A_.activate(i, ActivationStatus::EQUALITY); + } + else + break; + } + return TerminationStatus::SUCCESS; +} + +internal::TerminationType GoldfarbIdnaniSolver::initializeComputationData() +{ + int q = A_.nbActiveCstr(); + auto N = work_R_.asMatrix(nbVar_, q, nbVar_); + auto b_act = work_bact_.asVector(q); + for(int i = 0; i < q; ++i) + { + int cstrIdx = A_[i]; + switch(A_.activationStatus(cstrIdx)) + { + case ActivationStatus::LOWER: + [[fallthrough]]; + case ActivationStatus::EQUALITY: + N.col(i) = pb_.C.col(cstrIdx); + b_act[i] = pb_.bl(cstrIdx); + break; + case ActivationStatus::UPPER: + N.col(i) = -pb_.C.col(cstrIdx); + b_act[i] = -pb_.bu(cstrIdx); + break; + case ActivationStatus::LOWER_BOUND: + [[fallthrough]]; + case ActivationStatus::FIXED: + N.col(i).setZero(); + N.col(i)[cstrIdx - A_.nbCstr()] = 1; + b_act[i] = pb_.xl(cstrIdx - A_.nbCstr()); + break; + case ActivationStatus::UPPER_BOUND: + N.col(i).setZero(); + N.col(i)[cstrIdx - A_.nbCstr()] = -1; + b_act[i] = -pb_.xu(cstrIdx - A_.nbCstr()); + break; + default: + break; + } + } + + // J = L^-t + processMatrixG(); + + // B = L^-1 N + auto L = pb_.G.template triangularView(); + L.solveInPlace(N); // [OPTIM] There are other possible ways to do this: + // - use solveInPlace while filling N + // - multiply N by J^T = L^-1 (this would require that multiplication by triangular matrix can be + // done in place with Eigen) + + // QR in place + Eigen::Map hCoeffs(work_hCoeffs_.asVector(q).data(), + q); // Should be simply hCoeffs = work_hCoeffs_.asVector(q), but his does compile + // with householder_qr_inplace_blocked + WVector tmp = work_tmp_.asVector(nbVar_); + bool b = Eigen::internal::is_malloc_allowed(); + Eigen::internal::set_is_malloc_allowed(true); + Eigen::internal::householder_qr_inplace_blocked::run(N, hCoeffs, 48, tmp.data()); + + // J = J*Q + auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_); + Eigen::HouseholderSequence Q(N, hCoeffs); + Q.applyThisOnTheRight(J, tmp); + Eigen::internal::set_is_malloc_allowed(b); + + // Set lower part of R to 0 + JRLQP_DEBUG_ONLY(for(int i = 0; i < q; ++i) N.col(i).tail(nbVar_ - i - 1).setZero();); + + JRLQP_LOG(log_, LogFlags::INIT | LogFlags::NO_ITER, N, J, b_act); + + return TerminationStatus::SUCCESS; +} + +internal::TerminationType GoldfarbIdnaniSolver::initializePrimalDualPoints() +{ + int q = A_.nbActiveCstr(); + auto J = work_J_.asMatrix(nbVar_, nbVar_, nbVar_); + auto R = work_R_.asMatrix(q, q, nbVar_).template triangularView(); + WVector b_act = work_bact_.asVector(q); + WVector alpha = work_tmp_.asVector(nbVar_); + WVector beta = work_r_.asVector(q); + WVector x = work_x_.asVector(nbVar_); + WVector u = work_u_.asVector(q); + auto alpha1 = alpha.head(q); + auto alpha2 = alpha.tail(nbVar_ - q); + + alpha.noalias() = J.transpose() * pb_.a; + beta = R.transpose().solve(b_act); + x.noalias() = J.leftCols(q) * beta - J.rightCols(nbVar_ - q) * alpha2; + u = alpha1 + beta; + R.solveInPlace(u); + + f_ = beta.dot(0.5 * beta + alpha1) - 0.5 * alpha2.squaredNorm(); + + JRLQP_LOG(log_, LogFlags::INIT | LogFlags::NO_ITER, alpha, beta, x, u, f_); + + return TerminationStatus::SUCCESS; +} + } // namespace jrl::qp diff --git a/tests/OptionsTest.cpp b/tests/OptionsTest.cpp index 7e6b757..69f2524 100644 --- a/tests/OptionsTest.cpp +++ b/tests/OptionsTest.cpp @@ -48,4 +48,57 @@ TEST_CASE("Test FactorizedG") FAST_CHECK_UNARY(x2.isApprox(x0)); FAST_CHECK_UNARY(x3.isApprox(x0)); } +} + +TEST_CASE("Test EqualityFirst") +{ + jrl::qp::GoldfarbIdnaniSolver solver(7, 11, false); + jrl::qp::SolverOptions options; + + for(int i = 0; i < 10; ++i) + { + const int neq = 3; + auto pb = QPProblem(randomProblem(ProblemCharacteristics(7, 7, neq, 8))); + pb.C.transposeInPlace(); + + for (int i = 0; i < neq; ++i) + { + REQUIRE_EQ(pb.l[i], pb.u[i]); + } + + auto llt = pb.G.llt(); + Eigen::MatrixXd L = llt.matrixL(); + Eigen::MatrixXd invL = L.inverse(); + Eigen::MatrixXd invLT = invL.transpose(); + + options.gFactorization(jrl::qp::GFactorization::NONE); + solver.options(options); + solver.solve(pb.G, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu); + Eigen::VectorXd x0 = solver.solution(); + + options.equalityFirst(true); + solver.options(options); + solver.solve(pb.G, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu); + Eigen::VectorXd x1 = solver.solution(); + + options.gFactorization(jrl::qp::GFactorization::L); + solver.options(options); + solver.solve(L, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu); + Eigen::VectorXd x2 = solver.solution(); + + //options.gFactorization(jrl::qp::GFactorization::L_INV); + //solver.options(options); + //solver.solve(invL, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu); + //Eigen::VectorXd x3 = solver.solution(); + + //options.gFactorization(jrl::qp::GFactorization::L_TINV); + //solver.options(options); + //solver.solve(invLT, pb.a, pb.C, pb.l, pb.u, pb.xl, pb.xu); + //Eigen::VectorXd x4 = solver.solution(); + + FAST_CHECK_UNARY(x1.isApprox(x0)); + FAST_CHECK_UNARY(x2.isApprox(x0)); + //FAST_CHECK_UNARY(x3.isApprox(x0)); + //FAST_CHECK_UNARY(x4.isApprox(x0)); + } } \ No newline at end of file