Skip to content

Commit

Permalink
Fix mimic substitution issue
Browse files Browse the repository at this point in the history 
This commit greatly simplifies the handling of mimic joints'
substitution in the problem with a more straight forward approach

Closes #57
  • Loading branch information
@gergondet
gergondet committed May 31, 2022
1 parent 6e01efa commit f33798e
Show file tree
Hide file tree
Showing 6 changed files with 75 additions and 94 deletions.
31 changes: 18 additions & 13 deletions src/GenQPSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,31 +47,36 @@ GenQPSolver * createQPSolver(const std::string & name)

void GenQPSolver::setDependencies(int nrVars, std::vector<std::tuple<int, int, double>> dependencies)
{
assert(static_cast<size_t>(nrVars) > dependencies.size());
dependencies_ = dependencies;
fullToReduced_.resize(nrVars, -1);
reducedToFull_.resize(nrVars - static_cast<int>(dependencies_.size()), -1);
/* Retrieve the variables which are removed due to the dependencies */
std::vector<int> removedVars;
removedVars.reserve(dependencies.size() + 1);
for(const auto & d : dependencies)
{
removedVars.push_back(std::get<1>(d));
}
/* Prevent issue once we have gone past the last removed variable */
removedVars.push_back(nrVars);
std::sort(removedVars.begin(), removedVars.end());
using dependency_t = std::tuple<int, int, double>;
/* Sort dependencies by the index of removed variables */
std::sort(dependencies_.begin(), dependencies_.end(),
[](const dependency_t & lhs, const dependency_t & rhs) { return std::get<1>(lhs) < std::get<1>(rhs); });
fullToReduced_.resize(static_cast<size_t>(nrVars), -1);
reducedToFull_.resize(static_cast<size_t>(nrVars) - dependencies_.size(), -1);
/* Initialize the multipliers and offset variable */
multipliers_ = Eigen::MatrixXd::Zero(nrVars, nrVars - static_cast<int>(dependencies.size()));
/* Number of removed variables encountered so far */
size_t shift = 0;
for(size_t i = 0; i < fullToReduced_.size(); ++i)
{
if(i == static_cast<size_t>(removedVars[shift]))
if(shift < dependencies_.size() && std::get<1>(dependencies_[shift]) == static_cast<int>(i))
{
shift++;
continue;
}
fullToReduced_[i] = static_cast<int>(i - shift);
multipliers_(static_cast<Eigen::DenseIndex>(i), fullToReduced_[i]) = 1.0;
reducedToFull_[i - shift] = static_cast<int>(i);
}
for(const auto & d : dependencies_)
{
auto leader_idx = std::get<0>(d);
auto mimic_idx = std::get<1>(d);
auto mult = std::get<2>(d);
multipliers_(mimic_idx, fullToReduced_[static_cast<size_t>(leader_idx)]) = mult;
}
}

} // namespace qp
Expand Down
108 changes: 42 additions & 66 deletions src/GenQPUtils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,44 +59,32 @@ inline void fillQC(const std::vector<Task *> & tasks, int nrVars, Eigen::MatrixX
}

/**
* Reduce \f$ Q \f$ matrix and the \f$ c \f$ vector based on the dependencies list
* Reduce \f$ Q \f$ matrix and the \f$ c \f$ vector using the multiplier matrix
*
* Indeed, solving:
* \f{align}
* \underset{x}{\text{minimize }} & \frac{1}{2} x^T Q x + x^T c\\
* \f}
*
* Where:
* \f{align}
* x = M y
* \f}
*
* Is equivalent to solving:
* \f{align}
* \underset{x}{\text{minimize }} & \frac{1}{2} y^T M ^T Q M y + y^T M^T c\\
* \f}
*
*/
inline void reduceQC(const Eigen::MatrixXd & QFull,
const Eigen::VectorXd & CFull,
Eigen::MatrixXd & Q,
Eigen::VectorXd & C,
const std::vector<int> & fullToReduced,
const std::vector<int> & reducedToFull,
const std::vector<std::tuple<int, int, double>> & dependencies)
const Eigen::MatrixXd & M)
{
/* Start by moving the non-reduced variables to their new location */
for(size_t i = 0; i < reducedToFull.size(); ++i)
{
for(size_t j = i; j < reducedToFull.size(); ++j)
{
Q(i, j) = QFull(reducedToFull[i], reducedToFull[j]);
}
C(i) = CFull(reducedToFull[i]);
}
/* Apply the reduction */
for(const auto & d : dependencies)
{
const int & primaryFullI = std::get<0>(d);
const int & primaryReducedI = fullToReduced[primaryFullI];
const int & replicaFullI = std::get<1>(d);
const double & alpha = std::get<2>(d);
/* Apply reduction to C */
C(primaryReducedI) += alpha * CFull(replicaFullI);
/* Add cross-terms to Q */
for(size_t i = 0; i < reducedToFull.size(); ++i)
{
Q(i, primaryReducedI) += alpha * QFull(reducedToFull[i], replicaFullI);
}
/* Add diagonal element to Q */
Q(primaryReducedI, primaryReducedI) += alpha * alpha * QFull(replicaFullI, replicaFullI);
}
/* Update the lower triangular part of Q */
Q = Q.selfadjointView<Eigen::Upper>();
Q.noalias() = M.transpose() * QFull * M;
C.noalias() = M.transpose() * CFull;
}

