Jiminy 1.7.1 (#415)

* [core] Rename 'HeatMap' in 'HeightMap'. * [core] Write terrain generation tools in C++ for efficiency. * [core] Add tools to merge and sum heightmaps. * [core] Enable to re-initialize controllers. * [core/python] It is now possible to extract raw python function from 'HeightMapFunctor'. * [python/viewer] Faster ground profile rendering. * [python/viewer] Do not render ground profile if original flat ground. * [gym/common] Add 'BaseJiminyEnv.register_variable' method easily log variables. * [gym/common] Generic structured space handling by relying on 'dm-tree' package. * [gym/common] Support any observation space in gym pipelines. * [gym/rllib] Use adversarial state perturbation sampling for spatial regularization. (#414) * [misc] Fix CI not pushing gym_jiminy wheels on pypi.
duburcqa · Oct 16, 2021 · 2da152f · 2da152f
1 parent 792278b
commit 2da152f
Show file tree

Hide file tree

Showing 57 changed files with 1,270 additions and 868 deletions.
diff --git a/.github/workflows/linux.yml b/.github/workflows/linux.yml
@@ -90,9 +90,9 @@ jobs:
 
         "$InstallDir/bin/jiminy_double_pendulum"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
-        cmake "$RootDir/examples/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
+        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" \
                -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_EXECUTABLE="${PYTHON_EXECUTABLE}" \
                -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
         make install

diff --git a/.github/workflows/manylinux.yml b/.github/workflows/manylinux.yml
@@ -136,7 +136,7 @@ jobs:
         packages_dir: build/wheelhouse
     - name: Publish on PyPi the wheel of Gym Jiminy (Any platform / Any python3 version)
       if: >-
-        success() && matrix.container == 'manylinux2010_x86_64' && matrix.PYTHON_VERSION == 'cp36' && matrix.legacy == false &&
+        success() && matrix.container == 'quay.io/pypa/manylinux2010_x86_64' && matrix.PYTHON_VERSION == 'cp36' && matrix.legacy == false &&
         github.repository == 'duburcqa/jiminy' && github.event_name == 'push' && github.ref == 'refs/heads/master'
       uses: pypa/gh-action-pypi-publish@master
       with:

diff --git a/.github/workflows/ubuntu.yml b/.github/workflows/ubuntu.yml
@@ -82,9 +82,9 @@ jobs:
       run: |
         "$InstallDir/bin/jiminy_double_pendulum"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
-        cmake "$RootDir/examples/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" \
+        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}"
         make install
 

diff --git a/.github/workflows/win.yml b/.github/workflows/win.yml
@@ -136,13 +136,13 @@ jobs:
 
         & "$InstallDir/bin/jiminy_double_pendulum.exe"
 
-        mkdir -p "$RootDir/examples/pip_extension/build"
-        cd "$RootDir/examples/pip_extension/build"
+        mkdir -p "$RootDir/examples/cpp/pip_extension/build"
+        cd "$RootDir/examples/cpp/pip_extension/build"
 
         $JIMINY_LIB_DIR = (python -c "import os, jiminy_py ; print(os.path.dirname(jiminy_py.get_libraries()))")
         $env:Path += ";$JIMINY_LIB_DIR"
 
-        cmake "$RootDir/examples/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" `
+        cmake "$RootDir/examples/cpp/pip_extension" -DCMAKE_INSTALL_PREFIX="$InstallDir" `
               -DCMAKE_PREFIX_PATH="$InstallDir" -DPYTHON_REQUIRED_VERSION="${{ matrix.python-version }}" `
               -DCMAKE_BUILD_TYPE="${BUILD_TYPE}"
         cmake --build . --target INSTALL --config "${env:BUILD_TYPE}"

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.7.0)
+set(BUILD_VERSION 1.7.1)
 
 # Set compatibility
 if(CMAKE_VERSION VERSION_GREATER "3.11.0")

diff --git a/core/include/jiminy/core/Types.h b/core/include/jiminy/core/Types.h
@@ -69,6 +69,7 @@ namespace jiminy
     using matrix3_t = Eigen::Matrix<float64_t, 3, 3>;
     using matrix6N_t = Eigen::Matrix<float64_t, 6, Eigen::Dynamic>;
     using vectorN_t = Eigen::Matrix<float64_t, Eigen::Dynamic, 1>;
+    using vector2_t = Eigen::Matrix<float64_t, 2, 1>;
     using vector3_t = Eigen::Matrix<float64_t, 3, 1>;
     using vector6_t = Eigen::Matrix<float64_t, 6, 1>;
 
@@ -120,7 +121,7 @@ namespace jiminy
 
     /* Ground profile signature.
        Note that it is impossible to use function pointer since it does not support functors. */
-    using heatMapFunctor_t = std::function<std::pair<float64_t, vector3_t>(vector3_t const & /*pos*/)>;
+    using heightmapFunctor_t = std::function<std::pair<float64_t, vector3_t>(vector3_t const & /*pos*/)>;
 
     // Flexible joints
     struct flexibleJointData_t
@@ -159,7 +160,7 @@ namespace jiminy
 
     // Configuration/option holder
     using configField_t = boost::make_recursive_variant<
-        bool_t, uint32_t, int32_t, float64_t, std::string, vectorN_t, matrixN_t, heatMapFunctor_t,
+        bool_t, uint32_t, int32_t, float64_t, std::string, vectorN_t, matrixN_t, heightmapFunctor_t,
         std::vector<std::string>, std::vector<vectorN_t>, std::vector<matrixN_t>,
         flexibilityConfig_t, std::unordered_map<std::string, boost::recursive_variant_>
     >::type;

diff --git a/core/include/jiminy/core/constraints/SphereConstraint.h b/core/include/jiminy/core/constraints/SphereConstraint.h
@@ -43,7 +43,7 @@ namespace jiminy
         ///////////////////////////////////////////////////////////////////////////////////////////////
         SphereConstraint(std::string const & frameName,
                          float64_t   const & sphereRadius,
-                         vector3_t   const & groundNormal = (vector3_t() << 0.0, 0.0, 1.0).finished());
+                         vector3_t   const & groundNormal = vector3_t::UnitZ());
         virtual ~SphereConstraint(void);
 
         std::string const & getFrameName(void) const;

diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h
@@ -141,10 +141,10 @@ namespace jiminy
         {
             configHolder_t config;
             config["gravity"] = (vectorN_t(6) << 0.0, 0.0, -9.81, 0.0, 0.0, 0.0).finished();
-            config["groundProfile"] = heatMapFunctor_t(
-                [](vector3_t const & /* pos */) -> std::pair <float64_t, vector3_t>
+            config["groundProfile"] = heightmapFunctor_t(
+                [](vector3_t const & /* pos */) -> std::pair<float64_t, vector3_t>
                 {
-                    return {0.0, (vector3_t() << 0.0, 0.0, 1.0).finished()};
+                    return {0.0, vector3_t::UnitZ()};
                 });
 
             return config;
@@ -241,11 +241,11 @@ namespace jiminy
         struct worldOptions_t
         {
             vectorN_t const gravity;
-            heatMapFunctor_t const groundProfile;
+            heightmapFunctor_t const groundProfile;
 
             worldOptions_t(configHolder_t const & options) :
             gravity(boost::get<vectorN_t>(options.at("gravity"))),
-            groundProfile(boost::get<heatMapFunctor_t>(options.at("groundProfile")))
+            groundProfile(boost::get<heightmapFunctor_t>(options.at("groundProfile")))
             {
                 // Empty.
             }

diff --git a/core/include/jiminy/core/robot/AbstractMotor.h b/core/include/jiminy/core/robot/AbstractMotor.h
@@ -97,9 +97,6 @@ namespace jiminy
         AbstractMotorBase(AbstractMotorBase const & abstractMotor) = delete;
         AbstractMotorBase & operator = (AbstractMotorBase const & other) = delete;
 
-        auto shared_from_this() { return shared_from(this); }
-        auto shared_from_this() const { return shared_from(this); }
-
         ///////////////////////////////////////////////////////////////////////////////////////////////
         /// \brief      Constructor
         ///

diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h
@@ -251,9 +251,6 @@ namespace jiminy
         Model(void);
         ~Model(void) = default;
 
-        auto shared_from_this() { return shared_from(this); }
-        auto shared_from_this() const { return shared_from(this); }
-
         hresult_t initialize(std::string              const & urdfPath,
                              bool_t                   const & hasFreeflyer = true,
                              std::vector<std::string> const & meshPackageDirs = {});

diff --git a/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h b/core/include/jiminy/core/stepper/RungeKuttaDOPRIStepper.h
@@ -68,9 +68,9 @@ namespace jiminy
             /// \param[in] solution Current solution computed by the main Runge-Kutta step.
             /// \param[in, out] dt  Timestep to be scaled.
             /// \return True on step success, false otherwise. dt is updated in place.
-            virtual bool adjustStep(state_t   const & initialState,
-                                    state_t   const & solution,
-                                    float64_t       & dt) override final;
+            virtual bool_t adjustStep(state_t   const & initialState,
+                                      state_t   const & solution,
+                                      float64_t       & dt) override final;
 
         private:
             /// \brief Run error computation algorithm to return normalized error.

diff --git a/core/include/jiminy/core/utilities/Json.tpp b/core/include/jiminy/core/utilities/Json.tpp
@@ -25,7 +25,7 @@ namespace jiminy
     Json::Value convertToJson<flexibleJointData_t>(flexibleJointData_t const & value);
 
     template<>
-    Json::Value convertToJson<heatMapFunctor_t>(heatMapFunctor_t const & value);
+    Json::Value convertToJson<heightmapFunctor_t>(heightmapFunctor_t const & value);
 
     template<typename T, typename A>
     constexpr std::enable_if_t<!std::is_same<T, vectorN_t>::value
@@ -107,7 +107,7 @@ namespace jiminy
     flexibleJointData_t convertFromJson<flexibleJointData_t>(Json::Value const & value);
 
     template<>
-    heatMapFunctor_t convertFromJson<heatMapFunctor_t>(Json::Value const & value);
+    heightmapFunctor_t convertFromJson<heightmapFunctor_t>(Json::Value const & value);
 
     template<typename T>
     std::enable_if_t<is_vector_v<T>, T>

diff --git a/core/include/jiminy/core/utilities/Random.h b/core/include/jiminy/core/utilities/Random.h
@@ -33,6 +33,8 @@ namespace jiminy
 
     void shuffleIndices(std::vector<uint32_t> & vector);
 
+    // ************ Continuous 1D Perlin processes ***************
+
     class PeriodicGaussianProcess
     {
     public:
@@ -185,11 +187,11 @@ namespace jiminy
     /* \brief  Sum of Perlin noise octaves.
 
         \details  The original implementation uses fixed size permutation table to generate
-                    random gradient directions. As a result, the generated process is inherently
-                    periodic, which must be avoided. To circumvent this limitation, MurmurHash3
-                    algorithm is used to get random gradients at every point in time, without any
-                    periodicity, but deterministically for a given seed. It is computationally more
-                    depending but not critically slower.
+                  random gradient directions. As a result, the generated process is inherently
+                  periodic, which must be avoided. To circumvent this limitation, MurmurHash3
+                  algorithm is used to get random gradients at every point in time, without any
+                  periodicity, but deterministically for a given seed. It is computationally more
+                  depending but not critically slower.
 
         /sa  For technical references:
             - https://www.scratchapixel.com/lessons/procedural-generation-virtual-worlds/perlin-noise-part-2
@@ -262,6 +264,21 @@ namespace jiminy
     private:
         float64_t const period_;
     };
+
+    // ************ Random terrain generators ***************
+
+    heightmapFunctor_t randomTileGround(vector2_t const & tileSize,
+                                        int64_t   const & sparsity,
+                                        float64_t const & tileHeightMax,
+                                        vector2_t const & tileInterpDelta,
+                                        uint32_t  const & seed);
+
+    heightmapFunctor_t sumHeightmap(std::vector<heightmapFunctor_t> const & heightmaps);
+    heightmapFunctor_t mergeHeightmap(std::vector<heightmapFunctor_t> const & heightmaps);
+
+    matrixN_t discretizeHeightmap(heightmapFunctor_t const & heightmap,
+                                  float64_t          const & gridSize,
+                                  float64_t          const & gridUnit);
 }
 
 #endif  // JIMINY_RANDOM_H
diff --git a/core/src/control/AbstractController.cc b/core/src/control/AbstractController.cc
@@ -47,13 +47,15 @@ namespace jiminy
         // Backup robot
         robot_ = robotIn;
 
+        /* Set initialization flag to true temporarily to enable calling
+           'reset', 'computeCommand' and 'internalDynamics' methods. */
+        isInitialized_ = true;
+
         // Reset the controller completely
-        reset(true);
+        reset(true);  // It cannot fail at this point
 
         try
         {
-            // isInitialized_ must be true to execute the 'computeCommand' and 'internalDynamics' methods
-            isInitialized_ = true;
             float64_t t = 0.0;
             vectorN_t q = pinocchio::neutral(robot->pncModel_);
             vectorN_t v = vectorN_t::Zero(robot->nv());
@@ -80,6 +82,8 @@ namespace jiminy
         catch (std::exception const & e)
         {
             isInitialized_ = false;
+            robot_.reset();
+            sensorsData_.clear();
             PRINT_ERROR("Something is wrong, probably because of 'commandFct'.\n"
                         "Raised from exception: ", e.what());
             return hresult_t::ERROR_GENERIC;
@@ -90,6 +94,12 @@ namespace jiminy
 
     hresult_t AbstractController::reset(bool_t const & resetDynamicTelemetry)
     {
+        if (!isInitialized_)
+        {
+            PRINT_ERROR("The controller is not initialized.");
+            return hresult_t::ERROR_INIT_FAILED;
+        }
+
         // Reset the telemetry buffer of dynamically registered quantities
         if (resetDynamicTelemetry)
         {

diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -1843,7 +1843,7 @@ namespace jiminy
                */
             if (!std::isfinite(stepperUpdatePeriod_) || !engineOptions_->stepper.logInternalStepperSteps)
             {
-                bool mustUpdateTelemetry = !std::isfinite(stepperUpdatePeriod_);
+                bool_t mustUpdateTelemetry = !std::isfinite(stepperUpdatePeriod_);
                 if (!mustUpdateTelemetry)
                 {
                     float64_t dtNextStepperUpdatePeriod = stepperUpdatePeriod_ - std::fmod(t, stepperUpdatePeriod_);
@@ -2107,7 +2107,7 @@ namespace jiminy
 
             // Update sensors data if necessary, namely if time-continuous or breakpoint
             float64_t const & sensorsUpdatePeriod = engineOptions_->stepper.sensorsUpdatePeriod;
-            bool mustUpdateSensors = sensorsUpdatePeriod < EPS;
+            bool_t mustUpdateSensors = sensorsUpdatePeriod < EPS;
             float64_t dtNextSensorsUpdatePeriod = sensorsUpdatePeriod - std::fmod(t, sensorsUpdatePeriod);
             if (!mustUpdateSensors)
             {

diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc
@@ -279,7 +279,7 @@ namespace jiminy
                 // Its parent frame and parent joint are the universe. It is aligned with world frame,
                 // and the top face is the actual ground surface.
                 pinocchio::SE3 groundPose = pinocchio::SE3::Identity();
-                groundPose.translation() = (vector3_t() << 0.0, 0.0, -1.0).finished();
+                groundPose.translation() = - vector3_t::UnitZ();
                 pinocchio::GeometryObject groundPlane("ground", 0, 0, groudBox, groundPose);
 
                 // Add the ground plane pinocchio to the robot model

diff --git a/core/src/utilities/Json.cc b/core/src/utilities/Json.cc
@@ -58,7 +58,7 @@ namespace jiminy
     }
 
     template<>
-    Json::Value convertToJson<heatMapFunctor_t>(heatMapFunctor_t const & /* value */)
+    Json::Value convertToJson<heightmapFunctor_t>(heightmapFunctor_t const & /* value */)
     {
         return {"not supported"};
     }
@@ -201,13 +201,13 @@ namespace jiminy
     }
 
     template<>
-    heatMapFunctor_t convertFromJson<heatMapFunctor_t>(Json::Value const & /* value */)
+    heightmapFunctor_t convertFromJson<heightmapFunctor_t>(Json::Value const & /* value */)
     {
         return {
-            heatMapFunctor_t(
-                [](vector3_t const & /* pos */) -> std::pair <float64_t, vector3_t>
+            heightmapFunctor_t(
+                [](vector3_t const & /* pos */) -> std::pair<float64_t, vector3_t>
                 {
-                    return {0.0, (vector3_t() << 0.0, 0.0, 1.0).finished()};
+                    return {0.0, vector3_t::UnitZ()};
                 })
         };
     }