From d2e16d24ca9e35de201f41dc15cecff1c4d928a6 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Thu, 21 Mar 2024 16:38:36 +0100 Subject: [PATCH] [core] Catch exception during stepper integration. --- core/include/jiminy/core/engine/engine.h | 4 +- core/include/jiminy/core/robot/model.h | 2 +- .../jiminy/core/stepper/abstract_stepper.h | 28 ++++++-- .../core/stepper/runge_kutta_dopri_stepper.h | 7 +- .../core/telemetry/telemetry_recorder.h | 2 +- core/src/engine/engine.cc | 72 +++++++++++-------- core/src/stepper/abstract_stepper.cc | 49 +++++++------ core/src/stepper/euler_explicit_stepper.cc | 5 +- core/src/stepper/runge_kutta_dopri_stepper.cc | 62 ++++++++-------- python/jiminy_pywrap/src/engine.cc | 2 +- 10 files changed, 135 insertions(+), 98 deletions(-) diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index cf9522fc4..b6bc40265 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/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 diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index 80f0cb1b8..3148c21ea 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -25,7 +25,7 @@ namespace jiminy using ConstraintMap = static_map_t>; - enum class JIMINY_DLLAPI ConstraintNodeType : uint8_t + enum class ConstraintNodeType : uint8_t { BOUNDS_JOINTS = 0, CONTACT_FRAMES = 1, diff --git a/core/include/jiminy/core/stepper/abstract_stepper.h b/core/include/jiminy/core/stepper/abstract_stepper.h index dea23d42c..a847ad1bb 100644 --- a/core/include/jiminy/core/stepper/abstract_stepper.h +++ b/core/include/jiminy/core/stepper/abstract_stepper.h @@ -1,13 +1,31 @@ #ifndef JIMINY_ABSTRACT_STEPPER_H #define JIMINY_ABSTRACT_STEPPER_H +#include #include +#include #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 & /*qSplit*/, const std::vector & /*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 & q, - std::vector & v, - std::vector & a, - double & t, - double & dt); + stepper::StatusInfo tryStep(std::vector & q, + std::vector & v, + std::vector & a, + double & t, + double & dt); protected: /// \brief Internal tryStep method wrapping the arguments as State and diff --git a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h b/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h index bd0e97ae3..46519fc65 100644 --- a/core/include/jiminy/core/stepper/runge_kutta_dopri_stepper.h +++ b/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_; diff --git a/core/include/jiminy/core/telemetry/telemetry_recorder.h b/core/include/jiminy/core/telemetry/telemetry_recorder.h index 27f87db9d..a61089019 100644 --- a/core/include/jiminy/core/telemetry/telemetry_recorder.h +++ b/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}; }; } diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index 67438c07c..8cf54b877 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -1804,7 +1804,7 @@ namespace jiminy // Monitor iteration failure uint32_t successiveIterFailed = 0; std::vector 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) { diff --git a/core/src/stepper/abstract_stepper.cc b/core/src/stepper/abstract_stepper.cc index d9c2853df..15b430099 100644 --- a/core/src/stepper/abstract_stepper.cc +++ b/core/src/stepper/abstract_stepper.cc @@ -13,12 +13,15 @@ namespace jiminy { } - bool AbstractStepper::tryStep(std::vector & qSplit, - std::vector & vSplit, - std::vector & aSplit, - double & t, - double & dt) + stepper::StatusInfo AbstractStepper::tryStep(std::vector & qSplit, + std::vector & vSplit, + std::vector & 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; @@ -26,31 +29,37 @@ namespace jiminy 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) diff --git a/core/src/stepper/euler_explicit_stepper.cc b/core/src/stepper/euler_explicit_stepper.cc index 1a27bf361..2209cc148 100644 --- a/core/src/stepper/euler_explicit_stepper.cc +++ b/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; } } diff --git a/core/src/stepper/runge_kutta_dopri_stepper.cc b/core/src/stepper/runge_kutta_dopri_stepper.cc index 5b1e4dcd9..c38c97a6f 100644 --- a/core/src/stepper/runge_kutta_dopri_stepper.cc +++ b/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(); + } } diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc index e07a9973c..cfdedf81c 100644 --- a/python/jiminy_pywrap/src/engine.cc +++ b/python/jiminy_pywrap/src/engine.cc @@ -6,7 +6,7 @@ #include "jiminy/core/control/abstract_controller.h" #include "jiminy/core/robot/robot.h" #include "jiminy/core/engine/engine.h" -#include "jiminy/core/engine/engine.h" +#include "jiminy/core/stepper/abstract_stepper.h" #include "pinocchio/bindings/python/fwd.hpp" #include