Skip to content

Draft of adding dense backend to proxqp #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
traversaro wants to merge 1 commit into
base: main
Choose a base branch
from
Draft

Draft of adding dense backend to proxqp #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
179 changes: 153 additions & 26 deletions plugins/proxqp/QpSolversEigenProxqp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@

// proxsuite
#include <proxsuite/proxqp/sparse/sparse.hpp>
#include <proxsuite/proxqp/dense/dense.hpp>

#include <memory>

Expand All @@ -18,8 +19,18 @@ class ProxqpSolver final: public SolverInterface
private:
// proxqp settings
proxsuite::proxqp::Settings<double> proxqpSettings;

// backend used by the solver (sparse or dense)
enum class Backend
{
Sparse,
Dense
} backend{Backend::Dense};

// proxqp sparse solver
std::unique_ptr<proxsuite::proxqp::sparse::QP<double, long long>> proxqpSparseSolver;
// proxqp dense solver
std::unique_ptr<proxsuite::proxqp::dense::QP<double>> proxqpDenseSolver;

struct InitialSparseSolverData {
int numberOfVariables = 0;
Expand All @@ -28,18 +39,21 @@ class ProxqpSolver final: public SolverInterface
// Hessian
bool isHessianSet = false;
Eigen::SparseMatrix<double> H;
Eigen::MatrixXd Hdense;

// Gradient
bool isGradientSet = false;
Eigen::VectorXd g;

// Equality constraints data (unused)
Eigen::SparseMatrix<double> A;
Eigen::MatrixXd Adense;
Eigen::VectorXd b;

// Inequality constraints data
bool isLinearConstraintsSet = false;
Eigen::SparseMatrix<double> C;
Eigen::MatrixXd Cdense;
bool isLowerBoundSet = false;
Eigen::VectorXd l;
bool isUpperBoundSet = false;
Expand Down Expand Up @@ -137,6 +151,11 @@ void ProxqpSolver::syncSettings()
{
proxqpSparseSolver->settings = proxqpSettings;
}

if (proxqpDenseSolver)
{
proxqpDenseSolver->settings = proxqpSettings;
}
}

std::string ProxqpSolver::getSolverName() const
Expand Down Expand Up @@ -164,23 +183,42 @@ bool ProxqpSolver::initSolver()

// See https://github.com/ami-iit/qpsolvers-eigen/issues/4
int numberOfEqualityConstraints = 0;
proxqpSparseSolver = std::make_unique<proxsuite::proxqp::sparse::QP<double, long long>>(initialSparseSolverData.numberOfVariables, numberOfEqualityConstraints, initialSparseSolverData.numberOfConstraints);
syncSettings();
proxqpSparseSolver->init(initialSparseSolverData.H, initialSparseSolverData.g,
initialSparseSolverData.A, initialSparseSolverData.b,
initialSparseSolverData.C, initialSparseSolverData.l, initialSparseSolverData.u);

if (backend == Backend::Sparse)
{
proxqpSparseSolver = std::make_unique<proxsuite::proxqp::sparse::QP<double, long long>>(initialSparseSolverData.numberOfVariables, numberOfEqualityConstraints, initialSparseSolverData.numberOfConstraints);
syncSettings();
proxqpSparseSolver->init(initialSparseSolverData.H, initialSparseSolverData.g,
initialSparseSolverData.A, initialSparseSolverData.b,
initialSparseSolverData.C, initialSparseSolverData.l, initialSparseSolverData.u);
} else if (backend == Backend::Dense)
{
proxqpDenseSolver = std::make_unique<proxsuite::proxqp::dense::QP<double>>(initialSparseSolverData.numberOfVariables, numberOfEqualityConstraints, initialSparseSolverData.numberOfConstraints);
syncSettings();
proxqpDenseSolver->init(initialSparseSolverData.Hdense, initialSparseSolverData.g,
initialSparseSolverData.Adense, initialSparseSolverData.b,
initialSparseSolverData.Cdense, initialSparseSolverData.l, initialSparseSolverData.u);
}