// general qp form
Expand Down Expand Up @@ -292,26 +280,25 @@ inline void fillBound(const std::vector<Bound *> & bounds, Eigen::VectorXd & XL,
}

/**
* Reduce \f$ A \f$ matrix based on the dependencies list
* Reduce \f$ A \f$ matrix based on the multiplier and offset
*
* In this form, all non bounds constraints are represented as:
* \f{align}
* A x \eq b
* A x \leq b
* L \leq A x \leq U
* \f}
*
* Which we can rewrite as:
* \f{align}
* A M y \eq b
* A M y \leq b
* L \leq A M y \leq U
* \f}
*/
inline void reduceA(const Eigen::MatrixXd & AFull,
Eigen::MatrixXd & A,
const std::vector<int> & fullToReduced,
const std::vector<int> & reducedToFull,
const std::vector<std::tuple<int, int, double>> & dependencies)
inline void reduceA(const Eigen::MatrixXd & AFull, Eigen::MatrixXd & A, const Eigen::MatrixXd & M)
{
for(size_t i = 0; i < reducedToFull.size(); ++i)
{
A.col(i) = AFull.col(reducedToFull[i]);
}
for(const auto & d : dependencies)
{
const int & primaryFullI = std::get<0>(d);
const int & primaryReducedI = fullToReduced[primaryFullI];
const int & replicaFullI = std::get<1>(d);
const double & alpha = std::get<2>(d);
A.col(primaryReducedI) += alpha * AFull.col(replicaFullI);
}
A.noalias() = AFull * M;
}

/**
Expand All @@ -327,13 +314,13 @@ inline void reduceBound(const Eigen::VectorXd & XLFull,
{
for(size_t i = 0; i < static_cast<size_t>(XL.rows()); ++i)
{
XL(i) = XLFull(reducedToFull[i]);
XU(i) = XUFull(reducedToFull[i]);
XL(static_cast<Eigen::DenseIndex>(i)) = XLFull(reducedToFull[i]);
XU(static_cast<Eigen::DenseIndex>(i)) = XUFull(reducedToFull[i]);
}
for(const auto & d : dependencies)
{
const int & primaryFullI = std::get<0>(d);
const int & primaryReducedI = fullToReduced[primaryFullI];
const int & primaryReducedI = fullToReduced[static_cast<size_t>(primaryFullI)];
const int & replicaFullI = std::get<1>(d);
const double & alpha = std::get<2>(d);
if(alpha != 0)
Expand All @@ -358,20 +345,9 @@ inline void reduceBound(const Eigen::VectorXd & XLFull,
*/
inline void expandResult(const Eigen::VectorXd & result,
Eigen::VectorXd & resultFull,
const std::vector<int> & reducedToFull,
const std::vector<std::tuple<int, int, double>> & dependencies)
const Eigen::MatrixXd & multipliers)
{
for(size_t i = 0; i < reducedToFull.size(); ++i)
{
resultFull(reducedToFull[i]) = result(i);
}
for(const auto & d : dependencies)
{
const int & primaryFullI = std::get<0>(d);
const int & replicaFullI = std::get<1>(d);
const double & alpha = std::get<2>(d);
resultFull(replicaFullI) = alpha * resultFull(primaryFullI);
}
resultFull.noalias() = multipliers * result;
}

