Skip to content

Commit

Permalink
[core] Add relaxation to PGS solver to mitigate convergence instabili…
Browse files Browse the repository at this point in the history 
…ties.
  • Loading branch information
@duburcqa
duburcqa committed Feb 15, 2024
1 parent 21bb282 commit b0e6aaf
Show file tree
Hide file tree
Showing 6 changed files with 43 additions and 18 deletions.
10 changes: 6 additions & 4 deletions core/include/jiminy/core/robot/pinocchio_overload_algorithms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -494,10 +494,10 @@ namespace jiminy::pinocchio_overload
}

template<typename JacobianType>
void computeJMinvJt(const pinocchio::Model & model,
pinocchio::Data & data,
const Eigen::MatrixBase<JacobianType> & J,
bool updateDecomposition = true)
const Eigen::MatrixXd & computeJMinvJt(const pinocchio::Model & model,
pinocchio::Data & data,
const Eigen::MatrixBase<JacobianType> & J,
bool updateDecomposition = true)
{
// Compute the Cholesky decomposition of mass matrix M if requested
if (updateDecomposition)
Expand Down Expand Up @@ -536,6 +536,8 @@ namespace jiminy::pinocchio_overload
data.JMinvJt.resize(J.rows(), J.rows());
data.JMinvJt.triangularView<Eigen::Lower>().setZero();
data.JMinvJt.selfadjointView<Eigen::Lower>().rankUpdate(sDUiJt.transpose());

return data.JMinvJt;
}

template<typename RhsType>
Expand Down
1 change: 1 addition & 0 deletions core/include/jiminy/core/solver/constraint_solvers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,7 @@ namespace jiminy
private:
void ProjectedGaussSeidelIter(const Eigen::MatrixXd & A,
const Eigen::VectorXd::SegmentReturnType & b,
const double w,
Eigen::VectorXd::SegmentReturnType & x);
bool ProjectedGaussSeidelSolver(const Eigen::MatrixXd & A,
const Eigen::VectorXd::SegmentReturnType & b,
Expand Down
2 changes: 1 addition & 1 deletion core/src/engine/engine_multi_robot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@
namespace jiminy
{
inline constexpr uint32_t INIT_ITERATIONS{4U};
inline constexpr uint32_t PGS_MAX_ITERATIONS{100U};
inline constexpr uint32_t PGS_MAX_ITERATIONS{120U};

EngineMultiRobot::EngineMultiRobot() noexcept :
generator_{std::seed_seq{std::random_device{}()}},
Expand Down
26 changes: 23 additions & 3 deletions core/src/solver/constraint_solvers.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,12 @@ namespace jiminy
{
inline constexpr double PGS_MIN_REGULARIZER{1.0e-11};

inline constexpr double PGS_RELAX_MIN{0.01};
inline constexpr double PGS_RELAX_MAX{0.7};
inline constexpr double PGS_RELAX_ITER_REL_MIN{0.3};
inline constexpr double PGS_RELAX_ITER_REL_MAX{0.8};
inline constexpr double PGS_RELAX_RATIO_ORDER{2.0};

PGSSolver::PGSSolver(const pinocchio::Model * model,
pinocchio::Data * data,
ConstraintTree * constraints,
Expand Down Expand Up @@ -99,6 +105,7 @@ namespace jiminy

void PGSSolver::ProjectedGaussSeidelIter(const Eigen::MatrixXd & A,
const Eigen::VectorXd::SegmentReturnType & b,
const double w,
Eigen::VectorXd::SegmentReturnType & x)
{
// First, loop over all unbounded constraints
Expand Down Expand Up @@ -168,11 +175,11 @@ namespace jiminy
A_max = A_kk;
}
}
e += y_[i0] / A_max;
e += w * y_[i0] / A_max;
for (std::uint_fast8_t j = 1; j < fSize - 1; ++j)
{
const Eigen::Index k = o + fIndex[j];
x[k] += y_[k] / A_max;
x[k] += w * y_[k] / A_max;
}

// Project the coefficient between lower and upper bounds
Expand Down Expand Up @@ -232,8 +239,21 @@ namespace jiminy
// Backup previous residuals
yPrev_ = y_;

// Update the under-relaxation factor
double w = PGS_RELAX_MAX;
const double ratio = (PGS_RELAX_ITER_REL_MAX - static_cast<double>(iter) / iterMax_) /
(PGS_RELAX_ITER_REL_MAX - PGS_RELAX_ITER_REL_MIN);
if (ratio < 1.0)
{
w = PGS_RELAX_MIN;
if (ratio > 0.0)
{
w += (PGS_RELAX_MAX - PGS_RELAX_MIN) * std::pow(ratio, PGS_RELAX_RATIO_ORDER);
}
}

// Do a single iteration
ProjectedGaussSeidelIter(A, b, x);
ProjectedGaussSeidelIter(A, b, w, x);

/* Stopping the algorithm has stalled.
It is impossible to define a criteria on the residuals directly because they only
Expand Down
1 change: 1 addition & 0 deletions data/bipedal_robots/atlas/atlas_v4_options.toml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,7 @@ model = "constraint"
stabilizationFreq = 25.0
transitionEps = 5.0e-3
friction = 0.8
torsion = 0.0

# ======== Joints bounds configuration ========

Expand Down
21 changes: 11 additions & 10 deletions python/jiminy_pywrap/src/helpers.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -306,33 +306,33 @@ namespace jiminy::python

// Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors
bp::def("interpolate_positions", &interpolatePositions,
(bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out"),
bp::return_value_policy<result_converter<true>>());
bp::return_value_policy<result_converter<true>>(),
(bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out"));

bp::def("aba",
&pinocchio_overload::aba<
double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>,
bp::return_value_policy<result_converter<false>>(),
(bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "u", "fext"),
"Compute ABA with external forces, store the result in Data::ddq and return it.",
bp::return_value_policy<result_converter<false>>());
"Compute ABA with external forces, store the result in Data::ddq and return it.");
bp::def("rnea",
&pinocchio_overload::rnea<
double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd>,
bp::return_value_policy<result_converter<false>>(),
(bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"),
"Compute the RNEA without external forces, store the result in Data and return it.",
bp::return_value_policy<result_converter<false>>());
"Compute the RNEA without external forces, store the result in Data and return it.");
bp::def("rnea",
&pinocchio_overload::rnea<
double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>,
bp::return_value_policy<result_converter<false>>(),
(bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a", "fext"),
"Compute the RNEA with external forces, store the result in Data and return it.",
bp::return_value_policy<result_converter<false>>());
"Compute the RNEA with external forces, store the result in Data and return it.");
bp::def("crba",
&pinocchio_overload::crba<
double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>,
bp::return_value_policy<result_converter<false>>(),
(bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false),
"Computes CRBA, store the result in Data and return it.",
bp::return_value_policy<result_converter<false>>());
"Computes CRBA, store the result in Data and return it.");
bp::def("computeKineticEnergy",
&pinocchio_overload::computeKineticEnergy<
double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd>,
Expand All @@ -343,6 +343,7 @@ namespace jiminy::python

bp::def("computeJMinvJt",
&pinocchio_overload::computeJMinvJt<Eigen::MatrixXd>,
bp::return_value_policy<result_converter<false>>(),
(bp::arg("pinocchio_model"), "pinocchio_data", "J", bp::arg("update_decomposition") = true));
bp::def("solveJMinvJtv", &solveJMinvJtv,
(bp::arg("pinocchio_data"), "v", bp::arg("update_decomposition") = true));
Expand Down

0 comments on commit b0e6aaf

Please sign in to comment.