return isInitialized();
}

bool ProxqpSolver::isInitialized()
{
return static_cast<bool>(proxqpSparseSolver);
if (backend == Backend::Sparse)
{
return static_cast<bool>(proxqpSparseSolver);
} else if (backend == Backend::Dense)
{
return static_cast<bool>(proxqpDenseSolver);
}
return false;
}

void ProxqpSolver::clearSolver()
{
proxqpSparseSolver.reset();
proxqpDenseSolver.reset();
}

bool ProxqpSolver::clearSolverVariables()
Expand All @@ -191,42 +229,68 @@ bool ProxqpSolver::clearSolverVariables()

QpSolversEigen::ErrorExitFlag ProxqpSolver::solveProblem()
{
if (!proxqpSparseSolver)
if (!isInitialized())
{
QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::solveProblem: solver not initialized." << std::endl;
return QpSolversEigen::ErrorExitFlag::WorkspaceNotInitError;
}

proxqpSparseSolver->solve();
if (backend == Backend::Sparse)
{
proxqpSparseSolver->solve();
} else {
proxqpDenseSolver->solve();
}


return QpSolversEigen::ErrorExitFlag::NoError;
}

QpSolversEigen::Status ProxqpSolver::getStatus() const
{
if (!proxqpSparseSolver)
if (! (static_cast<bool>(proxqpSparseSolver) || static_cast<bool>(proxqpDenseSolver)))
{
QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::solveProblem: solver not initialized." << std::endl;
return QpSolversEigen::Status::SolverNotInitialized;
}

return this->convertStatus(proxqpSparseSolver->results.info.status);

if (backend == Backend::Sparse)
{
return this->convertStatus(proxqpSparseSolver->results.info.status);
} else {
return this->convertStatus(proxqpDenseSolver->results.info.status);
}
}

const double ProxqpSolver::getObjValue() const
{
return proxqpSparseSolver->results.info.objValue;
if (backend == Backend::Sparse)
{
return proxqpSparseSolver->results.info.objValue;
} else {
return proxqpDenseSolver->results.info.objValue;
}
}

const Eigen::Matrix<double, Eigen::Dynamic, 1>& ProxqpSolver::getSolution()
{
return proxqpSparseSolver->results.x;
if (backend == Backend::Sparse)
{
return proxqpSparseSolver->results.x;
} else {
return proxqpDenseSolver->results.x;
}
}

const Eigen::Matrix<double, Eigen::Dynamic, 1>& ProxqpSolver::getDualSolution()
{
return proxqpSparseSolver->results.z;
if (backend == Backend::Sparse)
{
return proxqpSparseSolver->results.z;
} else {
return proxqpDenseSolver->results.z;
}
}

bool ProxqpSolver::updateHessianMatrix(const Eigen::SparseMatrix<double> &hessianMatrix)
Expand All @@ -237,51 +301,103 @@ bool ProxqpSolver::updateHessianMatrix(const Eigen::SparseMatrix<double> &hessia
return false;
}

proxqpSparseSolver->update(initialSparseSolverData.H, proxsuite::nullopt,

if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(hessianMatrix, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, proxsuite::nullopt);
} else {
initialSparseSolverData.Hdense = hessianMatrix;
proxqpDenseSolver->update(initialSparseSolverData.Hdense, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, proxsuite::nullopt);
}


return true;
}

bool ProxqpSolver::updateLinearConstraintsMatrix(const Eigen::SparseMatrix<double> &linearConstraintsMatrix)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
if (!isInitialized())
{
debugStream() << "QpSolversEigen::ProxqpSolver::updateLinearConstraintsMatrix: solver was not initialized." << std::endl;
return false;
}

