Jiminy 1.7.2 (#418)

* [core] Add 'orientation' parameter 'randomTileGround' generator.
* [core/python] Fix python bindings of 'resetRandomGenerators' method.
* [core/python] Fix assert in debug.
* [python/simulation] Fix 'seed' method not resetting the generator if the seed does not change.
* [python/viewer] Close panda3d viewer cleanly.
* [gym/common] Render ground profile automatically during 'play_interactive'.
* [gym/rllib] Fix PPO for 'enable_adversarial_noise' = False and 'caps_global_reg' == 0.0.
* [misc] Build wheels for manylinux2014 instead of manylinux2010 because of 'dm-tree'.
* [misc] Build in Release and Debug on Ubuntu CI.

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

.github/workflows/manylinux.yml

@@ -15,7 +15,7 @@ jobs:
 
     strategy:
       matrix:
-        container: ['quay.io/pypa/manylinux2010_x86_64',
+        container: ['quay.io/pypa/manylinux2014_x86_64',
                     'quay.io/pypa/manylinux_2_24_x86_64']
         PYTHON_VERSION: ['cp36', 'cp37', 'cp38', 'cp39']
         legacy: [false]

.github/workflows/ubuntu.yml

@@ -22,9 +22,6 @@ jobs:
       run:
         shell: bash -ieo pipefail {0}  # Using bash enables automatic sourcing `.bashrc` and fail-fast behavior
 
-    env:
-      BUILD_TYPE: "Release"
-
     #####################################################################################
 
     steps:
@@ -49,8 +46,8 @@ jobs:
     - name: Installing requirements
       run: |
         sudo env "PATH=$PATH" "${GITHUB_WORKSPACE}/build_tools/easy_install_deps_ubuntu.sh"
-        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
         "${PYTHON_EXECUTABLE}" -m pip install tensorflow
+        "${PYTHON_EXECUTABLE}" -m pip install --upgrade numpy
         "${PYTHON_EXECUTABLE}" -m pip install "torch==1.8.0+cpu" -f https://download.pytorch.org/whl/torch_stable.html
         "${PYTHON_EXECUTABLE}" -m pip install --prefer-binary "gym>=0.18.3" "stable_baselines3>=0.10" "importlib-metadata>=3.3.0"
         "${PYTHON_EXECUTABLE}" -m pip install "ray[default,rllib]<=1.4.0"  # Type checking is not working with 1.4.1
@@ -73,7 +70,9 @@ jobs:
               -DCMAKE_INTERPROCEDURAL_OPTIMIZATION=OFF \
               -DBoost_NO_SYSTEM_PATHS=OFF -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \
               -DBUILD_TESTING=ON -DBUILD_EXAMPLES=ON -DBUILD_PYTHON_INTERFACE=ON \
-              -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
+              -DCMAKE_BUILD_TYPE="Debug"
+        make install -j2
+        cmake -DCMAKE_BUILD_TYPE="Release" .
         make install -j2
 
     #####################################################################################
@@ -85,7 +84,7 @@ jobs:
         mkdir -p "$RootDir/examples/cpp/pip_extension/build"
         cd "$RootDir/examples/cpp/pip_extension/build"
         cmake "$RootDir/examples/cpp/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
-              -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
+              -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" -DCMAKE_BUILD_TYPE="Release"
         make install
 
         "$InstallDir/bin/pip_double_pendulum"

CMakeLists.txt

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

core/include/jiminy/core/stepper/LieGroup.h

@@ -443,7 +443,7 @@ namespace Eigen
                                     StateBase<OutDerived>                   & out) const
         {
             // 'Sum' q = q + v, remember q is part of a Lie group (dim(q) != dim(v))
-            assert(robot() == velocity.robot() == out.robot());
+            assert(robot() == velocity.robot() && robot() == out.robot());
             pinocchio::integrate(robot()->pncModel_, q(), velocity.v(), out.q());
             out.v() = v() + velocity.a();
             return out;
@@ -459,7 +459,7 @@ namespace Eigen
         StateDerivativeBase<OutDerived> & difference(StateBase<OtherDerived>         const & position,
                                                      StateDerivativeBase<OutDerived>       & out) const
         {
-            assert(robot() == position.robot() == out.robot());
+            assert(robot() == position.robot() && robot() == out.robot());
             pinocchio::difference(robot()->pncModel_, q(), position.q(), out.v());
             out.a() = v() - position.v();
             return out;
@@ -1257,7 +1257,7 @@ namespace Eigen
     { \
         std::vector<typename internal::traits<Derived>::ValueType> const & \
             vectorIn = other.vector(); \
-        assert(vector_.size() == vectorOut.size()); \
+        assert(vector_.size() == vectorIn.size()); \
         for (std::size_t i = 0; i < vector_.size(); ++i) \
         { \
             vector_[i] += scale * vectorIn[i]; \
@@ -1305,7 +1305,7 @@ namespace Eigen
     { \
         std::vector<typename internal::traits<Derived>::ValueType> const & \
             vectorIn = other.vector(); \
-        assert(vector_.size() == vectorOut.size()); \
+        assert(vector_.size() == vectorIn.size()); \
         for (std::size_t i = 0; i < vector_.size(); ++i) \
         { \
             vector_[i].sumInPlace(scale * vectorIn[i]); \

core/include/jiminy/core/utilities/Random.h

@@ -267,10 +267,11 @@ namespace jiminy
 
     // ************ Random terrain generators ***************
 
-    heightmapFunctor_t randomTileGround(vector2_t const & tileSize,
-                                        int64_t   const & sparsity,
-                                        float64_t const & tileHeightMax,
-                                        vector2_t const & tileInterpDelta,
+    heightmapFunctor_t randomTileGround(vector2_t const & size,
+                                        float64_t const & heightMax,
+                                        vector2_t const & interpDelta,
+                                        uint32_t  const & sparsity,
+                                        float64_t const & orientation,
                                         uint32_t  const & seed);
 
     heightmapFunctor_t sumHeightmap(std::vector<heightmapFunctor_t> const & heightmaps);

core/src/utilities/Random.cc

@@ -819,33 +819,33 @@ namespace jiminy
     std::pair<float64_t, float64_t> tile2dInterp1d(Eigen::Matrix<int32_t, 2, 1> & posIdx,
                                                    vector2_t const & posRel,
                                                    uint32_t  const & dim,
-                                                   vector2_t const & tileSize,
+                                                   vector2_t const & size,
                                                    int64_t   const & sparsity,
-                                                   float64_t const & tileHeightMax,
-                                                   vector2_t const & tileInterpThreshold,
+                                                   float64_t const & heightMax,
+                                                   vector2_t const & interpThreshold,
                                                    uint32_t  const & seed)
     {
-        float64_t const z = randomDouble(posIdx, sparsity, tileHeightMax, seed);
+        float64_t const z = randomDouble(posIdx, sparsity, heightMax, seed);
         float64_t height, dheight;
-        if (posRel[dim] < tileInterpThreshold[dim])
+        if (posRel[dim] < interpThreshold[dim])
         {
             posIdx[dim] -= 1;
-            float64_t const z_m = randomDouble(posIdx, sparsity, tileHeightMax, seed);
+            float64_t const z_m = randomDouble(posIdx, sparsity, heightMax, seed);
             posIdx[dim] += 1;
 
-            float64_t const ratio = (1.0 - posRel[dim] / tileInterpThreshold[dim]) / 2.0;
+            float64_t const ratio = (1.0 - posRel[dim] / interpThreshold[dim]) / 2.0;
             height = z + (z_m - z) * ratio;
-            dheight = (z - z_m) / (2.0 * tileSize[dim] * tileInterpThreshold[dim]);
+            dheight = (z - z_m) / (2.0 * size[dim] * interpThreshold[dim]);
         }
-        else if (1.0 - posRel[dim] < tileInterpThreshold[dim])
+        else if (1.0 - posRel[dim] < interpThreshold[dim])
         {
             posIdx[dim] += 1;
-            float64_t const z_p = randomDouble(posIdx, sparsity, tileHeightMax, seed);
+            float64_t const z_p = randomDouble(posIdx, sparsity, heightMax, seed);
             posIdx[dim] -= 1;
 
-            float64_t const ratio = (1.0 + (posRel[dim] - 1.0) / tileInterpThreshold[dim]) / 2.0;
+            float64_t const ratio = (1.0 + (posRel[dim] - 1.0) / interpThreshold[dim]) / 2.0;
             height = z + (z_p - z) * ratio;
-            dheight = (z_p - z) / (2.0 * tileSize[dim] * tileInterpThreshold[dim]);
+            dheight = (z_p - z) / (2.0 * size[dim] * interpThreshold[dim]);
         }
         else
         {
@@ -856,100 +856,103 @@ namespace jiminy
         return {height, dheight};
     }
 
-    heightmapFunctor_t randomTileGround(vector2_t const & tileSize,
-                                        int64_t   const & sparsity,
-                                        float64_t const & tileHeightMax,
-                                        vector2_t const & tileInterpDelta,
+    heightmapFunctor_t randomTileGround(vector2_t const & size,
+                                        float64_t const & heightMax,
+                                        vector2_t const & interpDelta,
+                                        uint32_t  const & sparsity,
+                                        float64_t const & orientation,
                                         uint32_t  const & seed)
     {
-        if ((0.01 <= tileInterpDelta.array()).all()
-         && (tileInterpDelta.array() <= tileSize.array() / 2.0).all())
+        if ((0.01 <= interpDelta.array()).all()
+         && (interpDelta.array() <= size.array() / 2.0).all())
         {
-            PRINT_WARNING("'tileInterpDelta' must be in range [0.01, 'tileSize'/2.0].");
+            PRINT_WARNING("'interpDelta' must be in range [0.01, 'size'/2.0].");
         }
 
-        vector2_t tileInterpThreshold = tileInterpDelta.cwiseMax(0.01).cwiseMin(tileSize / 2.0);
-        tileInterpThreshold.array() /= tileSize.array();
+        vector2_t interpThreshold = interpDelta.cwiseMax(0.01).cwiseMin(size / 2.0);
+        interpThreshold.array() /= size.array();
 
-        vector2_t const tileOffset = vector2_t::NullaryExpr(
-            [&tileSize, &seed] (vectorN_t::Index const & i) -> float64_t
+        vector2_t const offset = vector2_t::NullaryExpr(
+            [&size, &seed] (vectorN_t::Index const & i) -> float64_t
             {
                 Eigen::Matrix<vectorN_t::Index, 1, 1> key;
                 key << i;
-                return randomDouble(key, 1, tileSize[i], seed);
+                return randomDouble(key, 1, size[i], seed);
             });
 
-        return [tileSize, tileOffset, sparsity, tileHeightMax, tileInterpThreshold, seed](
+        Eigen::Rotation2D<float64_t> rotationMat(orientation);
+
+        return [size, heightMax, interpDelta, rotationMat, sparsity, interpThreshold, offset, seed](
             vector3_t const & pos3) -> std::pair<float64_t, vector3_t>
         {
             // Compute the tile index and relative coordinate
-            vector2_t pos = pos3.head<2>() + tileOffset;
-            vector2_t posRel = pos.array() / tileSize.array();
+            vector2_t pos = rotationMat * (pos3.head<2>() + offset);
+            vector2_t posRel = pos.array() / size.array();
             Eigen::Matrix<int32_t, 2, 1> posIdx = posRel.array().floor().cast<int32_t>();
             posRel -= posIdx.cast<float64_t>();
 
             // Interpolate height based on nearby tiles if necessary
             Eigen::Matrix<bool_t, 2, 1> isEdge =
-                (posRel.array() < tileInterpThreshold.array()) ||
-                (1.0 - posRel.array() < tileInterpThreshold.array());
+                (posRel.array() < interpThreshold.array()) ||
+                (1.0 - posRel.array() < interpThreshold.array());
             float64_t height, dheight_x, dheight_y;
             if (isEdge[0] && !isEdge[1])
             {
                 auto result = tile2dInterp1d(
-                    posIdx, posRel, 0, tileSize, sparsity, tileHeightMax,
-                    tileInterpThreshold, seed);
+                    posIdx, posRel, 0, size, sparsity, heightMax,
+                    interpThreshold, seed);
                 height = std::get<0>(result);
                 dheight_x = std::get<1>(result);
                 dheight_y = 0.0;
             }
             else if (!isEdge[0] && isEdge[1])
             {
                 auto result = tile2dInterp1d(
-                    posIdx, posRel, 1, tileSize, sparsity, tileHeightMax,
-                    tileInterpThreshold, seed);
+                    posIdx, posRel, 1, size, sparsity, heightMax,
+                    interpThreshold, seed);
                 height = std::get<0>(result);
                 dheight_y = std::get<1>(result);
                 dheight_x = 0.0;
             }
             else if (isEdge[0] && isEdge[1])
             {
                 auto result_0 = tile2dInterp1d(
-                    posIdx, posRel, 0, tileSize, sparsity, tileHeightMax,
-                    tileInterpThreshold, seed);
+                    posIdx, posRel, 0, size, sparsity, heightMax,
+                    interpThreshold, seed);
                 float64_t height_0 = std::get<0>(result_0);
                 float64_t dheight_x_0 = std::get<1>(result_0);
-                if (posRel[1] < tileInterpThreshold[1])
+                if (posRel[1] < interpThreshold[1])
                 {
                     posIdx[1] -= 1;
                     auto result_m = tile2dInterp1d(
-                        posIdx, posRel, 0, tileSize, sparsity,
-                        tileHeightMax, tileInterpThreshold, seed);
+                        posIdx, posRel, 0, size, sparsity,
+                        heightMax, interpThreshold, seed);
                     float64_t height_m = std::get<0>(result_m);
                     float64_t dheight_x_m = std::get<1>(result_m);
 
-                    float64_t ratio = (1.0 - posRel[1] / tileInterpThreshold[1]) / 2.0;
+                    float64_t ratio = (1.0 - posRel[1] / interpThreshold[1]) / 2.0;
                     height = height_0 + (height_m - height_0) * ratio;
                     dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio;
-                    dheight_y = (height_0 - height_m) / (2.0 * tileSize[1] * tileInterpThreshold[1]);
+                    dheight_y = (height_0 - height_m) / (2.0 * size[1] * interpThreshold[1]);
                 }
                 else
                 {
                     posIdx[1] += 1;
                     auto result_p = tile2dInterp1d(
-                        posIdx, posRel, 0, tileSize, sparsity,
-                        tileHeightMax, tileInterpThreshold, seed);
+                        posIdx, posRel, 0, size, sparsity,
+                        heightMax, interpThreshold, seed);
                     float64_t height_p = std::get<0>(result_p);
                     float64_t dheight_x_p = std::get<1>(result_p);
 
-                    float64_t ratio = (1.0 + (posRel[1] - 1.0) / tileInterpThreshold[1]) / 2.0;
+                    float64_t ratio = (1.0 + (posRel[1] - 1.0) / interpThreshold[1]) / 2.0;
                     height = height_0 + (height_p - height_0) * ratio;
                     dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio;
-                    dheight_y = (height_p - height_0) / (2.0 * tileSize[1] * tileInterpThreshold[1]);
+                    dheight_y = (height_p - height_0) / (2.0 * size[1] * interpThreshold[1]);
                 }
             }
             else
             {
-                height = randomDouble(posIdx, sparsity, tileHeightMax, seed);
+                height = randomDouble(posIdx, sparsity, heightMax, seed);
                 dheight_x = 0.0;
                 dheight_y = 0.0;
             }

examples/python/box_uneven_ground_impulse_contact.py

@@ -11,11 +11,12 @@
 TILE_SIZE = [np.array([4.0, 4.0]),
              np.array([100.0, 0.05]),
              np.array([0.05, 100.0])]
-TILE_SPARSITY = [1, 8, 8]
 TILE_HEIGHT_MAX = [1.0, 0.05, 0.05]
 TILE_INTERP_DELTA = [np.array([0.5, 1.0]),
                      np.array([0.01, 0.01]),
                      np.array([0.01, 0.01])]
+TILE_SPARSITY = [1, 8, 8]
+TILE_ORIENTATION = [0.0, np.pi / 4.0, 0.0]
 TILE_SEED = [np.random.randint(0, 2 ** 32, dtype=np.uint32) for _ in range(3)]
 
 
@@ -31,7 +32,8 @@
 
 # Generate random ground profile
 ground_params = list(starmap(random_tile_ground, zip(
-    TILE_SIZE, TILE_SPARSITY, TILE_HEIGHT_MAX, TILE_INTERP_DELTA, TILE_SEED)))
+    TILE_SIZE, TILE_HEIGHT_MAX, TILE_INTERP_DELTA, TILE_SPARSITY,
+    TILE_ORIENTATION, TILE_SEED)))
 engine_options["world"]["groundProfile"] = sum_heightmap([
     ground_params[0], merge_heightmap(ground_params[1:])])
 env.engine.set_options(engine_options)

python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py

@@ -986,6 +986,11 @@ def play_interactive(env: Union["BaseJiminyEnv", gym.Wrapper],
         obs = env.reset()
         reward = None
 
+        # Refresh the ground profile
+        engine_options = self.engine.get_options()
+        ground_profile = engine_options["world"]["groundProfile"]
+        self.viewer.update_floor(ground_profile, show_meshes=False)
+
         # Enable travelling
         if enable_travelling is None:
             enable_travelling = \

python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py

@@ -334,7 +334,8 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor:
             model, dist_class, action_prev_logits)
 
     # Compute the mean action corresponding to the noisy observation
-    if policy.config["caps_global_reg"] > 0.0:
+    if policy.config["caps_global_reg"] > 0.0 or \
+            not policy.config["enable_adversarial_noise"]:
         action_noisy_logits = action_logits["noisy"]
         action_noisy_mean = get_action_mean(
             model, dist_class, action_noisy_logits)

python/jiminy_py/src/jiminy_py/simulator.py

@@ -324,9 +324,11 @@ def seed(self, seed: int) -> None:
         # Make sure no simulation is running before setting the seed
         self.engine.stop()
 
-        # Set the seed through the engine instead of using
-        # `jiminy.reset_random_generator` to keep track of the seed in options,
-        # and thereby to log it in the telemetry as constant.
+        # Force to reset the seed of the low-level engine
+        jiminy.reset_random_generator(seed)
+
+        # Set the seed in engine options to keep track of the seed and log it
+        # automatically in the telemetry as constant.
         engine_options = self.engine.get_options()
         engine_options["stepper"]["randomSeed"] = \
             np.array(seed, dtype=np.dtype('uint32'))

python/jiminy_py/src/jiminy_py/viewer/viewer.py

@@ -986,7 +986,11 @@ def close(self=None) -> None:
                     if Viewer.backend == 'meshcat':
                         recorder_proc = Viewer._backend_obj.recorder.proc
                         _ProcessWrapper(recorder_proc).kill()
+                    if Viewer.backend == 'panda3d':
+                        Viewer._backend_obj._app.stop()
+                        Viewer._backend_proc.wait(timeout=0.1)
                     Viewer._backend_proc.kill()
+                atexit.unregister(Viewer.close)
                 Viewer._backend_obj = None
                 Viewer._backend_proc = None
                 Viewer._has_gui = False
@@ -1288,6 +1292,10 @@ def _gepetto_client_connect(get_proc_info=False):
         Viewer._backend_obj = client
         Viewer._backend_proc = proc
 
+        # Make sure to close cleanly the viewer at exit
+        if close_at_exit:
+            atexit.register(Viewer.close)
+
         # Make sure the backend process is alive
         assert Viewer.is_alive(), (
             "Something went wrong. Impossible to instantiate viewer backend.")

python/jiminy_pywrap/include/jiminy/python/Utilities.h

@@ -62,7 +62,7 @@ namespace python
            consists in adding the expected tags as function doc. It works for now but
            it is not really reliable and may break in the future too. */
         bp::converter::registration const * r = bp::converter::registry::query(typeid(WrappedClassT));
-        assert(r && ("Class " + typeid(WrappedClassT).name() + " not registered to Boost Python."));
+        assert((std::string("Class ") + typeid(WrappedClassT).name() + " not registered to Boost Python.", r != nullptr));
         PyTypeObject * nsPtr = r->get_class_object();
         bp::object nsName(bp::handle<>(PyObject_GetAttrString(reinterpret_cast<PyObject *>(nsPtr), "__name__")));
         bp::objects::function * funcPtr = bp::downcast<bp::objects::function>(func.ptr());