Verified Commit ccb3f9ed authored by Justin Carpentier's avatar Justin Carpentier
Browse files

core: align signature and expose dual variables

parent 3ba84844
Pipeline #17925 failed with stage
in 7 minutes and 24 seconds
//
// Copyright (c) 2019 CNRS
// Copyright (c) 2019,2022 CNRS INRIA
//
// This file is part of eiquadprog.
//
......@@ -83,6 +83,7 @@
#include <iostream>
#include "eiquadprog/eiquadprog-utils.hxx"
#include "eiquadprog/deprecated.hpp"
// namespace internal {
namespace eiquadprog {
......@@ -97,7 +98,7 @@ inline void update_z(Eigen::VectorXd& z, const Eigen::MatrixXd& J, const Eigen::
}
inline void update_r(const Eigen::MatrixXd& R, Eigen::VectorXd& r, const Eigen::VectorXd& d, size_t iq) {
r.head(iq) = d.head(iq)
r.head(iq) = d.head(iq);
R.topLeftCorner(iq, iq).triangularView<Eigen::Upper>().solveInPlace(r.head(iq));
}
......@@ -105,16 +106,33 @@ bool add_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXd& d,
void delete_constraint(Eigen::MatrixXd& R, Eigen::MatrixXd& J, Eigen::VectorXi& A, Eigen::VectorXd& u, size_t p,
size_t& iq, size_t l);
/* solve_quadprog2 is used when the Cholesky decomposition of the G
matrix is precomputed */
double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
double solve_quadprog(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
Eigen::VectorXd& x, Eigen::VectorXd& y, Eigen::VectorXi& A, size_t& q);
EIQUADPROG_DEPRECATED
double solve_quadprog2(Eigen::LLT<Eigen::MatrixXd, Eigen::Lower>& chol, double c1, Eigen::VectorXd& g0,
const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0, const Eigen::MatrixXd& CI,
const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q);
const Eigen::VectorXd& ci0, Eigen::VectorXd& x, Eigen::VectorXi& A, size_t& q)
{
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, A, q);
}
/* solve_quadprog is used for on-demand QP solving */
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0, Eigen::VectorXd& x,
Eigen::VectorXi& activeSet, size_t& activeSetSize);
double solve_quadprog(Eigen::MatrixXd& G, Eigen::VectorXd& g0, const Eigen::MatrixXd& CE, const Eigen::VectorXd& ce0,
const Eigen::MatrixXd& CI, const Eigen::VectorXd& ci0,
Eigen::VectorXd& x, Eigen::VectorXd& y,
Eigen::VectorXi& activeSet, size_t& activeSetSize);
// }
} // namespace solvers
......
......@@ -5,8 +5,20 @@ namespace solvers {
using namespace Eigen;
/* solve_quadprog is used for on-demand QP solving */
double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI,
const VectorXd& ci0, VectorXd& x, VectorXi& activeSet, size_t& activeSetSize) {
Eigen::DenseIndex p = CE.cols();
Eigen::DenseIndex m = CI.cols();
VectorXd y(p + m);
return solve_quadprog(G, g0, CE, ce0, CI, ci0, x, y, activeSet, activeSetSize);
}
double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI,
const VectorXd& ci0, VectorXd& x, VectorXd& y, VectorXi& activeSet, size_t& activeSetSize) {
LLT<MatrixXd, Lower> chol(G.cols());
double c1;
/* compute the trace of the original matrix G */
......@@ -15,15 +27,26 @@ double solve_quadprog(MatrixXd& G, VectorXd& g0, const MatrixXd& CE, const Vecto
/* decompose the matrix G in the form LL^T */
chol.compute(G);
return solve_quadprog2(chol, c1, g0, CE, ce0, CI, ci0, x, activeSet, activeSetSize);
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet, activeSetSize);
}
double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0, const MatrixXd& CI,
const VectorXd& ci0, VectorXd& x, VectorXi& activeSet, size_t& activeSetSize) {
Eigen::DenseIndex p = CE.cols();
Eigen::DenseIndex m = CI.cols();
VectorXd y(p + m);
return solve_quadprog(chol, c1, g0, CE, ce0, CI, ci0, x, y, activeSet, activeSetSize);
}
/* solve_quadprog2 is used for when the Cholesky decomposition of G is pre-computed
* @param A Output vector containing the indexes of the active constraints.
* @param q Output value representing the size of the active set.
*/
double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXi& A, size_t& q) {
double solve_quadprog(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, const MatrixXd& CE, const VectorXd& ce0,
const MatrixXd& CI, const VectorXd& ci0, VectorXd& x, VectorXd& u, VectorXi& A, size_t& q) {
size_t i, k, l; /* indices */
size_t ip, me, mi;
size_t n = g0.size();
......@@ -31,7 +54,7 @@ double solve_quadprog2(LLT<MatrixXd, Lower>& chol, double c1, VectorXd& g0, cons
size_t m = CI.cols();
MatrixXd R(g0.size(), g0.size()), J(g0.size(), g0.size());
VectorXd s(m + p), z(n), r(m + p), d(n), np(n), u(m + p);
VectorXd s(m + p), z(n), r(m + p), d(n), np(n);
VectorXd x_old(n), u_old(m + p);
double f_value, psi, c2, sum, ss, R_norm;
const double inf = std::numeric_limits<double>::infinity();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment