[core] Catch exception during stepper integration.

core/include/jiminy/core/engine/engine.h

@@ -14,14 +14,14 @@ namespace jiminy
 {
     inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"};
 
-    enum class JIMINY_DLLAPI ContactModelType : uint8_t
+    enum class ContactModelType : uint8_t
     {
         UNSUPPORTED = 0,
         SPRING_DAMPER = 1,
         CONSTRAINT = 2
     };
 
-    enum class JIMINY_DLLAPI ConstraintSolverType : uint8_t
+    enum class ConstraintSolverType : uint8_t
     {
         UNSUPPORTED = 0,
         PGS = 1  // Projected Gauss-Seidel

core/include/jiminy/core/robot/model.h

@@ -25,7 +25,7 @@ namespace jiminy
 
     using ConstraintMap = static_map_t<std::string, std::shared_ptr<AbstractConstraintBase>>;
 
-    enum class JIMINY_DLLAPI ConstraintNodeType : uint8_t
+    enum class ConstraintNodeType : uint8_t
     {
         BOUNDS_JOINTS = 0,
         CONTACT_FRAMES = 1,

core/include/jiminy/core/stepper/abstract_stepper.h

@@ -1,13 +1,31 @@
 #ifndef JIMINY_ABSTRACT_STEPPER_H
 #define JIMINY_ABSTRACT_STEPPER_H
 
+#include <optional>
 #include <functional>
+#include <exception>
 
 #include "jiminy/core/fwd.h"
 #include "jiminy/core/stepper/lie_group.h"
 
 namespace jiminy
 {
+    namespace stepper
+    {
+        enum class ReturnCode : uint8_t
+        {
+            IS_SUCCESS = 0,
+            IS_FAILURE = 1,
+            IS_ERROR = 2
+        };
+
+        struct JIMINY_DLLAPI StatusInfo
+        {
+            ReturnCode returnCode;
+            std::exception_ptr exception;
+        };
+    }
+
     using systemDynamics = std::function<void(double /*t*/,
                                               const std::vector<Eigen::VectorXd> & /*qSplit*/,
                                               const std::vector<Eigen::VectorXd> & /*vSplit*/,
@@ -47,11 +65,11 @@ namespace jiminy
         ///                    unmodified.
         ///
         /// \return Whether integration was successful. If not, (q, v, a) are not updated.
-        bool tryStep(std::vector<Eigen::VectorXd> & q,
-                     std::vector<Eigen::VectorXd> & v,
-                     std::vector<Eigen::VectorXd> & a,
-                     double & t,
-                     double & dt);
+        stepper::StatusInfo tryStep(std::vector<Eigen::VectorXd> & q,
+                                    std::vector<Eigen::VectorXd> & v,
+                                    std::vector<Eigen::VectorXd> & a,
+                                    double & t,
+                                    double & dt);
 
     protected:
         /// \brief Internal tryStep method wrapping the arguments as State and

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

@@ -61,8 +61,12 @@ namespace jiminy
                                         double tolAbs) noexcept;
 
     protected:
+
         /// \brief Determine if step has succeeded or failed, and adjust dt.
         ///
+        /// \details Scale timestep based on normalized error wrt prescribed absolute and relative
+        ///          tolerances.
+        ///
         /// \param[in] intialState Starting state used to compute alternative estimates of the
         ///                        solution.
         /// \param[in] solution Current solution computed by the main Runge-Kutta step.
@@ -82,9 +86,6 @@ namespace jiminy
         /// \returns Normalized error, >1 indicates step failure.
         double computeError(const State & initialState, const State & solution, double dt);
 
-        /// \brief Scale timestep based on normalized error value.
-        bool adjustStepImpl(double error, double & dt);
-
     private:
         /// \brief Relative tolerance.
         double tolRel_;

core/include/jiminy/core/telemetry/telemetry_recorder.h

@@ -71,7 +71,7 @@ namespace jiminy
         int64_t floatSectionSize_{-1};
 
         /// \brief Precision to use when logging the time.
-        double timeUnitInv_{NAN};
+        double timeUnitInv_{qNAN};
     };
 }
 

core/src/engine/engine.cc

@@ -1804,7 +1804,7 @@ namespace jiminy
         // Monitor iteration failure
         uint32_t successiveIterFailed = 0;
         std::vector<uint32_t> successiveSolveFailedAll(robots_.size(), 0U);
-        bool isNan = false;
+        stepper::StatusInfo status{stepper::ReturnCode::IS_SUCCESS, {}};
 
         /* Flag monitoring if the current time step depends of a breakpoint or the integration
            tolerance. It will be used by the restoration mechanism, if dt gets very small to reach
@@ -2108,17 +2108,8 @@ namespace jiminy
                     dtLargest = dt;
 
                     // Try doing one integration step
-                    bool isStepSuccessful =
-                        stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
-
-                    /* Check if the integrator failed miserably even if successfully.
-                       It would happen the timestep is fixed and too large, causing the integrator
-                       to fail miserably returning nan. */
-                    isNan = std::isnan(dtLargest);
-                    if (isNan)
-                    {
-                        break;
-                    }
+                    status = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
+                    bool isStepSuccessful = status.returnCode == stepper::ReturnCode::IS_SUCCESS;
 
                     // Update buffer if really successful
                     if (isStepSuccessful)
@@ -2175,6 +2166,17 @@ namespace jiminy
                     }
                     else
                     {
+                        /* Check if the integrator raised an exception. This typically happens
+                           when the timestep is fixed and too large, causing the integrator to
+                           fail miserably returning nan. In such a case, adjust the timestep
+                           manually as a recovery mechanism based on a simple heuristic.
+                           Note that it has no effect for fixed-timestep integrator since
+                           `dtLargest` should be INF already. */
+                        if (status.returnCode == stepper::ReturnCode::IS_ERROR)
+                        {
+                            dtLargest *= 0.1;
+                        }
+
                         // Increment the failed iteration counters
                         ++successiveIterFailed;
                         ++stepperState_.iterFailed;
@@ -2207,9 +2209,6 @@ namespace jiminy
                 bool isStepSuccessful = false;
                 while (!isStepSuccessful)
                 {
-                    // Set the timestep to be tried by the stepper
-                    dtLargest = dt;
-
                     // Break the loop in case of too many successive failed inner iteration
                     if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax)
                     {
@@ -2236,15 +2235,12 @@ namespace jiminy
                         }
                     }
 
-                    // Try to do a step
-                    isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
+                    // Set the timestep to be tried by the stepper
+                    dtLargest = dt;
 
-                    // Check if the integrator failed miserably even if successfully
-                    isNan = std::isnan(dtLargest);
-                    if (isNan)
-                    {
-                        break;
-                    }
+                    // Try to do a step
+                    status = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
+                    isStepSuccessful = status.returnCode == stepper::ReturnCode::IS_SUCCESS;
 
                     if (isStepSuccessful)
                     {
@@ -2285,6 +2281,12 @@ namespace jiminy
                     }
                     else
                     {
+                        // Adjust timestep manually if necessary
+                        if (status.returnCode == stepper::ReturnCode::IS_ERROR)
+                        {
+                            dtLargest *= 0.1;
+                        }
+
                         // Increment the failed iteration counter
                         ++successiveIterFailed;
                         ++stepperState_.iterFailed;
@@ -2305,16 +2307,26 @@ namespace jiminy
             }
 
             // Exception handling
-            if (isNan)
-            {
-                THROW_ERROR(std::runtime_error,
-                            "Something is wrong with the physics. Aborting integration.");
-            }
             if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax)
             {
+                if (status.exception)
+                {
+                    try
+                    {
+                        std::rethrow_exception(status.exception);
+                    }
+                    catch (const std::exception & e)
+                    {
+                        // TODO: Try using `std::throw_with_nested` instead
+                        std::runtime_error(toString(
+                            "Something is wrong with the physics. Try using an adaptive stepper. "
+                            "Aborting integration.\nRaised from exception: ",
+                            e.what()));
+                    }
+                }
                 THROW_ERROR(std::runtime_error,
-                            "Too many successive iteration failures. Probably something "
-                            "is going wrong with the physics. Aborting integration.");
+                            "Too many successive iteration failures. Probably something is wrong "
+                            "with the physics. Aborting integration.");
             }
             for (uint32_t successiveSolveFailed : successiveSolveFailedAll)
             {

core/src/stepper/abstract_stepper.cc

@@ -13,44 +13,53 @@ namespace jiminy
     {
     }
 
-    bool AbstractStepper::tryStep(std::vector<Eigen::VectorXd> & qSplit,
-                                  std::vector<Eigen::VectorXd> & vSplit,
-                                  std::vector<Eigen::VectorXd> & aSplit,
-                                  double & t,
-                                  double & dt)
+    stepper::StatusInfo AbstractStepper::tryStep(std::vector<Eigen::VectorXd> & qSplit,
+                                                 std::vector<Eigen::VectorXd> & vSplit,
+                                                 std::vector<Eigen::VectorXd> & aSplit,
+                                                 double & t,
+                                                 double & dt)
     {
+        // Initialize return status
+        stepper::StatusInfo status{stepper::ReturnCode::IS_SUCCESS, {}};
+
         // Update buffers
         double t_next = t + dt;
         state_.q = qSplit;
         state_.v = vSplit;
         stateDerivative_.v = vSplit;
         stateDerivative_.a = aSplit;
 
-        // Try doing a single step
-        bool result = tryStepImpl(state_, stateDerivative_, t, dt);
-
-        // Make sure everything went fine
-        if (result)
+        // Do a single step
+        try
         {
+            // Try doing step
+            if (!tryStepImpl(state_, stateDerivative_, t, dt))
+            {
+                return {stepper::ReturnCode::IS_FAILURE, {}};
+            }
+
+            // Make sure everything went fine
             for (const Eigen::VectorXd & a : stateDerivative_.a)
             {
                 if ((a.array() != a.array()).any())
                 {
-                    dt = qNAN;
-                    result = false;
+                    THROW_ERROR(std::runtime_error, "The integrated acceleration contains 'nan'.");
                 }
             }
         }
-
-        // Update output if successful
-        if (result)
+        catch (...)
         {
-            t = t_next;
-            qSplit = state_.q;
-            vSplit = state_.v;
-            aSplit = stateDerivative_.a;
+
+            return {stepper::ReturnCode::IS_ERROR, std::current_exception()};
         }
-        return result;
+
+        // Update output if successful
+        t = t_next;
+        qSplit = state_.q;
+        vSplit = state_.v;
+        aSplit = stateDerivative_.a;
+
+        return {stepper::ReturnCode::IS_SUCCESS, {}};
     }
 
     const StateDerivative & AbstractStepper::f(double t, const State & state)

core/src/stepper/euler_explicit_stepper.cc

@@ -12,10 +12,11 @@ namespace jiminy
         // Compute the next state derivative
         stateDerivative = f(t, state);
 
-        // By default INF is returned in case of fixed timestep. It must be managed externally.
+        /* By default INF is returned no matter what for fixed-timestep integrators.
+           The user is responsible for managing it externally. */
         dt = INF;
 
-        // Scheme never considers failure.
+        // Scheme never considers failure
         return true;
     }
 }

core/src/stepper/runge_kutta_dopri_stepper.cc

@@ -18,43 +18,13 @@ namespace jiminy
     bool RungeKuttaDOPRIStepper::adjustStep(
         const State & initialState, const State & solution, double & dt)
     {
+        // Estimate the integration error
         const double error = computeError(initialState, solution, dt);
-        return adjustStepImpl(error, dt);
-    }
-
-    double RungeKuttaDOPRIStepper::computeError(
-        const State & initialState, const State & solution, double dt)
-    {
-        // Compute error scale given absolute and relative tolerance
-        otherSolution_.setZero();
-        initialState.difference(otherSolution_, scale_);
-        scale_.absInPlace();
-        scale_ *= tolRel_;
-        scale_ += tolAbs_;
-
-        // Compute alternative solution
-        stateIncrement_.setZero();
-        for (std::size_t i = 0; i < ki_.size(); ++i)
-        {
-            stateIncrement_.sumInPlace(ki_[i], dt * DOPRI::e[i]);
-        }
-        initialState.sum(stateIncrement_, otherSolution_);
-
-        // Evaluate error between both states to adjust step
-        solution.difference(otherSolution_, error_);
 
-        // Return element-wise maximum rescaled error
-        error_ /= scale_;
-        return error_.normInf();
-    }
-
-    bool RungeKuttaDOPRIStepper::adjustStepImpl(double error, double & dt)
-    {
-        // Make sure the error is defined, otherwise rely on simple heuristic
+        // Make sure the error is well defined, otherwise throw an exception
         if (std::isnan(error))
         {
-            dt *= 0.1;
-            return false;
+            THROW_ERROR(std::runtime_error, "The estimated integration error contains 'nan'.");
         }
 
         /* Adjustment algorithm from boost implementation.
@@ -84,4 +54,30 @@ namespace jiminy
             return false;
         }
     }
+
+    double RungeKuttaDOPRIStepper::computeError(
+        const State & initialState, const State & solution, double dt)
+    {
+        // Compute error scale given absolute and relative tolerance
+        otherSolution_.setZero();
+        initialState.difference(otherSolution_, scale_);
+        scale_.absInPlace();
+        scale_ *= tolRel_;
+        scale_ += tolAbs_;
+
+        // Compute alternative solution
+        stateIncrement_.setZero();
+        for (std::size_t i = 0; i < ki_.size(); ++i)
+        {
+            stateIncrement_.sumInPlace(ki_[i], dt * DOPRI::e[i]);
+        }
+        initialState.sum(stateIncrement_, otherSolution_);
+
+        // Evaluate error between both states to adjust step
+        solution.difference(otherSolution_, error_);
+
+        // Return element-wise maximum rescaled error
+        error_ /= scale_;
+        return error_.normInf();
+    }
 }