This repo contains a C++ implementation of the quadratic-programming algorithm by Valmorbida and Hovd (2023). While this algorithm is geared toward solving the optimal-control problems that arise in MPC, it also works well for convex inequality-constrained QP problems.
The paper is really interesting and well worth a read. I've seen lots of approaches to solving QP problems, but none that strike me as similar to the one in this paper. When I first read it, my first reaction was along the lines of "How on earth does this solve a QP problem?" After thinking about it more my main question was more along the lines of "Why haven't anyone thought of this before?"
There's a lot I could do to improve this, including better error handling, using Eigen/Sparse, improving memory use, adding support for output constraints and constraining the state and input to general polytopes, and so forth. I might get around to doing some of that one day.
- C++20
- Bazel
- Eigen (managed through Bazel)
- GoogleTest (managed through Bazel)
Both of the examples below can be found in examples.cpp and run with bazel run --config=linux //:examples (or --config=macos).
int SolveClassicQpExample() {
// Solve a "classic" QP, of the form min 1/2 xᵀQx + xᵀc, s. t. Ax ≥ b.
// Example 16.4 in Nocedal & Wright (2006):
std::cout << "\nSolving a classic QP with two variables and five constraints." << std::endl;
const Eigen::MatrixXd Q = 2.0 * Eigen::MatrixXd::Identity(2, 2);
const Eigen::Vector2d c{
{-2.0, -5.0}
};
const Eigen::MatrixXd A{
{ 1.0, -2.0},
{-1.0, -2.0},
{-1.0, 2.0},
{ 1.0, 0.0},
{ 0.0, 1.0}
};
const Eigen::VectorXd b{
{-2.0, -6.0, -2.0, 0.0, 0.0}
};
if (!RampSolver::parametersAreValid(Q, c, A, b)) {
std::cerr << "Invalid problem." << std::endl;
return 1;
}
RampSolver solver(Q, c, A, b);
const Eigen::VectorXd y0 = solver.getColdStartY();
const ConstraintSet activeSet0;
const auto [y, activeSet, invQ, iter, success] = solver.solveImplicitEquation(y0, activeSet0);
if (!success) {
std::cerr << "Solver failure." << std::endl;
return 1;
}
Eigen::IOFormat vectorFmt(5, 0, ", ", "\n", "[", "]'");
std::cout << "Solution: " << solver.getSolution(y).transpose().format(vectorFmt) << std::endl;
std::cout << "The KKT conditions are "
<< (std::ranges::all_of(solver.kktConditionsAreSatisfied(y), std::identity{})
? ""
: "NOT ")
<< "satisfied." << std::endl;
std::cout << "Number of active constraints at the solution: " << activeSet.size() << std::endl;
std::cout << "Multiplier: " << RampSolver::r(y).transpose().format(vectorFmt) << std::endl;
return 0;
}The output should be
Solving a classic QP with two variables and five constraints.
Solution: [1.4, 1.7]'
The KKT conditions are satisfied.
Number of active constraints at the solution: 1
Multiplier: [0.8, 0, 0, 0, 0]'
The code below solves the initial optimal-control problem in Example 1 in the paper.
int SolveDoubleIntegratorExample() {
std::cout << "\nSolving an optimal-control problem, from Example 1 in the paper." << std::endl;
const Eigen::MatrixXd A{
{1.0, 1.0},
{0.0, 1.0}
};
const Eigen::MatrixXd B{{1.0}, {0.3}};
const Eigen::MatrixXd P = Eigen::MatrixXd::Identity(2, 2);
const Eigen::MatrixXd R = Eigen::MatrixXd::Identity(1, 1);
const Eigen::MatrixXd P_T = Eigen::MatrixXd::Identity(2, 2);
const RampSolver::Constraints constraints{
.xMin = -Eigen::VectorXd::Ones(2) * 5.0,
.xMax = Eigen::VectorXd::Ones(2) * 5.0,
.uMin = -Eigen::VectorXd::Ones(1),
.uMax = Eigen::VectorXd::Ones(1)};
const int N = 20; // 10 in the paper -- too short to get reasonably close to the origin.
const Eigen::VectorXd x_0{
{5.0, -2.0}
};
if (!RampSolver::parametersAreValid(A, B, P, R, P_T, constraints)) {
std::cerr << "Invalid problem." << std::endl;
return 1;
}
RampSolver solver(A, B, P, R, P_T, constraints, N);
solver.setInitialState(x_0);
const Eigen::VectorXd y0 = solver.getColdStartY(); // Requires a valid initial state
const ConstraintSet activeSet0;
const auto [y, activeSet, invQ, iter, success] = solver.solveImplicitEquation(y0, activeSet0);
if (!success) {
std::cerr << "Solver failure." << std::endl;
return 1;
}
Eigen::IOFormat vectorFmt(5, 0, ", ", "\n", "[", "]'");
// Extract the input and predicted state sequences U and X (vectors), X = Āx(k) + B̅U
const Eigen::VectorXd U = solver.getSolution(y);
const Eigen::VectorXd X = solver.getStateTrajectory(U);
std::cout << "Final control input, u(k + N - 1) = " << U.tail(B.cols()) << std::endl;
std::cout << "Final predicted state, x(k + N) = "
<< X.tail(A.cols()).transpose().format(vectorFmt) << std::endl;
std::cout << "The KKT conditions are "
<< (std::ranges::all_of(solver.kktConditionsAreSatisfied(y), std::identity{})
? ""
: "NOT ")
<< "satisfied." << std::endl;
std::cout << "Number of active constraints at the solution: " << activeSet.size() << std::endl;
std::cout << "Iterations: " << iter << std::endl;
const std::string filename = "example_1_solution.csv";
const std::optional<std::string> maybeFile =
util::writeSolutionToCsv(filename, x_0, X, U, N, A.cols(), B.cols());
if (maybeFile.has_value()) {
std::cout << "Solution written to " << maybeFile.value() << std::endl;
} else {
std::cerr << "Unable to write solution to file." << std::endl;
}
return 0;
}The output should be
Solving an optimal-control problem, from Example 1 in the paper.
Final control input, u(k + N - 1) = 0.00331883
Final predicted state, x(k + N) = [-0.0022083, -0.0037017]'
The KKT conditions are satisfied.
Number of active constraints at the solution: 5
Iterations: 5
Solution written to /path/to/example_1_solution.csv
and the state and input profiles look like this:
Valmorbida, G. and Hovd, M. Quadratic programming with ramp functions and fast online QP-MPC solutions. Automatica, 153:111011, 2023. https://doi.org/10.1016/j.automatica.2023.111011
Nocedal, J. and Wright, S. J. Numerical Optimization. Springer, second edition, 2006. https://doi.org/10.1007/978-0-387-40065-5
Bemporad, A., Morari, M., Dua, V., and Pistikopoulos, E. N. The explicit linear quadratic regulator for constrained systems. Automatica, 38(1):3–20, 2002. https://doi.org/10.1016/S0005-1098(01)00174-1