Skip to content

Commit

Permalink
[core] Improve efficiency of PGS solver.
Browse files Browse the repository at this point in the history
  • Loading branch information
Alexis Duburcq committed Oct 23, 2021
1 parent 51ac68a commit 5c89439
Show file tree
Hide file tree
Showing 6 changed files with 66 additions and 55 deletions.
8 changes: 5 additions & 3 deletions core/include/jiminy/core/robot/PinocchioOverloadAlgorithms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -349,13 +349,15 @@ namespace pinocchio_overload
pinocchio::cholesky::decompose(model, data);
}

// Compute sqrt(D)^-1 * U^-1 * J.T
// Compute sDUiJt := sqrt(D)^-1 * U^-1 * J.T
data.sDUiJt = J.transpose();
pinocchio::cholesky::Uiv(model, data, data.sDUiJt);
data.sDUiJt.array().colwise() /= data.D.array().sqrt();

// Compute JMinvJt
data.JMinvJt.noalias() = data.sDUiJt.transpose() * data.sDUiJt;
// Compute JMinvJt := sDUiJt.T * sDUiJt
data.JMinvJt = matrixN_t::Zero(J.rows(), J.rows());
data.JMinvJt.selfadjointView<Eigen::Lower>().rankUpdate(data.sDUiJt.transpose());
data.JMinvJt.triangularView<Eigen::Upper>() = data.JMinvJt.transpose();

return data.JMinvJt;
}
Expand Down
30 changes: 18 additions & 12 deletions core/include/jiminy/core/solver/LCPSolvers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@

namespace jiminy
{
float64_t const EPS_DIVISION = 1.0e-9;
float64_t const EPS_DIVISION = 1.0e-6;

class AbstractLCPSolver
{
Expand All @@ -17,7 +17,7 @@ namespace jiminy
///
/// using boxed bounds lo < x < hi instead of 0 < x:
/// s.t. if fIdx[i] < 0, lo[i] < x[i] < hi[i]
/// else, - hi[i] x[fIdx[i]] < x[i] < hi[i] x[fIdx[i]]
/// else, - hi[i] * x[fIdx[i]] < x[i] < hi[i] * x[fIdx[i]]
///
/// The result x will be stored in data.lambda_c.
virtual bool_t BoxedForwardDynamics(pinocchio::Model const & model,
Expand Down Expand Up @@ -50,20 +50,20 @@ namespace jiminy
std::vector<int32_t> const & fIdx) override final;

private:
bool_t ProjectedGaussSeidelIter(matrixN_t const & A,
vectorN_t const & b,
vectorN_t const & lo,
vectorN_t const & hi,
bool_t ProjectedGaussSeidelIter(Eigen::MatrixXf const & A,
Eigen::VectorXf const & b,
Eigen::VectorXf const & lo,
Eigen::VectorXf const & hi,
std::vector<int32_t> const & fIdx,
bool_t const & checkAbs,
bool_t const & checkRel,
vectorN_t & x);
bool_t ProjectedGaussSeidelSolver(matrixN_t & A,
vectorN_t & b,
vectorN_t const & lo,
vectorN_t const & hi,
Eigen::VectorXf & x);
bool_t ProjectedGaussSeidelSolver(Eigen::MatrixXf & A,
Eigen::VectorXf & b,
Eigen::VectorXf const & lo,
Eigen::VectorXf const & hi,
std::vector<int32_t> const & fIdx,
vectorN_t & x);
Eigen::VectorXf & x);

private:
uint32_t maxIter_;
Expand All @@ -73,6 +73,12 @@ namespace jiminy
std::vector<uint32_t> indices_;
uint32_t lastShuffle_;
vectorN_t b_;

Eigen::MatrixXf A_;
Eigen::VectorXf bf_;
Eigen::VectorXf lo_;
Eigen::VectorXf hi_;
Eigen::VectorXf f_ ;
};


Expand Down
12 changes: 4 additions & 8 deletions core/include/jiminy/core/utilities/Helpers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,20 +107,16 @@ namespace jiminy
template<typename T0, typename T1, typename... Ts>
typename std::common_type_t<T0, T1, Ts...> min(T0 && val1, T1 && val2, Ts &&... vs);

template<typename DerivedType>
auto clamp(Eigen::MatrixBase<DerivedType> const & data,
float64_t const & minThr = -INF,
float64_t const & maxThr = +INF);
template<typename ScalarType>
ScalarType clamp(ScalarType const & data,
ScalarType const & minThr,
ScalarType const & maxThr);

template<typename DerivedType1, typename DerivedType2, typename DerivedType3>
Eigen::MatrixBase<DerivedType1> clamp(Eigen::MatrixBase<DerivedType1> const & data,
Eigen::MatrixBase<DerivedType2> const & minThr,
Eigen::MatrixBase<DerivedType2> const & maxThr);

float64_t clamp(float64_t const & data,
float64_t const & minThr = -INF,
float64_t const & maxThr = +INF);

template<typename... Args>
float64_t minClipped(float64_t val1, float64_t val2, Args ... vs);

Expand Down
8 changes: 8 additions & 0 deletions core/include/jiminy/core/utilities/Helpers.tpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,14 @@ namespace jiminy
return min(std::min(val1, val2), std::forward<Ts>(vs)...);
}

