Jiminy 1.7.9 (#502)

* [core] Fix joint constraint reverse flag.
* [core] Fix computation of ref contact position.
* [core] Better handling of integration failure.
* [core/python] Fix bindings class names and missing attributes.
* [misc] Fix documentation generation.

Co-authored-by: Alexis Duburcq <alexis.duburcq@wandercraft.eu>

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.7.8)
+set(BUILD_VERSION 1.7.9)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

build_tools/cmake/docs.cmake

@@ -3,7 +3,9 @@ macro(create_component_docs)
     find_package(Doxygen COMPONENTS dot)
     find_package(Sphinx)
     if (NOT Sphinx_FOUND OR NOT Doxygen_FOUND)
-        message(WARNING "Doxygen or Sphinx not available. Not creating 'docs' cmake component.")
+        install(CODE "message(FATAL_ERROR \"Doxygen or Sphinx not available.\")"
+                COMPONENT docs
+                EXCLUDE_FROM_ALL)
         return()
     endif()
 
@@ -63,13 +65,9 @@ macro(create_component_docs)
     # Nice named target so we can run the job easily
     add_custom_target(sphinx DEPENDS ${DOXYGEN_INDEX_FILE} ${SPHINX_INDEX_FILE})
 
-    # Install both documentations (if doxygen and sphinx are available).
-    if(${Doxygen_FOUND} AND ${Sphinx_FOUND})
-        install(CODE "execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} doxygen)
-                      execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} sphinx)"
-                COMPONENT docs
-                EXCLUDE_FROM_ALL)
-    else()
-        message(WARNING "Doxygen with Dot component not found. Documentation generation disabled.")
-    endif()
+    # Install both documentations
+    install(CODE "execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} doxygen)
+                  execute_process(COMMAND ${CMAKE_MAKE_PROGRAM} sphinx)"
+            COMPONENT docs
+            EXCLUDE_FROM_ALL)
 ENDMACRO()

build_tools/docs/jiminy.doxyfile.in

@@ -230,12 +230,6 @@ TAB_SIZE = 4
 
 ALIASES = "sideeffect=@par Side Effects:\n"
 
-# This tag can be used to specify a number of word-keyword mappings (TCL only).
-# A mapping has the form "name=value". For example adding "class=itcl::class"
-# will allow you to use the command class in the itcl::class meaning.
-
-TCL_SUBST =
-
 # Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
 # only. Doxygen will then generate output that is more tailored for C. For
 # instance, some of the names that are used will be different. The list of all
@@ -1014,13 +1008,6 @@ CLANG_OPTIONS =
 
 ALPHABETICAL_INDEX = YES
 
-# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
-# which the alphabetical index list will be split.
-# Minimum value: 1, maximum value: 20, default value: 5.
-# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
-
-COLS_IN_ALPHA_INDEX = 5
-
 # In case all classes in a project start with a common prefix, all classes will
 # be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
 # can be used to specify a prefix (or a list of prefixes) that should be ignored
@@ -2068,12 +2055,6 @@ EXTERNAL_GROUPS = YES
 
 EXTERNAL_PAGES = YES
 
-# The PERL_PATH should be the absolute path and name of the perl script
-# interpreter (i.e. the result of 'which perl').
-# The default file (with absolute path) is: /usr/bin/perl.
-
-PERL_PATH = /usr/bin/perl
-
 #---------------------------------------------------------------------------
 # Configuration options related to the dot tool
 #---------------------------------------------------------------------------
@@ -2087,15 +2068,6 @@ PERL_PATH = /usr/bin/perl
 
 CLASS_DIAGRAMS = YES
 
-# You can define message sequence charts within doxygen comments using the \msc
-# command. Doxygen will then run the mscgen tool (see:
-# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
-# documentation. The MSCGEN_PATH tag allows you to specify the directory where
-# the mscgen tool resides. If left empty the tool is assumed to be found in the
-# default search path.
-
-MSCGEN_PATH =
-
 # You can include diagrams made with dia in doxygen documentation. Doxygen will
 # then run dia to produce the diagram and insert it in the documentation. The
 # DIA_PATH tag allows you to specify the directory where the dia binary resides.

build_tools/easy_install_deps_ubuntu.sh

@@ -59,7 +59,7 @@ sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgr
 sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade setuptools auditwheel && \
 sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade flake8 pylint mypy types-toml && \
 sudo -u $(id -nu "${SUDO_UID}") env "PATH=${PATH}" python3 -m pip install --upgrade \
-    pygments colorama sphinx sphinx_rtd_theme recommonmark nbsphinx breathe aafigure
+    pygments colorama "jinja2>=3.0,<3.1" sphinx sphinx_rtd_theme recommonmark nbsphinx breathe aafigure
 
 # Install standard linux utilities
 apt install -y gnupg curl wget build-essential cmake doxygen graphviz pandoc

core/include/jiminy/core/constraints/JointConstraint.h

@@ -38,7 +38,7 @@ namespace jiminy
         vectorN_t const & getReferenceConfiguration(void) const;
 
         void setRotationDir(bool_t isReversed);
