Skip to content

Commit

Permalink
Adding equalityFirst option
Browse files Browse the repository at this point in the history
  • Loading branch information
@aescande
aescande committed Jun 13, 2024
1 parent f2bcd34 commit 6337651
Show file tree
Hide file tree
Showing 4 changed files with 236 additions and 39 deletions.
7 changes: 7 additions & 0 deletions include/jrl-qp/GoldfarbIdnaniSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
};

Expand Down
11 changes: 11 additions & 0 deletions include/jrl-qp/SolverOptions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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_;
Expand Down Expand Up @@ -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_;
Expand Down
204 changes: 165 additions & 39 deletions src/GoldfarbIdnaniSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Eigen::Lower>();
J.setIdentity();
L.transpose().solveInPlace(J);
}
break;
case GFactorization::L_INV:
{
auto invL = pb_.G.template triangularView<Eigen::Lower>();
J = invL.transpose();
}
break;
case GFactorization::L_TINV:
{
auto invLT = pb_.G.template triangularView<Eigen::Upper>();
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<Eigen::Upper>().transpose() * pb_.a;
x.noalias() = J.template triangularView<Eigen::Upper>() * 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;
}
Expand Down Expand Up @@ -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);
}
}

Expand Down Expand Up @@ -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<Eigen::Lower>();
J.setIdentity();
L.transpose().solveInPlace(J);
}
break;
case GFactorization::L_INV:
{
auto invL = pb_.G.template triangularView<Eigen::Lower>();
J = invL.transpose();
}
break;
case GFactorization::L_TINV:
{
auto invLT = pb_.G.template triangularView<Eigen::Upper>();
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<Eigen::Lower>();
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<Eigen::VectorXd> 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<decltype(N), decltype(hCoeffs)>::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<Eigen::Upper>();
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
53 changes: 53 additions & 0 deletions tests/OptionsTest.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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));
}
}

0 comments on commit 6337651

Please sign in to comment.