template<typename ScalarType>
ScalarType clamp(ScalarType const & data,
ScalarType const & minThr,
ScalarType const & maxThr)
{
return std::min(std::max(data, minThr), maxThr);
}

template<typename DerivedType>
auto clamp(Eigen::MatrixBase<DerivedType> const & data,
float64_t const & minThr,
Expand Down
47 changes: 31 additions & 16 deletions core/src/solver/LCPSolvers.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,19 +22,24 @@ namespace jiminy
tolRel_(tolRel),
indices_(),
lastShuffle_(0U),
b_()
b_(),
A_(),
bf_(),
lo_(),
hi_(),
f_ ()
{
// Empty on purpose.
}

bool_t PGSSolver::ProjectedGaussSeidelIter(matrixN_t const & A,
vectorN_t const & b,
vectorN_t const & lo,
vectorN_t const & hi,
bool_t PGSSolver::ProjectedGaussSeidelIter(Eigen::MatrixXf const & A,
Eigen::VectorXf const & b,
Eigen::VectorXf const & lo,
Eigen::VectorXf const & hi,
std::vector<int32_t> const & fIdx,
bool_t const & checkAbs,
bool_t const & checkRel,
vectorN_t & x)
Eigen::VectorXf & x)
{
bool_t isSuccess = true;

Expand All @@ -50,8 +55,8 @@ namespace jiminy
for (int32_t const & i : indices_)
{
// Update a single coefficient
float64_t const xPrev = x[i];
x[i] += (b[i] - A.row(i).dot(x)) / A(i, i);
float32_t const xPrev = x[i];
x[i] += b[i] - A.col(i).dot(x);

// Project the coefficient between lower and upper bounds
if (fIdx[i] < 0)
Expand All @@ -60,7 +65,7 @@ namespace jiminy
}
else
{
float64_t const hiTmp = hi[i] * x[fIdx[i]];
float32_t const hiTmp = hi[i] * x[fIdx[i]];
x[i] = clamp(x[i], - hiTmp, hiTmp);
}

Expand All @@ -78,12 +83,12 @@ namespace jiminy
return isSuccess;
}

bool_t PGSSolver::ProjectedGaussSeidelSolver(matrixN_t & A,
vectorN_t & b,
vectorN_t const & lo,
vectorN_t const & hi,
bool_t PGSSolver::ProjectedGaussSeidelSolver(Eigen::MatrixXf & A,
Eigen::VectorXf & b,
Eigen::VectorXf const & lo,
Eigen::VectorXf const & hi,
std::vector<int32_t> const & fIdx,
vectorN_t & x)
Eigen::VectorXf & x)
{
/* The implementation is partially adapted from Dart Simulator:
https://github.com/dartsim/dart/blob/master/dart/constraint/PgsBoxedLcpSolver.cpp */
Expand Down Expand Up @@ -126,7 +131,7 @@ namespace jiminy
for (Eigen::Index i = 0; i < b.size(); ++i)
{
b[i] /= A(i, i);
A.row(i).array() /= A(i, i);
A.col(i).array() /= A(i, i);
}

// Perform multiple PGS loop until convergence or max iter reached
Expand Down Expand Up @@ -198,8 +203,18 @@ namespace jiminy
}
else
{
// Cast data to float32_t
A_ = A.cast<float32_t>();
bf_ = b_.cast<float32_t>();
lo_ = lo.cast<float32_t>();
hi_ = hi.cast<float32_t>();
f_ = f.cast<float32_t>();

// Run standard PGS algorithm
isSuccess = ProjectedGaussSeidelSolver(A, b_, lo, hi, fIdx, f);
isSuccess = ProjectedGaussSeidelSolver(A_, bf_, lo_, hi_, fIdx, f_);

// Cast back the result in float64_t
f = f_.cast<float64_t>();
}

// Compute resulting acceleration, no matter if computing forces was successful
Expand Down
16 changes: 0 additions & 16 deletions core/src/utilities/Helpers.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -169,20 +169,4 @@ namespace jiminy
auto start = std::find(header.begin(), header.end(), "StartColumns");
return logData.col(std::distance(start, iterator) - 1);
}

// ********************** Math utilities *************************

float64_t clamp(float64_t const & data,
float64_t const & minThr,
float64_t const & maxThr)
{
if (!isnan(data))
{
return std::min(std::max(data, minThr), maxThr);
}
else
{
return 0.0;
}
}
}

0 comments on commit 5c89439

Please sign in to comment.