-        bool_t getRotationDir();
+        bool_t const & getRotationDir();
 
         virtual hresult_t reset(vectorN_t const & q,
                                 vectorN_t const & v) override final;

core/src/constraints/JointConstraint.cc

@@ -57,7 +57,7 @@ namespace jiminy
         isReversed_ = isReversed;
     }
 
-    bool_t JointConstraint::getRotationDir(void)
+    bool_t const & JointConstraint::getRotationDir(void)
     {
         return isReversed_;
     }
@@ -91,12 +91,13 @@ namespace jiminy
             // Get the joint model
             pinocchio::JointModel const & jointModel = model->pncModel_.joints[jointIdx_];
 
-            // Initialize the jacobian. It is simply the velocity selector mask.
-            setRotationDir(isReversed_);
-
             // Initialize constraint jacobian, drift and multipliers
             jacobian_.setZero(jointModel.nv(), model->pncModel_.nv);
             jacobian_.middleCols(jointModel.idx_v(), jointModel.nv()).setIdentity();
+            if (isReversed_)
+            {
+                jacobian_ *= -1;
+            }
             drift_.setZero(jointModel.nv());
             lambda_.setZero(jointModel.nv());
 

core/src/engine/EngineMultiRobot.cc

@@ -1738,8 +1738,9 @@ namespace jiminy
         std::vector<vectorN_t> & vSplit = stepperState_.vSplit;
         std::vector<vectorN_t> & aSplit = stepperState_.aSplit;
 
-        // Successive iteration failure
+        // Monitor iteration failure
         uint32_t successiveIterFailed = 0;
+        bool_t isNan = false;
 
         /* Flag monitoring if the current time step depends of a breakpoint
            or the integration tolerance. It will be used by the restoration
@@ -2022,7 +2023,20 @@ namespace jiminy
                     // Set the timestep to be tried by the stepper
                     dtLargest = dt;
 
-                    if (stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest))
+                    // Try doing one integration step
+                    bool_t isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
+
+                    /* Check if the integrator failed miserably even if successfully.
+                       It would happen if integration failed because of nan and the
+                       timestep is not adaptive. */
+                    isNan = std::isnan(dtLargest);
+                    if (isNan)
+                    {
+                        break;
+                    }
+
+                    // Update buffer if really successful
+                    if (isStepSuccessful)
                     {
                         // Reset successive iteration failure counter
                         successiveIterFailed = 0;
@@ -2081,14 +2095,6 @@ namespace jiminy
                         ++stepperState_.iterFailed;
                     }
 
-                    /* If the integrator is failing miserably, then rely on some
-                       very basic heuristic to try to recover. */
-                    if (std::isnan(dtLargest))
-                    {
-                        PRINT_WARNING("Something is wrong with the physics. Try decreasing timestep.");
-                        dtLargest = 0.1 * dt;
-                    }
-
                     // Initialize the next dt
                     dt = min(dtLargest, engineOptions_->stepper.dtMax);
                 }
@@ -2121,6 +2127,13 @@ namespace jiminy
                     // Try to do a step
                     isStepSuccessful = stepper_->tryStep(qSplit, vSplit, aSplit, t, dtLargest);
 
+                    // Check if the integrator failed miserably even if successfully
+                    isNan = std::isnan(dtLargest);
+                    if (isNan)
+                    {
+                        break;
+                    }
+
                     if (isStepSuccessful)
                     {
                         // Reset successive iteration failure counter
@@ -2168,51 +2181,58 @@ namespace jiminy
                 }
             }
 
