[misc] Check no memory allocated by Eigen during simulation. (#278)

* [misc] Check no memory allocation by Eigen during simulation.
* [misc] Update Eigen to latest release (3.3.9) on CI.
* [core] Fix joints accel/force restored right after computations instead of before sensor update.

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

CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.5.6)
+set(BUILD_VERSION 1.5.7)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

build_tools/build_install_deps_linux.sh

@@ -42,11 +42,11 @@ git submodule --quiet update --init --recursive --jobs 8
 
 ### Checkout eigen3
 if [ ! -d "$RootDir/eigen3" ]; then
-  git clone https://github.com/eigenteam/eigen-git-mirror.git "$RootDir/eigen3"
+  git clone https://gitlab.com/libeigen/eigen.git "$RootDir/eigen3"
 fi
 cd "$RootDir/eigen3"
 git reset --hard
-git checkout --force "3.3.7"
+git checkout --force "3.3.9"
 
 ### Checkout eigenpy and its submodules, then apply some patches (generated using `git diff --submodule=diff`)
 if [ ! -d "$RootDir/eigenpy" ]; then
@@ -243,7 +243,7 @@ mkdir -p "$RootDir/hpp-fcl/build"
 cd "$RootDir/hpp-fcl/build"
 cmake "$RootDir/hpp-fcl" -Wno-dev -DCMAKE_CXX_STANDARD=14 -DCMAKE_INSTALL_PREFIX="$InstallDir" \
       -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF \
-      -DCMAKE_PREFIX_PATH="$InstallDir" -DQhull_PREFIX="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \
+      -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="$PYTHON_EXECUTABLE" \
       -DPYTHON_STANDARD_LAYOUT=ON -DBoost_NO_SYSTEM_PATHS=TRUE -DBoost_NO_BOOST_CMAKE=TRUE \
       -DBOOST_ROOT="$InstallDir" -DBoost_INCLUDE_DIR="$InstallDir/include" \
       -DBUILD_PYTHON_INTERFACE=ON -DHPP_FCL_HAS_QHULL=ON \

build_tools/build_install_deps_windows.ps1

@@ -43,10 +43,10 @@ git submodule --quiet update --init --recursive --jobs 8
 
 ### Checkout eigen3
 if (-not (Test-Path -PathType Container "$RootDir/eigen3")) {
-  git clone https://github.com/eigenteam/eigen-git-mirror.git "$RootDir/eigen3"
+  git clone https://gitlab.com/libeigen/eigen.git "$RootDir/eigen3"
 }
 Set-Location -Path "$RootDir/eigen3"
-git checkout --force "3.3.7"
+git checkout --force "3.3.9"
 
 ### Checkout eigenpy and its submodules, then apply some patches (generated using `git diff --submodule=diff`)
 if (-not (Test-Path -PathType Container "$RootDir/eigenpy")) {

core/src/engine/EngineMultiRobot.cc

@@ -2928,6 +2928,14 @@ namespace jiminy
                and efforts since they depend on the sensor values themselves. */
             if (engineOptions_->stepper.sensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP)
             {
+                // Restore previous forces and accelerations that has been alterated
+                for (int32_t i = 0; i < systemIt->robot->pncModel_.njoints; ++i)
+                {
+                    systemIt->robot->pncData_.f[i] = (*fPrevIt)[i];
+                    systemIt->robot->pncData_.a[i] = (*aPrevIt)[i];
+                }
+
+                // Update sensors based on previous accelerations and forces.
                 systemIt->robot->setSensorsData(t, *qIt, *vIt, aPrev, uMotorPrev);
             }
 
@@ -2959,13 +2967,6 @@ namespace jiminy
 
             // Compute the dynamics
             *aIt = computeAcceleration(*systemIt, *qIt, *vIt, u, fext);
-
-            // Restore previous forces and accelerations that has been alterated
-            for (int32_t i = 0; i < systemIt->robot->pncModel_.njoints; ++i)
-            {
-                systemIt->robot->pncData_.f[i] = (*fPrevIt)[i];
-                systemIt->robot->pncData_.a[i] = (*aPrevIt)[i];
-            }
         }
 
         return hresult_t::SUCCESS;

unit/EngineSanityCheck.cc

@@ -1,9 +1,11 @@
 // Test the sanity of the simulation engine.
 // The tests in this file verify that the behavior of a simulated system matches
-// real-world physics.
+// real-world physics, and that no memory is allocated by Eigen during a simulation.
 // The test system is a double inverted pendulum.
 #include <gtest/gtest.h>
 
+#define EIGEN_RUNTIME_NO_MALLOC
+
 #include "jiminy/core/engine/Engine.h"
 #include "jiminy/core/robot/BasicMotors.h"
 #include "jiminy/core/control/ControllerFunctor.h"
@@ -91,8 +93,8 @@ TEST(EngineSanity, EnergyConservation)
 
     // Configure engine: High accuracy + Continuous-time integration
     configHolder_t simuOptions = engine->getDefaultEngineOptions();
-    boost::get<float64_t>(boost::get<configHolder_t>(simuOptions.at("stepper")).at("tolAbs")) = 1.0e-11;
-    boost::get<float64_t>(boost::get<configHolder_t>(simuOptions.at("stepper")).at("tolRel")) = 1.0e-11;
+    boost::get<float64_t>(boost::get<configHolder_t>(simuOptions.at("stepper")).at("tolAbs")) = TOLERANCE * 1.0e-2;
+    boost::get<float64_t>(boost::get<configHolder_t>(simuOptions.at("stepper")).at("tolRel")) = TOLERANCE * 1.0e-2;
     engine->setOptions(simuOptions);
 
     // Run simulation
@@ -102,12 +104,18 @@ TEST(EngineSanity, EnergyConservation)
     float64_t tf = 10.0;
 
     // Run simulation
-    engine->simulate(tf, q0, v0);
+    engine->start(q0, v0);
+    Eigen::internal::set_is_malloc_allowed(false);
+    engine->step(tf);
+    engine->stop();
+    Eigen::internal::set_is_malloc_allowed(true);
 
     // Get system energy
     std::vector<std::string> header;
     matrixN_t data;
     engine->getLogData(header, data);
+    auto timeCont = getLogFieldValue("Global.Time", header, data);
+    ASSERT_DOUBLE_EQ(timeCont[timeCont.size()-1], tf);
     auto energyCont = getLogFieldValue("HighLevelController.energy", header, data);
     ASSERT_GT(energyCont.size(), 0);
 
@@ -122,10 +130,16 @@ TEST(EngineSanity, EnergyConservation)
     engine->setOptions(simuOptions);
 
     // Run simulation
-    engine->simulate(tf, q0, v0);
+    engine->start(q0, v0);
+    Eigen::internal::set_is_malloc_allowed(false);
+    engine->step(tf);
+    engine->stop();
+    Eigen::internal::set_is_malloc_allowed(true);
 
     // Get system energy
     engine->getLogData(header, data);
+    auto timeDisc = getLogFieldValue("Global.Time", header, data);
+    ASSERT_DOUBLE_EQ(timeDisc[timeDisc.size()-1], tf);
     auto energyDisc = getLogFieldValue("HighLevelController.energy", header, data);
     ASSERT_GT(energyDisc.size(), 0);