// print of a constraint at a given line
Expand Down
6 changes: 3 additions & 3 deletions src/LSSOLQPSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -87,9 +87,9 @@ void LSSOLQPSolver::updateMatrix(const std::vector<Task *> & tasks,

if(dependencies_.size())
{
reduceA(AFull_, A_, fullToReduced_, reducedToFull_, dependencies_);
reduceA(AFull_, A_, multipliers_);
reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
reduceQC(QFull_, CFull_, Q_, C_, multipliers_);
}
}

Expand All @@ -100,7 +100,7 @@ bool LSSOLQPSolver::solve()
{
success = lssol_.solve(XL_, XU_, static_cast<Eigen::LSSOLBase::RefMat>(Q_), C_,
A_.block(0, 0, nrALines_, A_.cols()), AL_.segment(0, nrALines_), AU_.segment(0, nrALines_));
expandResult(lssol_.result(), XFull_, reducedToFull_, dependencies_);
expandResult(lssol_.result(), XFull_, multipliers_);
}
else
{
Expand Down
8 changes: 4 additions & 4 deletions src/QLDQPSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -95,10 +95,10 @@ void QLDQPSolver::updateMatrix(const std::vector<Task *> & tasks,
XU_.fill(std::numeric_limits<double>::infinity());
Q_.setZero();
C_.setZero();
reduceA(AeqFull_, Aeq_, fullToReduced_, reducedToFull_, dependencies_);
reduceA(AineqFull_, Aineq_, fullToReduced_, reducedToFull_, dependencies_);
reduceA(AeqFull_, Aeq_, multipliers_);
reduceA(AineqFull_, Aineq_, multipliers_);
reduceBound(XLFull_, XL_, XUFull_, XU_, fullToReduced_, reducedToFull_, dependencies_);
reduceQC(QFull_, CFull_, Q_, C_, fullToReduced_, reducedToFull_, dependencies_);
reduceQC(QFull_, CFull_, Q_, C_, multipliers_);
}
}

Expand All @@ -110,7 +110,7 @@ bool QLDQPSolver::solve()
success = qld_.solve(Q_, C_, Aeq_.block(0, 0, nrAeqLines_, int(Aeq_.cols())), beq_.segment(0, nrAeqLines_),
Aineq_.block(0, 0, nrAineqLines_, int(Aineq_.cols())), bineq_.segment(0, nrAineqLines_), XL_,
XU_, false, 1e-6);
expandResult(qld_.result(), XFull_, reducedToFull_, dependencies_);
expandResult(qld_.result(), XFull_, multipliers_);
}
else
{
Expand Down
6 changes: 3 additions & 3 deletions src/QPSolver.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,9 +120,9 @@ void QPSolver::nrVars(const std::vector<rbd::MultiBody> & mbs,
{
if(j.isMimic())
{
dependencies.emplace_back(data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.mimicName())),
data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.name())),
j.mimicMultiplier());
dependencies.push_back(std::make_tuple(
data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.mimicName())),
data_.alphaDBegin_[r] + mb.jointPosInDof(mb.jointIndexByName(j.name())), j.mimicMultiplier()));
}
}
}
Expand Down
10 changes: 5 additions & 5 deletions src/Tasks/GenQPSolver.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class TASKS_DLLAPI GenQPSolver
/**
* Setup dependent variables, only linear dependencies are supported
* @param nrVars Variable number.
* @param dependencies List of tuple {primary, replica, factor}
* @param dependencies List of tuple {primary, replica, factor, offset}
*/
virtual void setDependencies(int nrVars, std::vector<std::tuple<int, int, double>> dependencies);

Expand Down Expand Up @@ -112,11 +112,11 @@ class TASKS_DLLAPI GenQPSolver
std::vector<int> fullToReduced_;
/** Correspondence between reduced variable indices and full variable indices */
std::vector<int> reducedToFull_;

/** Variable dependencies, each tuple gives the primary variable index in the
* full variable, replica variable index in the full variable and the factor
* in the dependency equation: replica = factor * primary */
/** Variable dependencies, each tuple gives the primary variable index in the full variable, replica variable index in
* the full variable and the factor and offset in the dependency equation: replica = factor * primary */
std::vector<std::tuple<int, int, double>> dependencies_;
/** Multipliers matrix M of size (nFull, nReduced) such that full = M * reduce */
Eigen::MatrixXd multipliers_;
};

} // namespace qp
Expand Down

0 comments on commit f33798e

Please sign in to comment.