[core] Revert too optimistic error computation in adaptive steppers c…

…ausing integ failure.

core/include/jiminy/core/stepper/lie_group.h

@@ -76,6 +76,13 @@ namespace Eigen
         const DataType & a() const { return derived().a(); }
         DataType & a() { return derived().a(); }
 
+        StateDerivativeBase & absInPlace()
+        {
+            v().array() = v().array().abs();
+            a().array() = a().array().abs();
+            return *this;
+        }
+
         template<int p>
         RealScalar lpNorm() const;
         RealScalar norm() const { return lpNorm<2>(); };
@@ -1246,6 +1253,15 @@ namespace Eigen
             vector_[i] += scale * vectorIn[i];                                                \
         }                                                                                     \
         return *this;                                                                         \
+    }                                                                                         \
+                                                                                              \
+    StateDerivativeVector & absInPlace()                                                      \
+    {                                                                                         \
+        for (std::size_t i = 0; i < vector_.size(); ++i)                                      \
+        {                                                                                     \
+            vector_[i].absInPlace();                                                          \
+        }                                                                                     \
+        return *this;                                                                         \
     }
 
 #define State_SHARED_ADDON                                                                        \

core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h

@@ -34,8 +34,14 @@ namespace jiminy
         /// \brief Stepper order, used to scale the error.
         inline constexpr double STEPPER_ORDER = 5.0;
         /// \brief Safety factor when updating the error, should be less than 1.
-        inline constexpr double SAFETY = 0.8;
-        /// \brief Miminum allowed relative step decrease.
+        ///
+        /// \details The larger the more conservative. More precisely, a small safety factor means
+        ///          that the step size will be increased less aggressively when the error is small
+        ///          and decreased more aggressively when the error is large.
+        inline constexpr double SAFETY = 0.9;
+        /// \brief Maximum acceptable error threshold below which step size is increased.
+        inline constexpr double ERROR_THRESHOLD = 0.5;
+        /// \brief Mininum allowed relative step decrease.
         inline constexpr double MIN_FACTOR = 0.2;
         /// \brief Maximum allowed relative step increase.
         inline constexpr double MAX_FACTOR = 5.0;

core/src/stepper/runge_kutta_dopri_stepper.cc

@@ -36,44 +36,44 @@ namespace jiminy
         // Evaluate error between both states to adjust step
         solution.difference(otherSolution_, error_);
 
-        // Compute absolute and relative element-wise maximum error
-        double errorAbsNorm = INF;
-        double errorRelNorm = INF;
-        if (tolAbs_ > EPS)
-        {
-            errorAbsNorm = error_.normInf() / tolAbs_;
-        }
-        if (tolRel_ > EPS)
-        {
-            otherSolution_.setZero();
-            solution.difference(otherSolution_, scale_);
-            error_ /= scale_;
-            errorRelNorm = error_.normInf() / tolRel_;
-        }
+        // Compute error scale
+        otherSolution_.setZero();
+        solution.difference(otherSolution_, scale_);
+        scale_.absInPlace();
+        scale_ *= tolRel_;
+        scale_ += tolAbs_;
 
-        // Return the smallest error between absolute and relative
-        return std::min(errorAbsNorm, errorRelNorm);
+        // Return rescaled element-wise maximum error
+        error_ /= scale_;
+        return error_.normInf();
     }
 
     bool RungeKuttaDOPRIStepper::adjustStepImpl(double error, double & dt)
     {
-        // Make sure the error is defined, otherwise rely on a simple heuristic
+        // Make sure the error is defined, otherwise rely on simple heuristic
         if (std::isnan(error))
         {
             dt *= 0.1;
             return false;
         }
 
-        // Adjustment algorithm from boost implementation
+        /* Adjustment algorithm from boost implementation.
+           For technical reference, see original boost::odeint implementation:
+           https://beta.boost.org/doc/libs/1_82_0/libs/numeric/odeint/doc/html/boost_numeric_odeint/odeint_in_detail/steppers.html#boost_numeric_odeint.odeint_in_detail.steppers.controlled_steppers
+        */
         if (error < 1.0)
         {
-            // Only increase if error is sufficiently small
-            if (error < std::pow(DOPRI::SAFETY, DOPRI::STEPPER_ORDER))
+            /* Increase step size only if the error is sufficiently small.
+               The threshold must be chosen in a way to guarantee that it actually decreases. */
+            if (error <
+                std::min(DOPRI::ERROR_THRESHOLD, std::pow(DOPRI::SAFETY, DOPRI::STEPPER_ORDER)))
             {
-                // Prevent numeric rounding error when close to zero
-                const double newError = std::max(
+                /* Prevent numeric rounding error when close to zero.
+                   The step size 'dt' can be multiply by up to 'DOPRI::MAX_FACTOR' if small enough,
+                   otherwise it is given by 'DOPRI::SAFETY / (error ** DOPRI::STEPPER_ORDER)'. */
+                const double clippedError = std::max(
                     error, std::pow(DOPRI::MAX_FACTOR / DOPRI::SAFETY, -DOPRI::STEPPER_ORDER));
-                dt *= DOPRI::SAFETY * std::pow(newError, -1.0 / DOPRI::STEPPER_ORDER);
+                dt *= DOPRI::SAFETY * std::pow(clippedError, -1.0 / DOPRI::STEPPER_ORDER);
             }
             return true;
         }