diff --git a/src/GenQPSolver.cpp b/src/GenQPSolver.cpp index 84f9d049..3ddf5aa7 100644 --- a/src/GenQPSolver.cpp +++ b/src/GenQPSolver.cpp @@ -56,7 +56,9 @@ void GenQPSolver::setDependencies(int nrVars, std::vector(nrVars), -1); reducedToFull_.resize(static_cast(nrVars) - dependencies_.size(), -1); /* Initialize the multipliers and offset variable */ - multipliers_ = Eigen::MatrixXd::Zero(nrVars, nrVars - static_cast(dependencies.size())); + multipliers_ = Eigen::SparseMatrix(nrVars, nrVars - static_cast(dependencies.size())); + std::vector> triplets; + triplets.reserve(static_cast(nrVars)); /* Number of removed variables encountered so far */ size_t shift = 0; for(size_t i = 0; i < fullToReduced_.size(); ++i) @@ -67,7 +69,7 @@ void GenQPSolver::setDependencies(int nrVars, std::vector(i - shift); - multipliers_(static_cast(i), fullToReduced_[i]) = 1.0; + triplets.push_back({static_cast::StorageIndex>(i), fullToReduced_[i], 1.0}); reducedToFull_[i - shift] = static_cast(i); } for(const auto & d : dependencies_) @@ -75,8 +77,9 @@ void GenQPSolver::setDependencies(int nrVars, std::vector(d); auto mimic_idx = std::get<1>(d); auto mult = std::get<2>(d); - multipliers_(mimic_idx, fullToReduced_[static_cast(leader_idx)]) = mult; + triplets.push_back({mimic_idx, fullToReduced_[static_cast(leader_idx)], mult}); } + multipliers_.setFromTriplets(triplets.begin(), triplets.end()); } } // namespace qp diff --git a/src/GenQPUtils.h b/src/GenQPUtils.h index 48119710..64a6fb22 100644 --- a/src/GenQPUtils.h +++ b/src/GenQPUtils.h @@ -10,6 +10,7 @@ // Eigen #include +#include // Tasks #include "Tasks/QPSolver.h" @@ -81,7 +82,7 @@ inline void reduceQC(const Eigen::MatrixXd & QFull, const Eigen::VectorXd & CFull, Eigen::MatrixXd & Q, Eigen::VectorXd & C, - const Eigen::MatrixXd & M) + const Eigen::SparseMatrix & M) { Q.noalias() = M.transpose() * QFull * M; C.noalias() = M.transpose() * CFull; @@ -296,7 +297,7 @@ inline void fillBound(const std::vector & bounds, Eigen::VectorXd & XL, * L \leq A M y \leq U * \f} */ -inline void reduceA(const Eigen::MatrixXd & AFull, Eigen::MatrixXd & A, const Eigen::MatrixXd & M) +inline void reduceA(const Eigen::MatrixXd & AFull, Eigen::MatrixXd & A, const Eigen::SparseMatrix & M) { A.noalias() = AFull * M; } @@ -345,7 +346,7 @@ inline void reduceBound(const Eigen::VectorXd & XLFull, */ inline void expandResult(const Eigen::VectorXd & result, Eigen::VectorXd & resultFull, - const Eigen::MatrixXd & multipliers) + const Eigen::SparseMatrix & multipliers) { resultFull.noalias() = multipliers * result; } diff --git a/src/Tasks/GenQPSolver.h b/src/Tasks/GenQPSolver.h index 63d76984..940a5eec 100644 --- a/src/Tasks/GenQPSolver.h +++ b/src/Tasks/GenQPSolver.h @@ -8,10 +8,12 @@ // std #include -// Eigen +// Tasks #include +// Eigen #include +#include // forward declaration // RBDyn @@ -116,7 +118,7 @@ class TASKS_DLLAPI GenQPSolver * the full variable and the factor and offset in the dependency equation: replica = factor * primary */ std::vector> dependencies_; /** Multipliers matrix M of size (nFull, nReduced) such that full = M * reduce */ - Eigen::MatrixXd multipliers_; + Eigen::SparseMatrix multipliers_; }; } // namespace qp