-            // Update sensors data if necessary, namely if time-continuous or breakpoint
-            float64_t const & sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod;
-            bool_t mustUpdateSensors = sensorsUpdatePeriod < EPS;
-            float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod);
-            if (!mustUpdateSensors)
-            {
-                mustUpdateSensors = dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP
-                                 || sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < STEPPER_MIN_TIMESTEP;
-            }
-            if (mustUpdateSensors)
+            // Exception handling
+            if (isNan)
             {
-                auto systemIt = systems_.begin();
-                auto systemDataIt = systemsDataHolder_.begin();
-                for ( ; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
-                {
-                    vectorN_t const & q = systemDataIt->state.q;
-                    vectorN_t const & v = systemDataIt->state.v;
-                    vectorN_t const & a = systemDataIt->state.a;
-                    vectorN_t const & uMotor = systemDataIt->state.uMotor;
-                    forceVector_t const & fext = systemDataIt->state.fExternal;
-                    systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
-                }
+                PRINT_ERROR("Something is wrong with the physics. Aborting integration.");
+                returnCode = hresult_t::ERROR_GENERIC;
             }
-
             if (successiveIterFailed > engineOptions_->stepper.successiveIterFailedMax)
             {
                 PRINT_ERROR("Too many successive iteration failures. Probably something is "
                             "going wrong with the physics. Aborting integration.");
                 returnCode = hresult_t::ERROR_GENERIC;
             }
-
             if (dt < STEPPER_MIN_TIMESTEP)
             {
                 PRINT_ERROR("The internal time step is getting too small. Impossible to "
                             "integrate physics further in time.");
                 returnCode = hresult_t::ERROR_GENERIC;
             }
-
             timer_->toc();
             if (EPS < engineOptions_->stepper.timeout
                 && engineOptions_->stepper.timeout < timer_->dt)
             {
                 PRINT_ERROR("Step computation timeout.");
                 returnCode = hresult_t::ERROR_GENERIC;
             }
+
+            // Update sensors data if necessary, namely if time-continuous or breakpoint
+            if (returnCode == hresult_t::SUCCESS)
+            {
+                float64_t const & sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod;
+                bool_t mustUpdateSensors = sensorsUpdatePeriod < EPS;
+                float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod);
+                if (!mustUpdateSensors)
+                {
+                    mustUpdateSensors = dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP
+                                     || sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < STEPPER_MIN_TIMESTEP;
+                }
+                if (mustUpdateSensors)
+                {
+                    auto systemIt = systems_.begin();
+                    auto systemDataIt = systemsDataHolder_.begin();
+                    for ( ; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
+                    {
+                        vectorN_t const & q = systemDataIt->state.q;
+                        vectorN_t const & v = systemDataIt->state.v;
+                        vectorN_t const & a = systemDataIt->state.a;
+                        vectorN_t const & uMotor = systemDataIt->state.uMotor;
+                        forceVector_t const & fext = systemDataIt->state.fExternal;
+                        systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
+                    }
+                }
+            }
         }
 
         /* Update the final time and dt to make sure it corresponds
@@ -3034,7 +3054,7 @@ namespace jiminy
         float64_t const & zGround = std::get<float64_t>(ground);
         vector3_t & nGround = std::get<vector3_t>(ground);
         nGround.normalize();  // Make sure the ground normal is normalized
-        float64_t const depth = (posFrame[2] - zGround) * nGround[2];  // First-order projection (exact assuming flat surface)
+        float64_t const depth = (posFrame[2] - zGround) * nGround[2];  // First-order projection (exact assuming no curvature)
 
         // Only compute the ground reaction force if the penetration depth is negative
         if (depth < 0.0)
@@ -3084,9 +3104,8 @@ namespace jiminy
         {
             auto & frameConstraint = static_cast<FixedFrameConstraint &>(*constraint.get());
             frameConstraint.setReferenceTransform({
-                system.robot->pncData_.oMf[frameIdx].rotation(),
-                (vector3_t() << posFrame.head<2>(), zGround).finished()
-            });
+                transformFrameInWorld.rotation(),
+                posFrame - depth * nGround});
             frameConstraint.setNormal(nGround);
         }
     }

core/src/stepper/AbstractStepper.cc

@@ -14,24 +14,42 @@ namespace jiminy
         // Empty on purpose.
     }
 
-    bool_t AbstractStepper::tryStep(std::vector<vectorN_t> & q,
-                                    std::vector<vectorN_t> & v,
-                                    std::vector<vectorN_t> & a,
+    bool_t AbstractStepper::tryStep(std::vector<vectorN_t> & qSplit,
+                                    std::vector<vectorN_t> & vSplit,
+                                    std::vector<vectorN_t> & aSplit,
                                     float64_t              & t,
                                     float64_t              & dt)
     {
+        // Update buffers
         float64_t t_next = t + dt;
-        state_.q = q;
-        state_.v = v;
-        stateDerivative_.v = v;
-        stateDerivative_.a = a;
+        state_.q = qSplit;
+        state_.v = vSplit;
+        stateDerivative_.v = vSplit;
+        stateDerivative_.a = aSplit;
+
+        // Try doing a single step
         bool_t result = tryStepImpl(state_, stateDerivative_, t, dt);
+
+        // Make sure everything went fine
+        if (result)
+        {
+            for (vectorN_t const & a : stateDerivative_.a)
+            {
+                if ((a.array() != a.array()).any())
+                {
+                    dt = qNAN;
+                    result = false;
+                }
+            }
+        }
+
+        // Update output if successfull
         if (result)
         {
             t = t_next;
-            q = state_.q;
-            v = state_.v;
-            a = stateDerivative_.a;
+            qSplit = state_.q;
+            vSplit = state_.v;
+            aSplit = stateDerivative_.a;
         }
         return result;
     }

core/src/stepper/EulerExplicitStepper.cc

@@ -19,7 +19,7 @@ namespace jiminy
         stateDerivative = f(t, state);
         state.sumInPlace(stateDerivative, dt);
 
-        /* By default INF is retuned in case of fixed time step, so that the
+        /* By default INF is returned in case of fixed time step, so that the
            engine will always try to perform the latest timestep possible,
            or stop to the next breakpoint otherwise. */
         dt = INF;

core/src/stepper/RungeKuttaDOPRIStepper.cc

@@ -48,6 +48,13 @@ namespace jiminy
     bool_t RungeKuttaDOPRIStepper::adjustStepImpl(float64_t const & error,
                                                   float64_t       & dt)
     {
+        // Make sure the error is defined, otherwise rely on a simple heuristic
+        if (std::isnan(error))
+        {
+            dt *= 0.1;
+            return false;
+        }
+
         // Adjustment algorithm from boost implementation.
         if (error < 1.0)
         {