diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h index 2ffa18cc3b..fb90296191 100644 --- a/core/include/jiminy/core/engine/EngineMultiRobot.h +++ b/core/include/jiminy/core/engine/EngineMultiRobot.h @@ -116,6 +116,8 @@ namespace jiminy configHolder_t config; config["model"] = std::string("spring_damper"); // ["spring_damper", "impulse"] config["solver"] = std::string("PGS"); // ["PGS",] + config["tolAbs"] = 1.0e-3; + config["tolRel"] = 1.0e-3; config["regularization"] = 0.0; // Relative inverse damping wrt. diagonal of J.Minv.J.t. 0.0 to enforce the minimum absolute regularizer. config["stabilizationFreq"] = 20.0; // [s-1]: 0.0 to disable config["stiffness"] = 1.0e6; @@ -200,6 +202,8 @@ namespace jiminy { std::string const model; std::string const solver; + float64_t const tolAbs; + float64_t const tolRel; float64_t const regularization; float64_t const stabilizationFreq; float64_t const stiffness; @@ -212,6 +216,8 @@ namespace jiminy contactOptions_t(configHolder_t const & options) : model(boost::get(options.at("model"))), solver(boost::get(options.at("solver"))), + tolAbs(boost::get(options.at("tolAbs"))), + tolRel(boost::get(options.at("tolRel"))), regularization(boost::get(options.at("regularization"))), stabilizationFreq(boost::get(options.at("stabilizationFreq"))), stiffness(boost::get(options.at("stiffness"))), diff --git a/core/include/jiminy/core/solver/LCPSolvers.h b/core/include/jiminy/core/solver/LCPSolvers.h index 9eba6f962b..55c70e32a2 100644 --- a/core/include/jiminy/core/solver/LCPSolvers.h +++ b/core/include/jiminy/core/solver/LCPSolvers.h @@ -6,8 +6,6 @@ namespace jiminy { - float64_t const EPS_DIVISION = 1.0e-4; - class AbstractLCPSolver { public: @@ -55,8 +53,6 @@ namespace jiminy vectorN_t const & lo, vectorN_t const & hi, std::vector const & fIdx, - bool_t const & checkAbs, - bool_t const & checkRel, vectorN_t & x); bool_t ProjectedGaussSeidelSolver(matrixN_t & A, vectorN_t & b, diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index b5aad68fb2..c6a660c72f 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -888,8 +888,8 @@ namespace jiminy contactSolver_ = std::make_unique( PGS_MAX_ITERATIONS, PGS_RANDOM_PERMUTATION_PERIOD, - engineOptions_->stepper.tolAbs, - engineOptions_->stepper.tolRel); + engineOptions_->contacts.tolAbs, + engineOptions_->contacts.tolRel); } } diff --git a/core/src/solver/LCPSolvers.cc b/core/src/solver/LCPSolvers.cc index afb5b0e127..23589c07b3 100644 --- a/core/src/solver/LCPSolvers.cc +++ b/core/src/solver/LCPSolvers.cc @@ -32,8 +32,6 @@ namespace jiminy vectorN_t const & lo, vectorN_t const & hi, std::vector const & fIdx, - bool_t const & checkAbs, - bool_t const & checkRel, vectorN_t & x) { bool_t isSuccess = true; @@ -49,30 +47,26 @@ namespace jiminy // Update every coefficients sequentially for (uint32_t const & i : indices_) { + // Extract single coefficient + float64_t & e = x[i]; + float64_t const ePrev = e; + // Update a single coefficient - float64_t const xPrev = x[i]; - x[i] += (b[i] - A.col(i).dot(x)) / A(i, i); + e += (b[i] - A.col(i).dot(x)) / A(i, i); // Project the coefficient between lower and upper bounds if (fIdx[i] < 0) { - x[i] = clamp(x[i], lo[i], hi[i]); + e = std::clamp(e, lo[i], hi[i]); } else { float64_t const hiTmp = hi[i] * x[fIdx[i]]; - x[i] = clamp(x[i], - hiTmp, hiTmp); + e = std::clamp(e, -hiTmp, hiTmp); } // Check if still possible to terminate after complete update - if (checkAbs) - { - isSuccess = isSuccess && (std::abs(x[i] - xPrev) < tolAbs_); - } - if (checkRel && std::abs(x[i]) > EPS_DIVISION) - { - isSuccess = isSuccess && (std::abs((x[i] - xPrev) / x[i]) < tolRel_); - } + isSuccess = isSuccess && (std::abs(e - ePrev) < tolAbs_ || std::abs((e - ePrev) / e) < tolRel_); } return isSuccess; @@ -123,29 +117,21 @@ namespace jiminy } // Perform multiple PGS loop until convergence or max iter reached - uint32_t iter = 0; - bool_t isSuccess = ProjectedGaussSeidelIter(A, b, lo, hi, fIdx, true, false, x); - while (!isSuccess) + for (uint32_t iter = 0; iter < maxIter_; ++iter) { + bool_t isSuccess = ProjectedGaussSeidelIter(A, b, lo, hi, fIdx, x); if (isSuccess) { // Do NOT shuffle indices unless necessary to avoid discontinuities + // std::cout << "PGS iter: " << iter + 1 << std::endl; lastShuffle_ = 0U; - break; + return true; } - if (iter < maxIter_) - { - isSuccess = ProjectedGaussSeidelIter(A, b, lo, hi, fIdx, false, true, x); - } - else - { - break; - } - ++iter; } + // std::cout << "PGS iter: " << maxIter_ << std::endl; // Impossible to converge - return isSuccess; + return false; } bool_t PGSSolver::BoxedForwardDynamics(pinocchio::Model const & model, diff --git a/data/bipedal_robots/atlas/atlas_v4_options.toml b/data/bipedal_robots/atlas/atlas_v4_options.toml index 13cafff0eb..d634264706 100644 --- a/data/bipedal_robots/atlas/atlas_v4_options.toml +++ b/data/bipedal_robots/atlas/atlas_v4_options.toml @@ -3,8 +3,6 @@ [engine.stepper] verbose = false odeSolver = "runge_kutta_4" -tolAbs = 1.0e-4 -tolRel = 1.0e-3 sensorsUpdatePeriod = 0.01 controllerUpdatePeriod = 0.01 logInternalStepperSteps = false @@ -15,6 +13,8 @@ randomSeed = 0 [engine.contacts] model = "impulse" solver = "PGS" +tolAbs = 1.0e-3 +tolRel = 1.0e-3 regularization = 2.0e-3 stabilizationFreq = 20.0 transitionEps = 5.0e-3 diff --git a/data/bipedal_robots/cassie/cassie_options.toml b/data/bipedal_robots/cassie/cassie_options.toml index 3fbdf03875..f5d73b2b4f 100644 --- a/data/bipedal_robots/cassie/cassie_options.toml +++ b/data/bipedal_robots/cassie/cassie_options.toml @@ -3,8 +3,6 @@ [engine.stepper] verbose = false odeSolver = "runge_kutta_4" -tolAbs = 1.0e-4 -tolRel = 1.0e-3 sensorsUpdatePeriod = 0.01 controllerUpdatePeriod = 0.01 logInternalStepperSteps = false @@ -15,6 +13,8 @@ randomSeed = 0 [engine.contacts] model = "impulse" solver = "PGS" +tolAbs = 1.0e-3 +tolRel = 1.0e-3 regularization = 2.0e-3 stabilizationFreq = 20.0 transitionEps = 2.0e-3 diff --git a/data/toys_models/ant/ant_options.toml b/data/toys_models/ant/ant_options.toml index be419b3c55..41d4721cfc 100644 --- a/data/toys_models/ant/ant_options.toml +++ b/data/toys_models/ant/ant_options.toml @@ -15,6 +15,8 @@ randomSeed = 0 [engine.contacts] model = "impulse" solver = "PGS" +tolAbs = 1.0e-3 +tolRel = 1.0e-3 regularization = 1.0e-1 stabilizationFreq = 5.0 transitionEps = 1.0e-2