[core] Even faster PGS.

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<std::string>(options.at("model"))),
             solver(boost::get<std::string>(options.at("solver"))),
+            tolAbs(boost::get<float64_t>(options.at("tolAbs"))),
+            tolRel(boost::get<float64_t>(options.at("tolRel"))),
             regularization(boost::get<float64_t>(options.at("regularization"))),
             stabilizationFreq(boost::get<float64_t>(options.at("stabilizationFreq"))),
             stiffness(boost::get<float64_t>(options.at("stiffness"))),

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<int32_t> const & fIdx,
-                                        bool_t const & checkAbs,
-                                        bool_t const & checkRel,
                                         vectorN_t & x);
         bool_t ProjectedGaussSeidelSolver(matrixN_t & A,
                                           vectorN_t & b,

core/src/engine/EngineMultiRobot.cc

@@ -888,8 +888,8 @@ namespace jiminy
             contactSolver_ = std::make_unique<PGSSolver>(
                 PGS_MAX_ITERATIONS,
                 PGS_RANDOM_PERMUTATION_PERIOD,
-                engineOptions_->stepper.tolAbs,
-                engineOptions_->stepper.tolRel);
+                engineOptions_->contacts.tolAbs,
+                engineOptions_->contacts.tolRel);
         }
     }
 

core/src/solver/LCPSolvers.cc

@@ -32,8 +32,6 @@ namespace jiminy
                                                vectorN_t const & lo,
                                                vectorN_t const & hi,
                                                std::vector<int32_t> 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,

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

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

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