diff --git a/core/include/jiminy/core/solver/LCPSolvers.h b/core/include/jiminy/core/solver/LCPSolvers.h index 462bf441f3..b03c0818b2 100644 --- a/core/include/jiminy/core/solver/LCPSolvers.h +++ b/core/include/jiminy/core/solver/LCPSolvers.h @@ -72,6 +72,8 @@ namespace jiminy std::vector 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 index 85d55dae6f..b0b81a4acf 100644 --- 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 > 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 index 118ff19bd7..47b26f143c 100644 --- 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)