Jiminy release 1.8.1 (#722)

* [core] Error and warning reporting more consistent with Python. (#716)
* [core] Fix exception handling for visco-elastic coupling and internal flex forces. (#716)
* [core] Fix error estimation too optimistic for adaptive steppers. (#716)
* [core] Fix partially broken step adjustment optim that could lead to integration fail. (#721)
* [gym/common] Move 'sample' from 'spaces.py' to 'helpers.py'. (#721)
* [gym/zoo] Fix 'acrobot' and 'cartpole' envs not using wrong integrator. (#719)
* [misc] Do not verify metadata before pushing wheels on PyPi.
* [misc] Fix easy install doc. (#721)
* [misc] Fix support of virtual env ('venv', 'pyenv', ...) when building dependencies. (#721)

---------

Co-authored-by: Alexis Duburcq <[email protected]>

.github/workflows/macos.yml

@@ -219,3 +219,4 @@ jobs:
       uses: pypa/[email protected]
       with:
         packages-dir: wheelhouse
+        verify-metadata: false

.github/workflows/manylinux.yml

@@ -229,3 +229,4 @@ jobs:
       uses: pypa/[email protected]
       with:
         packages-dir: wheelhouse
+        verify-metadata: false

.github/workflows/win.yml

@@ -226,3 +226,4 @@ jobs:
       uses: pypa/[email protected]
       with:
         packages-dir: wheelhouse
+        verify-metadata: false

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.12.4)
 
 # Set the build version (specify a tweak version to indicated post-release if needed)
-set(BUILD_VERSION 1.8.0)
+set(BUILD_VERSION 1.8.1)
 
 # Set compatibility
 set(COMPATIBILITY_VERSION SameMinorVersion)

INSTALL.md

@@ -50,7 +50,7 @@ python -m pip install --prefer-binary gym-jiminy[all]
 
 ## Excluding dependencies on Ubuntu 18+
 
-First, one must install the pre-compiled libraries of the dependencies. Most of them are available on `robotpkg` APT repository. Just run the bash script to install them automatically for Ubuntu 18 and upward. It should be straightforward to adapt it to any other distribution for which `robotpkg` is available.
+First, one must install the pre-compiled libraries of the dependencies. Most of them are available on `robotpkg` APT repository. Just run the bash script to install them automatically for Ubuntu 18 and upward. It should be straightforward to adapt it to any other distribution for which `robotpkg` is available. Note that if you plan to use a virtual environment (`venv`, `pyenv`, ...), you must install the Python dependencies manually using `pip`, i.e. `"wheel", "numpy>=1.21,<2.0"`.
 
 ```bash
 sudo env "PATH=$PATH" ./build_tools/easy_install_deps_ubuntu.sh
@@ -66,16 +66,16 @@ InstallDir="$RootDir/install"
 
 mkdir "$RootDir/build" "$InstallDir"
 cd "$RootDir/build"
-cmake "$RootDir" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-      -DBoost_NO_SYSTEM_PATHS=OFF \
+cmake "$RootDir" -DCMAKE_INSTALL_PREFIX="$InstallDir" -DCMAKE_PREFIX_PATH="/opt/openrobots/" \
+      -DPYTHON_EXECUTABLE="$(python3 -c 'import sys; sys.stdout.write(sys.executable)')" \
       -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \
       -DCMAKE_BUILD_TYPE="$BUILD_TYPE"
 make install -j2
 ```
 
-## Including dependencies on Linux-based OS
+## Including dependencies on Unix-based OS
 
-### Prerequisites
+### Prerequisites (must be adapted to package managers other than `apt`)
 
 ```bash
 sudo apt install -y gnupg curl wget build-essential cmake doxygen graphviz

build_tools/build_install_deps_unix.sh

@@ -61,7 +61,7 @@ if [ -z ${PYTHON_EXECUTABLE} ]; then
 fi
 
 ### Configure site-packages pythonic "symlink" pointing to install directory
-PYTHON_USER_SITELIB="$("${PYTHON_EXECUTABLE}" -m site --user-site)" || true
+PYTHON_USER_SITELIB="$("${PYTHON_EXECUTABLE}" -c 'import sysconfig; print(sysconfig.get_path("purelib"))')" || true
 PYTHON_VERSION="$(${PYTHON_EXECUTABLE} -c "import sysconfig; print(sysconfig.get_config_var('py_version_short'))")"
 mkdir -p "${PYTHON_USER_SITELIB}"
 echo "${InstallDir}/lib/python${PYTHON_VERSION}/site-packages" > "${PYTHON_USER_SITELIB}/install_site.pth"

core/include/jiminy/core/macros.h

@@ -65,18 +65,19 @@ namespace jiminy::internal
 */
 #define THROW_ERROR(exception, ...)                                                       \
     throw exception(                                                                      \
-        toString("In " FILE_LINE ": In ",                                                 \
+        toString("In ",                                                                   \
                  jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION), \
-                 ":\n\x1b[1;31merror:\x1b[0m ",                                           \
+                 " (" FILE_LINE "):\n",                                                   \
                  __VA_ARGS__))
 
 #ifdef NDEBUG
 #    define PRINT_WARNING(...)
 #else
 #    define PRINT_WARNING(...)                                                               \
-        std::cout << "In " FILE_LINE ": In "                                                 \
+        std::cout << "\x1b[1;93mWARNING\x1b[0m: In "                                         \
                   << jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
-                  << ":\n\x1b[1;93mwarning:\x1b[0m " << toString(__VA_ARGS__) << std::endl
+                  << " (" FILE_LINE "):\n"                                                   \
+                  << toString(__VA_ARGS__) << std::endl
 #endif
 
 #endif  // JIMINY_MACRO_H

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/engine/engine_multi_robot.cc

@@ -352,7 +352,7 @@ namespace jiminy
             // Compute intermediary quantities
             rot12.noalias() = oMf1.rotation().transpose() * oMf2.rotation();
             rotLog12 = pinocchio::log3(rot12, angle);
-            if (angle < 0.95 * M_PI)
+            if (angle > 0.95 * M_PI)
             {
                 THROW_ERROR(std::runtime_error,
                             "Relative angle between reference frames of viscoelastic "
@@ -1957,21 +1957,24 @@ namespace jiminy
                         hasDynamicsChanged = false;
                     }
 
-                    // Adjust stepsize to end up exactly at the next breakpoint
-                    dt = std::min(dt, tNext - t);
-                    if (dtLargest > SIMULATION_MIN_TIMESTEP)
+                    /* Adjust stepsize to end up exactly at the next breakpoint if it is reasonable
+                       to expect that integration will be successful, namely:
+                       - If the next breakpoint is closer than the estimated maximum step size
+                       OR
+                       - If the next breakpoint is farther but not so far away compared to the
+                         estimated maximum step size, AND the previous integration trial was
+                         successful. This last condition is essential to prevent infinite loop of
+                         slightly increasing the step size, failing to integrate, then try again
+                         and again until triggering maximum successive iteration failure exception.
+                         The current implementation is conservative and does not check that the
+                         previous failure was due to this stepsize adjustment procedure, but it is
+                         just a performance optimization trick, so it should not be a big deal. */
+                    const double dtResidualThr =
+                        std::min(SIMULATION_MIN_TIMESTEP, 0.1 * dtLargest);
+                    if (tNext - t < dt ||
+                        (successiveIterFailed == 0 && tNext - t < dt + dtResidualThr))
                     {
-                        if (tNext - (t + dt) < SIMULATION_MIN_TIMESTEP)
-                        {
-                            dt = tNext - t;
-                        }
-                    }
-                    else
-                    {
-                        if (tNext - (t + dt) < STEPPER_MIN_TIMESTEP)
-                        {
-                            dt = tNext - t;
-                        }
+                        dt = tNext - t;
                     }
 
                     /* Trying to reach multiples of STEPPER_MIN_TIMESTEP whenever possible. The
@@ -2555,7 +2558,12 @@ namespace jiminy
         const double dtMax = boost::get<double>(stepperOptions.at("dtMax"));
         if (SIMULATION_MAX_TIMESTEP + EPS < dtMax || dtMax < SIMULATION_MIN_TIMESTEP)
         {
-            THROW_ERROR(std::invalid_argument, "'dtMax' option is out of range.");
+            THROW_ERROR(std::invalid_argument,
+                        "'dtMax' option must bge in range [",
+                        SIMULATION_MIN_TIMESTEP,
+                        ", ",
+                        SIMULATION_MAX_TIMESTEP,
+                        "].");
         }
 
         // Make sure successiveIterFailedMax is strictly positive
@@ -3409,8 +3417,11 @@ namespace jiminy
 
             const Eigen::Map<const Eigen::Quaterniond> quat(q.segment<4>(positionIndex).data());
             const Eigen::Vector3d angleAxis = pinocchio::quaternion::log3(quat, angle);
-            assert((angle < 0.95 * M_PI) &&
-                   "Flexible joint angle must be smaller than 0.95 * pi.");
+            if (angle > 0.95 * M_PI)  // Angle is always positive
+            {
+                THROW_ERROR(std::runtime_error,
+                            "Flexible joint angle must be smaller than 0.95 * pi.");
+            }
             pinocchio::Jlog3(angle, angleAxis, rotJlog3);
             uInternal.segment<3>(velocityIndex) -=
                 rotJlog3 * (stiffness.array() * angleAxis.array()).matrix();

core/src/stepper/runge_kutta_dopri_stepper.cc

@@ -25,6 +25,13 @@ namespace jiminy
     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)
@@ -36,44 +43,38 @@ 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_;
-        }
-
-        // Return the smallest error between absolute and relative
-        return std::min(errorAbsNorm, errorRelNorm);
+        // 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 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.
+                   Multiply step size by 'DOPRI::SAFETY / (error ** (1 / DOPRI::STEPPER_ORDER))',
+                   up to 'DOPRI::MAX_FACTOR'.
+                */
+                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;
         }

python/gym_jiminy/common/gym_jiminy/common/utils/__init__.py

@@ -3,7 +3,6 @@
 from .spaces import (DataNested,
                      FieldNested,
                      get_bounds,
-                     sample,
                      zeros,
                      fill,
                      copyto,
@@ -20,7 +19,8 @@
 from .helpers import (is_breakpoint,
                       is_nan,
                       get_fieldnames,
-                      register_variables)
+                      register_variables,
+                      sample)
 from .pipeline import build_pipeline, load_pipeline