[core] Compute PGS error in residual space instead of optimization sp…

…ace.
duburcqa · Dec 1, 2021 · a94541a · a94541a
1 parent 01101a4
commit a94541a
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 8 deletions.
diff --git a/core/include/jiminy/core/solver/LCPSolvers.h b/core/include/jiminy/core/solver/LCPSolvers.h
@@ -72,6 +72,8 @@ namespace jiminy
         std::vector<uint32_t> indices_;
         uint32_t lastShuffle_;
         vectorN_t b_;
+        vectorN_t y_;
+        vectorN_t yPrev_;
     };
 
 

diff --git a/core/src/solver/LCPSolvers.cc b/core/src/solver/LCPSolvers.cc
@@ -22,7 +22,9 @@ namespace jiminy
     tolRel_(tolRel),
     indices_(),
     lastShuffle_(0U),
-    b_()
+    b_(),
+    y_(),
+    yPrev_()
     {
         // Empty on purpose.
     }
@@ -34,8 +36,6 @@ namespace jiminy
                                                std::vector<std::vector<int32_t> > const & fIndices,
                                                vectorN_t & x)
     {
-        bool_t isSuccess = true;
-
         // Shuffle coefficient update order to break any repeating cycles
         if (randomPermutationPeriod_ > 0 && lastShuffle_ > randomPermutationPeriod_)
         {
@@ -44,12 +44,14 @@ namespace jiminy
         }
         ++lastShuffle_;
 
+        // Backup previous solution
+        yPrev_.noalias() = A * x;
+
         // 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
             e += (b[i] - A.col(i).dot(x)) / A(i, i);
@@ -95,11 +97,15 @@ namespace jiminy
                     e = 0.0;
                 }
             }
-
-            // Check if still possible to terminate after complete update
-            isSuccess = isSuccess && (std::abs(e - ePrev) < tolAbs_ || std::abs((e - ePrev) / e) < tolRel_);
         }
 
+        // Check if terminate conditions are satisfied
+        y_.noalias() = A * x;
+        bool_t isSuccess = (
+            (y_ - yPrev_).array().abs() < tolAbs_ ||
+            ((y_ - yPrev_).array() / y_.array()).abs() < tolRel_
+        ).all();
+
         return isSuccess;
     }
 

diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py
@@ -41,7 +41,7 @@ def _test_pid_standing(self):
         action_init['Q'], action_init['V'] = encoder_data[
             :, self.env.controller.motor_to_encoder]
 
-        # Run the simulation during 12s
+        # Run the simulation
         while self.env.stepper_state.t < 12.0:
             self.env.step(action_init)