if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
linearConstraintsMatrix, proxsuite::nullopt, proxsuite::nullopt);
} else {
initialSparseSolverData.Cdense = linearConstraintsMatrix;
proxqpDenseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
linearConstraintsMatrix, proxsuite::nullopt, proxsuite::nullopt);
initialSparseSolverData.Cdense, proxsuite::nullopt, proxsuite::nullopt);
}
return true;
}

bool ProxqpSolver::updateGradient(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& gradient)
{
proxqpSparseSolver->update(proxsuite::nullopt, gradient,
if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(proxsuite::nullopt, gradient,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, proxsuite::nullopt);
} else {
proxqpDenseSolver->update(proxsuite::nullopt, gradient,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, proxsuite::nullopt);
}
return true;
}

bool ProxqpSolver::updateLowerBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, proxsuite::nullopt);
if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, proxsuite::nullopt);
} else {
proxqpDenseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, proxsuite::nullopt);
}
return true;
}

bool ProxqpSolver::updateUpperBound(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, upperBound);
} else {
proxqpDenseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt, upperBound);
}
return true;
}

bool ProxqpSolver::updateBounds(const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& lowerBound,
const Eigen::Ref<const Eigen::Matrix<double, Eigen::Dynamic, 1>>& upperBound)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, upperBound);
if (backend == Backend::Sparse)
{
proxqpSparseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, upperBound);
} else {
proxqpDenseSolver->update(proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, proxsuite::nullopt,
proxsuite::nullopt, lowerBound, upperBound);
}
return true;
}

Expand All @@ -299,7 +415,12 @@ void ProxqpSolver::clearLinearConstraintsMatrix()

bool ProxqpSolver::setHessianMatrix(const Eigen::SparseMatrix<double>& hessianMatrix)
{
initialSparseSolverData.H = hessianMatrix;
if (backend == Backend::Sparse)
{
initialSparseSolverData.H = hessianMatrix;
} else {
initialSparseSolverData.Hdense = hessianMatrix;
}
initialSparseSolverData.isHessianSet = true;
return true;
}
Expand All @@ -319,7 +440,13 @@ Eigen::Matrix<double, Eigen::Dynamic, 1> ProxqpSolver::getGradient()
bool
ProxqpSolver::setLinearConstraintsMatrix(const Eigen::SparseMatrix<double>& linearConstraintsMatrix)
{
initialSparseSolverData.C = linearConstraintsMatrix;
if (backend == Backend::Sparse)
{
initialSparseSolverData.C = linearConstraintsMatrix;
} else {
initialSparseSolverData.Cdense = linearConstraintsMatrix;
}

initialSparseSolverData.isLinearConstraintsSet = true;
return true;
}
Expand Down
6 changes: 6 additions & 0 deletions tests/MPCUpdateMatricesTest.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -301,19 +301,25 @@ TEST_CASE("MPCTest Update matrices")
setDynamicsMatrices(a, b, c, i * T);

// update the constraint bound
std::cerr << "=======> Update hessian " << std::endl;
REQUIRE(updateHessianMatrix(solver, Q, R, mpcWindow, i));
std::cerr << "=======> Update linear " << std::endl;
REQUIRE(updateLinearConstraintsMatrix(solver, mpcWindow, i));

castMPCToQPGradient(Q, yRef, mpcWindow, i, gradient);
std::cerr << "=======> Update gradient " << std::endl;
REQUIRE(solver.updateGradient(gradient));

updateConstraintVectors(x0, lowerBound, upperBound);
std::cerr << "=======> updateBoundsn " << std::endl;
REQUIRE(solver.updateBounds(lowerBound, upperBound));

// solve the QP problem
std::cerr << "=======> solveProblem " << std::endl;
REQUIRE(solver.solveProblem() == QpSolversEigen::ErrorExitFlag::NoError);

// get the controller input
std::cerr << "=======> get solution " << std::endl;
QPSolution = solver.getSolution();
ctr = QPSolution.block(2 * (mpcWindow + 1), 0, 1, 1);

Expand Down