[gym] Fix 'step' observation not consistent with system state. (#230)

* [core] Add 'PRINT_WARNING' macro, to print debug message when compiling in debug mode only. * [core] Update sensors data at the end of 'step' for consistency with robot state. * [python] Replace 'BaseJiminyController' controller by 'BaseJiminyObserverController' to add dedicated observer support. * [gym] Controller/observer blocks must be fully configured at init. Block '_setup' is now called at 'reset'. * [gym] Fix observe/control update period computed at init instead of reset. * [gym] Take advantage of new dedicated observer. * [gym] 'get_observation' recursively shadow-copy observation before returning it to avoid altering original data structure. * [gym] Rename '*DictRecursive' in '*DictNested' for clarity. * [gym] Add frame stacking pipeline block and wrapper. * [gym] Rework pipeline builder to be more flexible. * [gym] Update '_action' buffer to match lastest 'compute_command' call. Co-authored-by: Alexis Duburcq <[email protected]>
duburcqa · Nov 21, 2020 · ef3ffee · ef3ffee
1 parent c0059de
commit ef3ffee
Show file tree

Hide file tree

Showing 21 changed files with 764 additions and 570 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 3.10)
 
 # Set the build version
-set(BUILD_VERSION 1.4.12)
+set(BUILD_VERSION 1.4.13)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

diff --git a/core/include/jiminy/core/Macro.h b/core/include/jiminy/core/Macro.h
@@ -165,7 +165,14 @@ namespace jiminy
     #define FILE_LINE __FILE__ ":" STRINGIFY(__LINE__)
 
     #define PRINT_ERROR(...) \
-    std::cout << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\033[1;31merror:\033[0m " << to_string(__VA_ARGS__) << std::endl;
+    std::cout << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\033[1;32merror:\033[0m " << to_string(__VA_ARGS__) << std::endl;
+
+    #ifdef NDEBUG
+    #define PRINT_WARNING(...)
+    #else
+    #define PRINT_WARNING(...) \
+    std::cout << "In " FILE_LINE ": In " << BOOST_CURRENT_FUNCTION << ":\n\033[1;93mwarning:\033[0m " << to_string(__VA_ARGS__) << std::endl;
+    #endif
 }
 
 #endif  // JIMINY_MACRO_H
diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -1127,48 +1127,24 @@ namespace jiminy
                 }
             }
 
-            if (stepperUpdatePeriod_ > EPS)
+            // Update the controller command if necessary (only for finite update frequency)
+            if (stepperUpdatePeriod_ > EPS && engineOptions_->stepper.controllerUpdatePeriod > EPS)
             {
-                // Update the sensor data if necessary (only for finite update frequency)
-                if (engineOptions_->stepper.sensorsUpdatePeriod > EPS)
+                float64_t const & controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod;
+                float64_t dtNextControllerUpdatePeriod = controllerUpdatePeriod - std::fmod(t, controllerUpdatePeriod);
+                if (dtNextControllerUpdatePeriod <= SIMULATION_MIN_TIMESTEP / 2.0
+                || controllerUpdatePeriod - dtNextControllerUpdatePeriod < SIMULATION_MIN_TIMESTEP / 2.0)
                 {
-                    float64_t const & sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod;
-                    float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod);
-                    if (dtNextSensorsUpdatePeriod <= SIMULATION_MIN_TIMESTEP / 2.0
-                    || sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP / 2.0)
+                    auto systemIt = systems_.begin();
+                    auto systemDataIt = systemsDataHolder_.begin();
+                    for ( ; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
                     {
-                        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;
-                            systemIt->robot->setSensorsData(t, q, v, a, uMotor);
-                        }
-                    }
-                }
-
-                // Update the controller command if necessary (only for finite update frequency)
-                if (engineOptions_->stepper.controllerUpdatePeriod > EPS)
-                {
-                    float64_t const & controllerUpdatePeriod = engineOptions_->stepper.controllerUpdatePeriod;
-                    float64_t dtNextControllerUpdatePeriod = controllerUpdatePeriod - std::fmod(t, controllerUpdatePeriod);
-                    if (dtNextControllerUpdatePeriod <= SIMULATION_MIN_TIMESTEP / 2.0
-                    || controllerUpdatePeriod - dtNextControllerUpdatePeriod < SIMULATION_MIN_TIMESTEP / 2.0)
-                    {
-                        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 & uCommand = systemDataIt->state.uCommand;
-                            computeCommand(*systemIt, t, q, v, uCommand);
-                        }
-                        hasDynamicsChanged = true;
+                        vectorN_t const & q = systemDataIt->state.q;
+                        vectorN_t const & v = systemDataIt->state.v;
+                        vectorN_t & uCommand = systemDataIt->state.uCommand;
+                        computeCommand(*systemIt, t, q, v, uCommand);
                     }
+                    hasDynamicsChanged = true;
                 }
             }
 
@@ -1400,6 +1376,29 @@ namespace jiminy
                 }
             }
 
+            // Update sensors data if necessary, namely if time-continuous or breakpoint
+            float64_t const & sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod;
+            bool mustUpdateSensors = sensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP;
+            if (!mustUpdateSensors)
+            {
+                float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod);
+                mustUpdateSensors = (dtNextSensorsUpdatePeriod <= SIMULATION_MIN_TIMESTEP / 2.0
+                || sensorsUpdatePeriod - dtNextSensorsUpdatePeriod < SIMULATION_MIN_TIMESTEP / 2.0);
+            }
+            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;
+                    systemIt->robot->setSensorsData(t, q, v, a, uMotor);
+                }
+            }
+
             if (sucessiveIterFailed > engineOptions_->stepper.successiveIterFailedMax)
             {
                 PRINT_ERROR("Too many successive iteration failures. Probably something is "

diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc
@@ -1069,7 +1069,7 @@ namespace jiminy
         }
         catch (std::logic_error const & e)
         {
-            std::cout << "hpp-fcl not built with qhull. Impossible to convert meshes to convex hulls." << std::endl;
+            PRINT_WARNING("hpp-fcl not built with qhull. Impossible to convert meshes to convex hulls.");
         }
 
         // Instantiate ground FCL box geometry, wrapped as a pinocchio collision geometry.

diff --git a/examples/tutorial.ipynb b/examples/tutorial.ipynb
@@ -64,7 +64,6 @@
     "\n",
     "import jiminy_py.core as jiminy # The main module of jiminy - this is what gives access to the Robot and Engine class.\n",
     "from jiminy_py.simulator import Simulator\n",
-    "from jiminy_py.controller import BaseJiminyController\n",
     "\n",
     "\n",
     "data_root_path = \"../data/toys_models/simple_pendulum\"\n",