Repository: duburcqa/jiminy (language: python)

Issue:
[core] Random number generator local to Engine instead of global



Discussion / hints:

Fix patch:
diff --git a/.pylintrc b/.pylintrc
index 05277d067..f6f9034f2 100644
--- a/.pylintrc
+++ b/.pylintrc
@@ -74,7 +74,9 @@ disable =
     cyclic-import
 
 # Enable the message, report, category or checker with the given id(s)
-enable = c-extension-no-member
+enable =
+    c-extension-no-member,
+    useless-suppression
 
 [tool.pylint.miscellaneous]
 # List of note tags to take in consideration, separated by a comma.
diff --git a/core/CMakeLists.txt b/core/CMakeLists.txt
index 776bdbdd3..56423895d 100644
--- a/core/CMakeLists.txt
+++ b/core/CMakeLists.txt
@@ -31,6 +31,7 @@ set(SRC
     "${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/helpers.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/pinocchio.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/json.cc"
+    "${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/geometry.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/utilities/random.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/io/abstract_io_device.cc"
     "${CMAKE_CURRENT_SOURCE_DIR}/src/io/memory_device.cc"
diff --git a/core/examples/double_pendulum/double_pendulum.cc b/core/examples/double_pendulum/double_pendulum.cc
index 378cff4b3..fe16d10dd 100644
--- a/core/examples/double_pendulum/double_pendulum.cc
+++ b/core/examples/double_pendulum/double_pendulum.cc
@@ -60,8 +60,6 @@ int main(int /* argc */, char * /* argv */[])
     // Instantiate timer
     Timer timer;
 
-    timer.tic();
-
     // Instantiate and configuration the robot
     std::vector<std::string> motorJointNames{"SecondPendulumJoint"};
 
@@ -110,7 +108,8 @@ int main(int /* argc */, char * /* argv */[])
     boost::get<double>(stepperOptions.at("sensorsUpdatePeriod")) = 1.0e-3;
     boost::get<double>(stepperOptions.at("controllerUpdatePeriod")) = 1.0e-3;
     boost::get<bool>(stepperOptions.at("logInternalStepperSteps")) = false;
-    boost::get<uint32_t>(stepperOptions.at("randomSeed")) = 0U;  // `time(nullptr)` for random seed
+    boost::get<VectorX<uint32_t>>(stepperOptions.at("randomSeedSeq")) =
+        VectorX<uint32_t>::Zero(1);  // `time(nullptr)` for random seed
     GenericConfig & contactsOptions = boost::get<GenericConfig>(simuOptions.at("contacts"));
     boost::get<std::string>(contactsOptions.at("model")) = std::string("spring_damper");
     boost::get<double>(contactsOptions.at("stiffness")) = 1.0e6;
@@ -120,8 +119,7 @@ int main(int /* argc */, char * /* argv */[])
     boost::get<double>(contactsOptions.at("transitionVelocity")) = 0.01;
     engine->setOptions(simuOptions);
     engine->initialize(robot, controller, callback);
-
-    timer.toc();
+    std::cout << "Initialization: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     // =====================================================================
     // ======================= Run the simulation ==========================
@@ -136,8 +134,7 @@ int main(int /* argc */, char * /* argv */[])
     // Run simulation
     timer.tic();
     engine->simulate(tf, q0, v0);
-    timer.toc();
-    std::cout << "Simulation time: " << (timer.dt * 1.0e3) << "ms" << std::endl;
+    std::cout << "Simulation: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     // Write the log file
     std::vector<std::string> fieldnames;
@@ -147,12 +144,10 @@ int main(int /* argc */, char * /* argv */[])
     std::cout << engine->getStepperState().iter << " internal integration steps" << std::endl;
     timer.tic();
     engine->writeLog((outputDirPath / "log.data").string(), "binary");
-    timer.toc();
-    std::cout << "Write log binary: " << (timer.dt * 1.0e3) << "ms" << std::endl;
+    std::cout << "Write log binary: " << timer.toc<std::milli>() << "ms" << std::endl;
     timer.tic();
     engine->writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
-    timer.toc();
-    std::cout << "Write log HDF5: " << (timer.dt * 1.0e3) << "ms" << std::endl;
+    std::cout << "Write log HDF5: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     return 0;
 }
diff --git a/core/examples/external_project/double_pendulum.cc b/core/examples/external_project/double_pendulum.cc
index 776fb51f5..55f533181 100644
--- a/core/examples/external_project/double_pendulum.cc
+++ b/core/examples/external_project/double_pendulum.cc
@@ -60,8 +60,6 @@ int main(int argc, char * argv[])
     // Instantiate timer
     Timer timer;
 
-    timer.tic();
-
     // Instantiate and configuration the robot
     std::vector<std::string> motorJointNames{"SecondPendulumJoint"};
     auto robot = std::make_shared<Robot>();
@@ -87,8 +85,7 @@ int main(int argc, char * argv[])
     // Instantiate the engine
     auto engine = std::make_shared<Engine>();
     engine->initialize(robot, controller, callback);
-
-    timer.toc();
+    std::cout << "Initialization: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     // =====================================================================
     // ======================= Run the simulation ==========================
@@ -103,8 +100,7 @@ int main(int argc, char * argv[])
     // Run simulation
     timer.tic();
     engine->simulate(tf, q0, v0);
-    timer.toc();
-    std::cout << "Simulation time: " << (timer.dt * 1.0e3) << "ms" << std::endl;
+    std::cout << "Simulation: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     // Write the log file
     std::vector<std::string> fieldnames;
@@ -114,12 +110,9 @@ int main(int argc, char * argv[])
     std::cout << engine->getStepperState().iter << " internal integration steps" << std::endl;
     timer.tic();
     engine->writeLog((outputDirPath / "log.data").string(), "binary");
-    timer.toc();
-    std::cout << "Write log binary: " << (timer.dt * 1.0e3) << "ms" << std::endl;
-    timer.tic();
+    std::cout << "Write log binary: " << timer.toc<std::milli>() << "ms" << std::endl;
     engine->writeLog((outputDirPath / "log.hdf5").string(), "hdf5");
-    timer.toc();
-    std::cout << "Write log HDF5: " << (timer.dt * 1.0e3) << "ms" << std::endl;
+    std::cout << "Write log HDF5: " << timer.toc<std::milli>() << "ms" << std::endl;
 
     return 0;
 }
diff --git a/core/include/jiminy/core/constants.h b/core/include/jiminy/core/constants.h
index f0639dac2..4ebb112bc 100644
--- a/core/include/jiminy/core/constants.h
+++ b/core/include/jiminy/core/constants.h
@@ -2,7 +2,7 @@
 #define JIMINY_CONSTANTS_H
 
 #include <string_view>  // `std::string_view`
-
+#include <limits>       // `std::numeric_limits`
 
 namespace jiminy
 {
diff --git a/core/include/jiminy/core/constraints/frame_constraint.h b/core/include/jiminy/core/constraints/frame_constraint.h
index a2e2ad8b1..1818a9d8d 100644
--- a/core/include/jiminy/core/constraints/frame_constraint.h
+++ b/core/include/jiminy/core/constraints/frame_constraint.h
@@ -63,6 +63,6 @@ namespace jiminy
         /// \brief Stores full frame drift in reference frame.
         pinocchio::Motion frameDrift_{};
     };
-}  // namespace jiminy
+}
 
 #endif  // end of JIMINY_ABSTRACT_MOTOR_H
diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h
index b9c80ae38..382ba08bf 100644
--- a/core/include/jiminy/core/control/abstract_controller.h
+++ b/core/include/jiminy/core/control/abstract_controller.h
@@ -85,10 +85,12 @@ namespace jiminy
         /// \return Return code to determine whether the execution of the method was successful.
         hresult_t registerVariable(
             const std::vector<std::string> & fieldnames,
-            const Eigen::Ref<Eigen::Matrix<double, -1, 1>, 0, Eigen::InnerStride<>> & values);
+            const Eigen::Ref<VectorX<double>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
+                values);
         hresult_t registerVariable(
             const std::vector<std::string> & fieldnames,
-            const Eigen::Ref<Eigen::Matrix<int64_t, -1, 1>, 0, Eigen::InnerStride<>> & values);
+            const Eigen::Ref<VectorX<int64_t>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
+                values);
 
         /// \brief Register a constant float64 to the telemetry.
         ///
diff --git a/core/include/jiminy/core/engine/engine_multi_robot.h b/core/include/jiminy/core/engine/engine_multi_robot.h
index 56a44286e..26a125e97 100644
--- a/core/include/jiminy/core/engine/engine_multi_robot.h
+++ b/core/include/jiminy/core/engine/engine_multi_robot.h
@@ -5,6 +5,8 @@
 #include <functional>
 
 #include "jiminy/core/fwd.h"
+#include "jiminy/core/utilities/helpers.h"  // `Timer`
+#include "jiminy/core/utilities/random.h"   // `PCG32`
 #include "jiminy/core/engine/system.h"
 
 
@@ -35,7 +37,6 @@ namespace jiminy
 
     const std::set<std::string> STEPPERS{"euler_explicit", "runge_kutta_4", "runge_kutta_dopri5"};
 
-    class Timer;
     class Robot;
     class AbstractConstraintBase;
     class AbstractController;
@@ -128,8 +129,12 @@ namespace jiminy
             GenericConfig config;
             config["gravity"] = (Eigen::VectorXd(6) << 0.0, 0.0, -9.81, 0.0, 0.0, 0.0).finished();
             config["groundProfile"] = HeightmapFunctor(
-                [](const Eigen::Vector3d & /* pos */) -> std::pair<double, Eigen::Vector3d> {
-                    return {0.0, Eigen::Vector3d::UnitZ()};
+                [](const Eigen::Vector2d & /* xy */,
+                   double & height,
+                   Eigen::Vector3d & normal) -> void
+                {
+                    height = 0.0;
+                    normal = Eigen::Vector3d::UnitZ();
                 });
 
             return config;
@@ -139,7 +144,7 @@ namespace jiminy
         {
             GenericConfig config;
             config["verbose"] = false;
-            config["randomSeed"] = 0U;
+            config["randomSeedSeq"] = VectorX<uint32_t>::Zero(1).eval();
             /// \details Must be either "runge_kutta_dopri5", "runge_kutta_4" or "euler_explicit".
             config["odeSolver"] = std::string{"runge_kutta_dopri5"};
             config["tolAbs"] = 1.0e-5;
@@ -248,7 +253,7 @@ namespace jiminy
         struct stepperOptions_t
         {
             const bool verbose;
-            const uint32_t randomSeed;
+            const VectorX<uint32_t> randomSeedSeq;
             const std::string odeSolver;
             const double tolAbs;
             const double tolRel;
@@ -263,7 +268,7 @@ namespace jiminy
 
             stepperOptions_t(const GenericConfig & options) :
             verbose{boost::get<bool>(options.at("verbose"))},
-            randomSeed{boost::get<uint32_t>(options.at("randomSeed"))},
+            randomSeedSeq{boost::get<VectorX<uint32_t>>(options.at("randomSeedSeq"))},
             odeSolver{boost::get<std::string>(options.at("odeSolver"))},
             tolAbs{boost::get<double>(options.at("tolAbs"))},
             tolRel{boost::get<double>(options.at("tolRel"))},
@@ -661,9 +666,10 @@ namespace jiminy
         bool isTelemetryConfigured_{false};
         bool isSimulationRunning_{false};
         GenericConfig engineOptionsHolder_{};
+        PCG32 generator_;
 
     private:
-        std::unique_ptr<Timer> timer_{std::make_unique<Timer>()};
+        Timer timer_{};
         contactModel_t contactModel_{contactModel_t::UNSUPPORTED};
         std::unique_ptr<TelemetrySender> telemetrySender_;
         std::shared_ptr<TelemetryData> telemetryData_;
diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h
index d3781e2b4..ee0f8d574 100644
--- a/core/include/jiminy/core/fwd.h
+++ b/core/include/jiminy/core/fwd.h
@@ -3,7 +3,7 @@
 
 #include <string_view>    // `std::string_view`
 #include <cstdint>        // `int32_t`, `int64_t`, `uint32_t`, `uint64_t`, ...
-#include <functional>     // `std::function`
+#include <functional>     // `std::function`, `std::invoke`
 #include <limits>         // `std::numeric_limits`
 #include <map>            // `std::map`
 #include <string>         // `std::string`
@@ -11,7 +11,10 @@
 #include <unordered_map>  // `std::unordered_map`
 #include <utility>        // `std::pair`
 #include <vector>         // `std::vector`
+#include <memory>         // `std::addressof`
+#include <utility>        // `std::forward`
 #include <stdexcept>      // `std::runtime_error`, `std::logic_error`
+#include <type_traits>  // `std::enable_if_t`, `std::decay_t`, `std::add_pointer_t`, `std::is_same_v`, ...
 
 #include "pinocchio/fwd.hpp"            // To avoid having to include it everywhere
 #include "pinocchio/multibody/fwd.hpp"  // `pinocchio::JointIndex`, `pinocchio::FrameIndex`, ...
@@ -20,7 +23,7 @@
 #include "pinocchio/spatial/force.hpp"             // `pinocchio::Force`
 #include "pinocchio/spatial/motion.hpp"            // `pinocchio::Motion`
 
-#include <Eigen/Core>  // `Eigen::Matrix`, `Eigen::Dynamic`, `Eigen::IOFormat`, `Eigen::FullPrecision`
+#include <Eigen/Core>  // `Eigen::Matrix`, `Eigen::Dynamic`, `Eigen::IOFormat`, `Eigen::FullPrecision`, ...
 #include <Eigen/StdVector>    // `Eigen::aligned_allocator`
 #include <boost/variant.hpp>  // `boost::make_recursive_variant`
 
@@ -42,9 +45,6 @@ namespace jiminy
     template<typename Scalar>
     using MatrixX = Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>;
 
-    template<typename Scalar>
-    using Matrix3X = Eigen::Matrix<Scalar, 3, Eigen::Dynamic>;
-
     using Matrix6Xd = Eigen::Matrix<double, 6, Eigen::Dynamic>;
 
 #define EIGEN_MAKE_FIXED_TYPEDEFS(Size) \
@@ -90,6 +90,76 @@ namespace jiminy
         FREE = 7
     };
 
+    // ******************************** Cpp features backporting ******************************* //
+
+    template<typename F>
+    class function_ref;
+
+    /// \brief Backport `std::function_ref`, a lightweight non-owning reference to a callable.
+    ///
+    /// \details Initially, `std::function_ref` that was supposed to be part of C++23 but has been
+    ///          postponed to C++26. For reference, see proposal standards paper P0792R13:
+    ///          https://www.open-std.org/jtc1/sc22/wg21/docs/papers/2023/p0792r13.html
+    ///
+    /// \warning As prescribed by the proposal paper, the exposed 'operator()' is unconditionally
+    ///          const-qualified, regardless of the constness of the original callable. Whether the
+    ///          const or non-const original method will be called depends on the constness of the
+    ///          object passed to the constructor.
+    ///
+    /// \sa The proposed implementation is heavily inspired from Simon (Sy) Brand and Zhihao Yuan:
+    ///     https://github.com/TartanLlama/function_ref
+    ///     https://github.com/zhihaoy/nontype_functional/tree/p0792r13
+    template<typename R, typename... Args>
+    class function_ref<R(Args...)>
+    {
+    public:
+        constexpr function_ref(const function_ref<R(Args...)> & rhs) noexcept = default;
+
+        template<typename F,
+                 typename = std::enable_if_t<std::is_function_v<F> &&
+                                             std::is_invocable_r_v<R, F, Args...>>>
+        constexpr function_ref(F * f) noexcept :
+        obj_{const_cast<void *>(reinterpret_cast<const void *>(f))},
+        callback_{[](void * obj, Args... args) -> R
+                  {
+                      return std::invoke(*reinterpret_cast<typename std::add_pointer_t<F>>(obj),
+                                         std::forward<Args>(args)...);
+                  }}
+        {
+        }
+
+        template<typename F,
+                 typename = std::enable_if_t<!std::is_same_v<std::decay_t<F>, function_ref> &&
+                                             !std::is_member_pointer_v<std::decay_t<F>> &&
+                                             std::is_invocable_r_v<R, F, Args...>>>
+        constexpr function_ref(F && f) noexcept :
+        obj_{const_cast<void *>(static_cast<const void *>(std::addressof(f)))},
+        callback_{[](void * obj, Args... args) -> R
+                  {
+                      return std::invoke(*static_cast<typename std::add_pointer_t<F>>(obj),
+                                         std::forward<Args>(args)...);
+                  }}
+        {
+        }
+
+        constexpr function_ref<R(Args...)> & operator=(
+            const function_ref<R(Args...)> & rhs) noexcept = default;
+
+        template<typename F,
+                 typename = std::enable_if_t<!std::is_same_v<std::decay_t<F>, function_ref>>>
+        function_ref<R(Args...)> & operator=(F && f) noexcept = delete;
+
+        R operator()(Args... args) const { return callback_(obj_, std::forward<Args>(args)...); }
+
+    private:
+        void * obj_;
+        R (*callback_)(void *, Args...);
+    };
+
+    // Additional deduction guides
+    template<typename F>
+    function_ref(F *) -> function_ref<F>;
+
     // ****************************** Jiminy-specific declarations ***************************** //
 
     // Exceptions
@@ -123,9 +193,8 @@ namespace jiminy
     };
 
     // Ground profile functors
-    using HeightmapFunctor =
-        std::function<std::pair<double /*height*/, Eigen::Vector3d /*normal*/>(
-            const Eigen::Vector3d & /*pos*/)>;
+    using HeightmapFunctor = std::function<void(
+        const Eigen::Vector2d & /* xy */, double & /* height */, Eigen::Vector3d & /* normal */)>;
 
     // Flexible joints
     struct FlexibleJointData
@@ -153,6 +222,7 @@ namespace jiminy
                                int32_t,
                                double,
                                std::string,
+                               VectorX<uint32_t>,
                                Eigen::VectorXd,
                                Eigen::MatrixXd,
                                HeightmapFunctor,
@@ -169,13 +239,16 @@ namespace jiminy
         std::ostringstream sstr;
         auto format = [](auto && var)
         {
-            if constexpr (is_eigen_v<decltype(var)>)
+            if constexpr (is_eigen_any_v<decltype(var)>)
             {
                 static const Eigen::IOFormat k_heavy_fmt(
                     Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
                 return var.format(k_heavy_fmt);
             }
-            return var;
+            else
+            {
+                return var;
+            }
         };
         ((sstr << format(std::forward<Args>(args))), ...);
         return sstr.str();
diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h
index ed1d599a6..dad931229 100644
--- a/core/include/jiminy/core/hardware/abstract_sensor.h
+++ b/core/include/jiminy/core/hardware/abstract_sensor.h
@@ -2,7 +2,7 @@
 #define JIMINY_ABSTRACT_SENSOR_H
 
 #include "jiminy/core/fwd.h"
-#include "jiminy/core/utilities/helpers.h"
+#include "jiminy/core/utilities/random.h"  // `PCG32`
 
 #include <boost/circular_buffer.hpp>
 #include <boost/functional/hash.hpp>
@@ -117,7 +117,7 @@ namespace jiminy
         ///
         /// \remark This method is not intended to be called manually. The Robot to which the
         ///         sensor is added is taking care of it when its own `reset` method is called.
-        virtual hresult_t resetAll() = 0;
+        virtual hresult_t resetAll(uint32_t seed) = 0;
 
         /// \brief Refresh the proxies.
         ///
@@ -309,6 +309,8 @@ namespace jiminy
         bool isTelemetryConfigured_{false};
         /// \brief Robot for which the command and internal dynamics Name of the sensor.
         std::weak_ptr<const Robot> robot_{};
+        /// \brief Random number generator used internally for sampling measurement noise.
+        PCG32 generator_;
         /// \brief Name of the sensor.
         std::string name_;
 
@@ -330,7 +332,7 @@ namespace jiminy
         auto shared_from_this() { return shared_from(this); }
         auto shared_from_this() const { return shared_from(this); }
 
-        virtual hresult_t resetAll() override;
+        hresult_t resetAll(uint32_t seed) override final;
         void updateTelemetryAll() override final;
 
         virtual hresult_t setOptionsAll(const GenericConfig & sensorOptions) override final;
diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx
index 6f114728c..5d7d45cfe 100644
--- a/core/include/jiminy/core/hardware/abstract_sensor.hxx
+++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx
@@ -153,7 +153,7 @@ namespace jiminy
     }
 
     template<typename T>
-    hresult_t AbstractSensorTpl<T>::resetAll()
+    hresult_t AbstractSensorTpl<T>::resetAll(uint32_t seed)
     {
         // Make sure all the sensors are attached to a robot
         for (AbstractSensorBase * sensor : sharedHolder_->sensors_)
@@ -194,14 +194,26 @@ namespace jiminy
                                                        return std::max(delay, value);
                                                    });
 
-        // Update sensor scope information
-        for (AbstractSensorBase * sensor : sharedHolder_->sensors_)
+        // Generate high-entropy seed sequence from the provided initial seed
+        std::seed_seq seq{seed};
+        std::vector<std::uint32_t> seeds(sharedHolder_->num_);
+        seq.generate(seeds.begin(), seeds.end());
+
+        // Reset sensor-specific state
+        std::vector<AbstractSensorBase *>::iterator sensorIt = sharedHolder_->sensors_.begin();
+        std::vector<std::uint32_t>::const_iterator seedIt = seeds.cbegin();
+        for (; sensorIt != sharedHolder_->sensors_.end(); ++sensorIt, ++seedIt)
         {
+            AbstractSensorBase & sensor = *(*sensorIt);
+
+            // Reset the internal random number generator
+            sensor.generator_.seed(*seedIt);
+
             // Refresh proxies that are robot-dependent
-            sensor->refreshProxies();
+            sensor.refreshProxies();
 
             // Reset the telemetry state
-            sensor->isTelemetryConfigured_ = false;
+            sensor.isTelemetryConfigured_ = false;
         }
 
         return hresult_t::SUCCESS;
@@ -294,14 +306,25 @@ namespace jiminy
     template<typename T>
     hresult_t AbstractSensorTpl<T>::interpolateData()
     {
-        assert(sharedHolder_->time_.size() > 0 && "Do data to interpolate.");
+        assert(sharedHolder_->time_.size() > 0 && "No data to interpolate.");
 
         // Sample the delay uniformly
         const double delay =
-            baseSensorOptions_->delay + randUniform(0.0, baseSensorOptions_->jitter);
-
-        // Add STEPPER_MIN_TIMESTEP to timeDesired to avoid float comparison issues
-        const double timeDesired = sharedHolder_->time_.back() - delay + STEPPER_MIN_TIMESTEP;
+            baseSensorOptions_->delay +
+            uniform(generator_, 0.0F, static_cast<float>(baseSensorOptions_->jitter));
+
+        // Get time at which to fetch sensor data
+        double timeDesired = sharedHolder_->time_.back() - delay;
+
+        // Floating-point comparison is every sensitive to rounding errors. This is an issue when
+        // the sensor delay exactly matches the sensor update period and Zero-Order Hold (ZOH)
+        // interpolation is being used, as it would translate in a seemingly noisy sensor signal.
+        // To prevent it, the desired time is slightly shifted before calling bisect. This promotes
+        // picking an index that is always on the same side by introducing bias in the comparison.
+        if (baseSensorOptions_->delayInterpolationOrder == 0)
+        {
+            timeDesired += STEPPER_MIN_TIMESTEP;
+        }
 
         /* Determine the position of the closest right element.
            Bisection method can be used since times are sorted. */
@@ -361,13 +384,13 @@ namespace jiminy
             }
             else if (baseSensorOptions_->delayInterpolationOrder == 1)
             {
-                // FIXME: the linear interpolation is not valid for quaternion
-                const double dt =
-                    sharedHolder_->time_[idxLeft + 1] - sharedHolder_->time_[idxLeft];
-                const double ratioNext = (sharedHolder_->time_[idxLeft + 1] - timeDesired) / dt;
-                const double ratioPrev = (timeDesired - sharedHolder_->time_[idxLeft]) / dt;
-                get() = ratioPrev * sharedHolder_->data_[idxLeft + 1].col(sensorIdx_) +
-                        ratioNext * sharedHolder_->data_[idxLeft].col(sensorIdx_);
+                // FIXME: Linear interpolation is not valid on Lie algebra
+                const double ratio =
+                    (timeDesired - sharedHolder_->time_[idxLeft]) /
+                    (sharedHolder_->time_[idxLeft + 1] - sharedHolder_->time_[idxLeft]);
+                auto dataNext = sharedHolder_->data_[idxLeft + 1].col(sensorIdx_);
+                auto dataPrev = sharedHolder_->data_[idxLeft].col(sensorIdx_);
+                get() = dataPrev + ratio * (dataNext - dataPrev);
             }
             else
             {
@@ -484,7 +507,7 @@ namespace jiminy
                    always provide the last true value instead of some initialized memory. The
                    previous value is used for the quaternion of IMU sensors to choice the right
                    value that ensures its continuity over time amond to two possible choices. */
-                sharedHolder_->time_.push_back(-1);
+                sharedHolder_->time_.push_back(INF);
                 sharedHolder_->data_.push_back(sharedHolder_->data_.back());
             }
         }
diff --git a/core/include/jiminy/core/hardware/basic_motors.h b/core/include/jiminy/core/hardware/basic_motors.h
index 88b244cf4..2524b70cc 100644
--- a/core/include/jiminy/core/hardware/basic_motors.h
+++ b/core/include/jiminy/core/hardware/basic_motors.h
@@ -77,14 +77,14 @@ namespace jiminy
 
         hresult_t initialize(const std::string & jointName);
 
-        virtual hresult_t setOptions(const GenericConfig & motorOptions) final override;
+        hresult_t setOptions(const GenericConfig & motorOptions) override;
 
     private:
-        virtual hresult_t computeEffort(double t,
-                                        const Eigen::VectorBlock<const Eigen::VectorXd> & q,
-                                        double v,
-                                        double a,
-                                        double command) final override;
+        hresult_t computeEffort(double t,
+                                const Eigen::VectorBlock<const Eigen::VectorXd> & q,
+                                double v,
+                                double a,
+                                double command) override;
 
     private:
         std::unique_ptr<const motorOptions_t> motorOptions_{nullptr};
diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h
index 0920893ab..a95a171ff 100644
--- a/core/include/jiminy/core/hardware/basic_sensors.h
+++ b/core/include/jiminy/core/hardware/basic_sensors.h
@@ -10,30 +10,29 @@ namespace jiminy
 {
     class Robot;
 
-    class JIMINY_DLLAPI ImuSensor : public AbstractSensorTpl<ImuSensor>
+    class JIMINY_DLLAPI ImuSensor final : public AbstractSensorTpl<ImuSensor>
     {
     public:
         using AbstractSensorTpl<ImuSensor>::AbstractSensorTpl;
-        virtual ~ImuSensor() = default;
 
         auto shared_from_this() { return shared_from(this); }
 
         hresult_t initialize(const std::string & frameName);
 
-        virtual hresult_t setOptions(const GenericConfig & sensorOptions) final override;
-        virtual hresult_t refreshProxies() final override;
+        hresult_t setOptions(const GenericConfig & sensorOptions) override;
+        hresult_t refreshProxies() override;
 
         const std::string & getFrameName() const;
         pinocchio::FrameIndex getFrameIdx() const;
 
     private:
-        virtual hresult_t set(double t,
-                              const Eigen::VectorXd & q,
-                              const Eigen::VectorXd & v,
-                              const Eigen::VectorXd & a,
-                              const Eigen::VectorXd & uMotor,
-                              const ForceVector & fExternal) final override;
-        virtual void measureData() final override;
+        hresult_t set(double t,
+                      const Eigen::VectorXd & q,
+                      const Eigen::VectorXd & v,
+                      const Eigen::VectorXd & a,
+                      const Eigen::VectorXd & uMotor,
+                      const ForceVector & fExternal) override;
+        void measureData() override;
 
     private:
         std::string frameName_{};
@@ -49,28 +48,27 @@ namespace jiminy
     template<>
     const bool AbstractSensorTpl<ImuSensor>::areFieldnamesGrouped_;
 
-    class JIMINY_DLLAPI ContactSensor : public AbstractSensorTpl<ContactSensor>
+    class JIMINY_DLLAPI ContactSensor final : public AbstractSensorTpl<ContactSensor>
     {
     public:
         using AbstractSensorTpl<ContactSensor>::AbstractSensorTpl;
-        virtual ~ContactSensor() = default;
 
         auto shared_from_this() { return shared_from(this); }
 
         hresult_t initialize(const std::string & frameName);
 
-        virtual hresult_t refreshProxies() final override;
+        hresult_t refreshProxies() override;
 
         const std::string & getFrameName() const;
         pinocchio::FrameIndex getFrameIdx() const;
 
     private:
-        virtual hresult_t set(double t,
-                              const Eigen::VectorXd & q,
-                              const Eigen::VectorXd & v,
-                              const Eigen::VectorXd & a,
-                              const Eigen::VectorXd & uMotor,
-                              const ForceVector & fExternal) final override;
+        hresult_t set(double t,
+                      const Eigen::VectorXd & q,
+                      const Eigen::VectorXd & v,
+                      const Eigen::VectorXd & a,
+                      const Eigen::VectorXd & uMotor,
+                      const ForceVector & fExternal) override;
 
     private:
         std::string frameName_{};
@@ -84,29 +82,28 @@ namespace jiminy
     template<>
     const bool AbstractSensorTpl<ContactSensor>::areFieldnamesGrouped_;
 
-    class JIMINY_DLLAPI ForceSensor : public AbstractSensorTpl<ForceSensor>
+    class JIMINY_DLLAPI ForceSensor final : public AbstractSensorTpl<ForceSensor>
     {
     public:
         using AbstractSensorTpl<ForceSensor>::AbstractSensorTpl;
-        virtual ~ForceSensor() = default;
 
         auto shared_from_this() { return shared_from(this); }
 
         hresult_t initialize(const std::string & frameName);
 
-        virtual hresult_t refreshProxies() final override;
+        hresult_t refreshProxies() override;
 
         const std::string & getFrameName() const;
         pinocchio::FrameIndex getFrameIdx() const;
         pinocchio::JointIndex getJointModelIdx() const;
 
     private:
-        virtual hresult_t set(double t,
-                              const Eigen::VectorXd & q,
-                              const Eigen::VectorXd & v,
-                              const Eigen::VectorXd & a,
-                              const Eigen::VectorXd & uMotor,
-                              const ForceVector & fExternal) final override;
+        hresult_t set(double t,
+                      const Eigen::VectorXd & q,
+                      const Eigen::VectorXd & v,
+                      const Eigen::VectorXd & a,
+                      const Eigen::VectorXd & uMotor,
+                      const ForceVector & fExternal) override;
 
     private:
         std::string frameName_{};
@@ -122,29 +119,28 @@ namespace jiminy
     template<>
     const bool AbstractSensorTpl<ForceSensor>::areFieldnamesGrouped_;
 
-    class JIMINY_DLLAPI EncoderSensor : public AbstractSensorTpl<EncoderSensor>
+    class JIMINY_DLLAPI EncoderSensor final : public AbstractSensorTpl<EncoderSensor>
     {
     public:
         using AbstractSensorTpl<EncoderSensor>::AbstractSensorTpl;
-        virtual ~EncoderSensor() = default;
 
         auto shared_from_this() { return shared_from(this); }
 
         hresult_t initialize(const std::string & jointName);
 
-        virtual hresult_t refreshProxies() final override;
+        hresult_t refreshProxies() override;
 
         const std::string & getJointName() const;
         pinocchio::JointIndex getJointModelIdx() const;
         JointModelType getJointType() const;
 
     private:
-        virtual hresult_t set(double t,
-                              const Eigen::VectorXd & q,
-                              const Eigen::VectorXd & v,
-                              const Eigen::VectorXd & a,
-                              const Eigen::VectorXd & uMotor,
-                              const ForceVector & fExternal) final override;
+        hresult_t set(double t,
+                      const Eigen::VectorXd & q,
+                      const Eigen::VectorXd & v,
+                      const Eigen::VectorXd & a,
+                      const Eigen::VectorXd & uMotor,
+                      const ForceVector & fExternal) override;
 
     private:
         std::string jointName_{};
@@ -159,28 +155,27 @@ namespace jiminy
     template<>
     const bool AbstractSensorTpl<EncoderSensor>::areFieldnamesGrouped_;
 
-    class JIMINY_DLLAPI EffortSensor : public AbstractSensorTpl<EffortSensor>
+    class JIMINY_DLLAPI EffortSensor final : public AbstractSensorTpl<EffortSensor>
     {
     public:
         using AbstractSensorTpl<EffortSensor>::AbstractSensorTpl;
-        virtual ~EffortSensor() = default;
 
         auto shared_from_this() { return shared_from(this); }
 
         hresult_t initialize(const std::string & motorName);
 
-        virtual hresult_t refreshProxies() final override;
+        hresult_t refreshProxies() override;
 
         const std::string & getMotorName() const;
         std::size_t getMotorIdx() const;
 
     private:
-        virtual hresult_t set(double t,
-                              const Eigen::VectorXd & q,
-                              const Eigen::VectorXd & v,
-                              const Eigen::VectorXd & a,
-                              const Eigen::VectorXd & uMotor,
-                              const ForceVector & fExternal) final override;
+        hresult_t set(double t,
+                      const Eigen::VectorXd & q,
+                      const Eigen::VectorXd & v,
+                      const Eigen::VectorXd & a,
+                      const Eigen::VectorXd & uMotor,
+                      const ForceVector & fExternal) override;
 
     private:
         std::string motorName_{};
diff --git a/core/include/jiminy/core/hardware/fwd.h b/core/include/jiminy/core/hardware/fwd.h
index 7c11588ac..e5cc54fc5 100644
--- a/core/include/jiminy/core/hardware/fwd.h
+++ b/core/include/jiminy/core/hardware/fwd.h
@@ -14,7 +14,7 @@
 namespace jiminy
 {
     // Sensor data holder
-    namespace details
+    namespace internal
     {
         struct SensorDataItem
         {
@@ -33,18 +33,18 @@ namespace jiminy
 
     struct SensorDataTypeMap :
     public boost::multi_index::multi_index_container<
-        details::SensorDataItem,
+        internal::SensorDataItem,
         boost::multi_index::indexed_by<
             boost::multi_index::ordered_unique<
                 boost::multi_index::tag<IndexByIndex>,
                 boost::multi_index::
-                    member<details::SensorDataItem, std::size_t, &details::SensorDataItem::idx>,
+                    member<internal::SensorDataItem, std::size_t, &internal::SensorDataItem::idx>,
                 std::less<std::size_t>  // Ordering by ascending order
                 >,
             boost::multi_index::hashed_unique<
                 boost::multi_index::tag<IndexByName>,
                 boost::multi_index::
-                    member<details::SensorDataItem, std::string, &details::SensorDataItem::name>>,
+                    member<internal::SensorDataItem, std::string, &internal::SensorDataItem::name>>,
             boost::multi_index::sequenced<>>>
     {
     public:
diff --git a/core/include/jiminy/core/macros.h b/core/include/jiminy/core/macros.h
index 6a8ddf707..6b28f2be2 100644
--- a/core/include/jiminy/core/macros.h
+++ b/core/include/jiminy/core/macros.h
@@ -37,18 +37,18 @@
 
 // ******************************* Error and warnings utilities ******************************** //
 
-namespace details
+namespace jiminy::internal
 {
     template<size_t FL, size_t PFL>
-    const char * extractMethodName(const char (&function)[FL], const char (&prettyFunction)[PFL])
+    const char * extractFunctionName(const char (&func)[FL], const char (&pretty_func)[PFL])
     {
         using reverse_ptr = std::reverse_iterator<const char *>;
         thread_local static char result[PFL];
         const char * locFuncName =
-            std::search(prettyFunction, prettyFunction + PFL - 1, function, function + FL - 1);
+            std::search(pretty_func, pretty_func + PFL - 1, func, func + FL - 1);
         const char * locClassName =
-            std::find(reverse_ptr(locFuncName), reverse_ptr(prettyFunction), ' ').base();
-        const char * endFuncName = std::find(locFuncName, prettyFunction + PFL - 1, '(');
+            std::find(reverse_ptr(locFuncName), reverse_ptr(pretty_func), ' ').base();
+        const char * endFuncName = std::find(locFuncName, pretty_func + PFL - 1, '(');
         std::copy(locClassName, endFuncName, result);
         return result;
     }
@@ -63,17 +63,17 @@ namespace details
    https://solarianprogrammer.com/2019/04/08/c-programming-ansi-escape-codes-windows-macos-linux-terminals/
 */
 
-#define PRINT_ERROR(...)                                                        \
-    std::cerr << "In " FILE_LINE ": In "                                        \
-              << ::details::extractMethodName(__func__, BOOST_CURRENT_FUNCTION) \
+#define PRINT_ERROR(...)                                                                 \
+    std::cerr << "In " FILE_LINE ": In "                                                 \
+              << jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
               << ":\n\x1b[1;31merror:\x1b[0m " << toString(__VA_ARGS__) << std::endl
 
 #ifdef NDEBUG
 #    define PRINT_WARNING(...)
 #else
-#    define PRINT_WARNING(...)                                                      \
-        std::cerr << "In " FILE_LINE ": In "                                        \
-                  << ::details::extractMethodName(__func__, BOOST_CURRENT_FUNCTION) \
+#    define PRINT_WARNING(...)                                                               \
+        std::cerr << "In " FILE_LINE ": In "                                                 \
+                  << jiminy::internal::extractFunctionName(__func__, BOOST_CURRENT_FUNCTION) \
                   << ":\n\x1b[1;93mwarning:\x1b[0m " << toString(__VA_ARGS__) << std::endl
 #endif
 
diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h
index ee9a77b8a..2cd5ff732 100644
--- a/core/include/jiminy/core/robot/model.h
+++ b/core/include/jiminy/core/robot/model.h
@@ -8,7 +8,7 @@
 #include "pinocchio/multibody/frame.hpp"  // `pinocchio::FrameType` (C-style enum cannot be forward declared)
 
 #include "jiminy/core/fwd.h"
-#include "jiminy/core/utilities/helpers.h"
+#include "jiminy/core/utilities/random.h"  // `uniform_random_bit_generator_ref`
 
 
 namespace jiminy
@@ -321,7 +321,7 @@ namespace jiminy
 
         /// \remark This method are not intended to be called manually. The Engine is taking care
         ///         of it.
-        virtual void reset();
+        virtual void reset(const uniform_random_bit_generator_ref<uint32_t> & g);
 
         bool getIsInitialized() const;
         const std::string & getName() const;
@@ -366,7 +366,7 @@ namespace jiminy
 
     protected:
         hresult_t generateModelFlexible();
-        hresult_t generateModelBiased();
+        hresult_t generateModelBiased(const uniform_random_bit_generator_ref<uint32_t> & g);
 
         hresult_t addFrame(const std::string & frameName,
                            const std::string & parentBodyName,
diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h
index 3ee65ed5f..5e714db10 100644
--- a/core/include/jiminy/core/robot/robot.h
+++ b/core/include/jiminy/core/robot/robot.h
@@ -111,7 +111,7 @@ namespace jiminy
 
         /// \remarks Those methods are not intended to be called manually. The Engine is taking
         ///          care of it.
-        virtual void reset() override;
+        virtual void reset(const uniform_random_bit_generator_ref<uint32_t> & g) override;
         virtual hresult_t configureTelemetry(std::shared_ptr<TelemetryData> telemetryData,
                                              const std::string & objectPrefixName = {});
         void updateTelemetry();
diff --git a/core/include/jiminy/core/traits.h b/core/include/jiminy/core/traits.h
index 020b32311..5e3720c92 100644
--- a/core/include/jiminy/core/traits.h
+++ b/core/include/jiminy/core/traits.h
@@ -28,7 +28,7 @@ namespace jiminy
 {
     // ******************************** is_contiguous_container ******************************** //
 
-    namespace details
+    namespace internal
     {
         template<class...>
         struct voider
@@ -67,7 +67,7 @@ namespace jiminy
 
     template<class T, class I = std::size_t>
     inline constexpr bool is_contiguous_container_v =
-        std::conjunction<details::hasSize<T>, details::isContiguousIndexable<T, I>>::value;
+        std::conjunction<internal::hasSize<T>, internal::isContiguousIndexable<T, I>>::value;
 
     // ************************************* remove_cvref ************************************** //
 
@@ -83,7 +83,7 @@ namespace jiminy
 
     // ********************************* is_base_of_template_v ********************************* //
 
-    namespace details
+    namespace internal
     {
         /// @sa For reference, see:
         /// https://stackoverflow.com/a/34672753/4820605
@@ -95,10 +95,10 @@ namespace jiminy
             static constexpr std::false_type test(...);
             using value = decltype(test(std::declval<Derived *>()));
         };
-    }  // namespace details
+    }
 
     template<template<typename...> class Base, typename Derived>
-    using is_base_of_template_v = typename details::IsBaseOfTemplateImpl<Base, Derived>::value;
+    using is_base_of_template_v = typename internal::IsBaseOfTemplateImpl<Base, Derived>::value;
 
     // *************************************** is_vector *************************************** //
 
@@ -132,7 +132,7 @@ namespace jiminy
 
     // **************************************** is_map ***************************************** //
 
-    namespace details::is_map
+    namespace internal::is_map
     {
         template<typename K, typename T, typename C, typename A>
         std::true_type test(const std::map<K, T, C, A> *);
@@ -144,7 +144,7 @@ namespace jiminy
         struct Test : public decltype(test(std::declval<std::add_pointer_t<T>>()))
         {
         };
-    }  // namespace details::is_map
+    }
 
     template<typename T, typename Enable = void>
     struct is_map : std::false_type
@@ -152,7 +152,7 @@ namespace jiminy
     };
 
     template<typename T>
-    struct is_map<T, typename std::enable_if_t<details::is_map::Test<T>::value>> : std::true_type
+    struct is_map<T, typename std::enable_if_t<internal::is_map::Test<T>::value>> : std::true_type
     {
     };
 
@@ -161,7 +161,7 @@ namespace jiminy
 
     // ************************************ is_eigen_vector ************************************ //
 
-    namespace details::is_eigen_vector
+    namespace internal::is_eigen_vector
     {
         template<typename T, int RowsAtCompileTime>
         std::true_type test(const Eigen::Matrix<T, RowsAtCompileTime, 1> *);
@@ -189,7 +189,7 @@ namespace jiminy
         struct Test : public decltype(test(std::declval<std::add_pointer_t<T>>()))
         {
         };
-    }  // namespace details::is_eigen_vector
+    }
 
     template<typename T, typename Enable = void>
     struct is_eigen_vector : std::false_type
@@ -197,7 +197,8 @@ namespace jiminy
     };
 
     template<typename T>
-    struct is_eigen_vector<T, typename std::enable_if_t<details::is_eigen_vector::Test<T>::value>> :
+    struct is_eigen_vector<T,
+                           typename std::enable_if_t<internal::is_eigen_vector::Test<T>::value>> :
     std::true_type
     {
     };
@@ -207,7 +208,7 @@ namespace jiminy
 
     // ************************************* is_eigen_ref ************************************** //
 
-    namespace details::is_eigen_ref
+    namespace internal::is_eigen_ref
     {
         template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
         std::true_type
@@ -253,7 +254,7 @@ namespace jiminy
         struct Test : public decltype(test(std::declval<std::add_pointer_t<T>>()))
         {
         };
-    }  // namespace details::is_eigen_ref
+    }
 
     template<typename T, typename Enable = void>
     struct is_eigen_ref : std::false_type
@@ -261,7 +262,7 @@ namespace jiminy
     };
 
     template<typename T>
-    struct is_eigen_ref<T, typename std::enable_if_t<details::is_eigen_ref::Test<T>::value>> :
+    struct is_eigen_ref<T, typename std::enable_if_t<internal::is_eigen_ref::Test<T>::value>> :
     std::true_type
     {
     };
@@ -271,7 +272,7 @@ namespace jiminy
 
     // *************************************** is_eigen **************************************** //
 
-    namespace details::is_eigen_plain
+    namespace internal::is_eigen_plain
     {
         template<typename T, int RowsAtCompileTime, int ColsAtCompileTime>
         std::true_type test(const Eigen::Matrix<T, RowsAtCompileTime, ColsAtCompileTime> *);
@@ -291,7 +292,7 @@ namespace jiminy
     };
 
     template<typename T>
-    struct is_eigen_plain<T, typename std::enable_if_t<details::is_eigen_plain::Test<T>::value>> :
+    struct is_eigen_plain<T, typename std::enable_if_t<internal::is_eigen_plain::Test<T>::value>> :
     std::true_type
     {
     };
@@ -300,39 +301,44 @@ namespace jiminy
     inline constexpr bool is_eigen_plain_v = is_eigen_plain<std::decay_t<T>>::value;
 
     template<class T>
-    inline constexpr bool is_eigen_v = std::disjunction<is_eigen_plain<T>, is_eigen_ref<T>>::value;
+    inline constexpr bool is_eigen_object_v =
+        std::disjunction<is_eigen_plain<T>, is_eigen_ref<T>>::value;
+
+    template<typename Derived>
+    inline constexpr bool is_eigen_any_v =
+        std::is_base_of_v<Eigen::MatrixBase<std::decay_t<Derived>>, std::decay_t<Derived>>;
 
     // ********************************** is_pinocchio_joint_ ********************************** //
 
-#define IS_PINOCCHIO_JOINT_ENABLE_IF(type, name)                                         \
-    IS_PINOCCHIO_JOINT_DETAILS(type, name)                                               \
-                                                                                         \
-    namespace details::is_pinocchio_joint_##name                                         \
-    {                                                                                    \
-        template<typename T>                                                             \
-        struct Test : public decltype(test(std::declval<std::add_pointer_t<T>>()))       \
-        {                                                                                \
-        };                                                                               \
-    }                                                                                    \
-                                                                                         \
-    template<typename T, typename Enable = void>                                         \
-    struct is_pinocchio_joint_##name : public std::false_type                            \
-    {                                                                                    \
-    };                                                                                   \
-                                                                                         \
-    template<typename T>                                                                 \
-    struct is_pinocchio_joint_##name<                                                    \
-        T,                                                                               \
-        typename std::enable_if_t<details::is_pinocchio_joint_##name::Test<T>::value>> : \
-    std::true_type                                                                       \
-    {                                                                                    \
-    };                                                                                   \
-                                                                                         \
-    template<typename T>                                                                 \
+#define IS_PINOCCHIO_JOINT_ENABLE_IF(type, name)                                          \
+    IS_PINOCCHIO_JOINT_DETAILS(type, name)                                                \
+                                                                                          \
+    namespace internal::is_pinocchio_joint_##name                                         \
+    {                                                                                     \
+        template<typename T>                                                              \
+        struct Test : public decltype(test(std::declval<std::add_pointer_t<T>>()))        \
+        {                                                                                 \
+        };                                                                                \
+    }                                                                                     \
+                                                                                          \
+    template<typename T, typename Enable = void>                                          \
+    struct is_pinocchio_joint_##name : public std::false_type                             \
+    {                                                                                     \
+    };                                                                                    \
+                                                                                          \
+    template<typename T>                                                                  \
+    struct is_pinocchio_joint_##name<                                                     \
+        T,                                                                                \
+        typename std::enable_if_t<internal::is_pinocchio_joint_##name::Test<T>::value>> : \
+    std::true_type                                                                        \
+    {                                                                                     \
+    };                                                                                    \
+                                                                                          \
+    template<typename T>                                                                  \
     inline constexpr bool is_pinocchio_joint_##name##_v = is_pinocchio_joint_##name<T>::value;
 
 #define IS_PINOCCHIO_JOINT_DETAILS(type, name)                                          \
-    namespace details::is_pinocchio_joint_##name                                        \
+    namespace internal::is_pinocchio_joint_##name                                       \
     {                                                                                   \
         template<typename Scalar, int Options>                                          \
         std::true_type test(const pinocchio::JointModel##type##Tpl<Scalar, Options> *); \
@@ -352,7 +358,7 @@ namespace jiminy
 
 #undef IS_PINOCCHIO_JOINT_DETAILS
 #define IS_PINOCCHIO_JOINT_DETAILS(type, name)                                                \
-    namespace details::is_pinocchio_joint_##name                                              \
+    namespace internal::is_pinocchio_joint_##name                                             \
     {                                                                                         \
         template<typename Scalar, int Options, int axis>                                      \
         std::true_type test(const pinocchio::JointModel##type##Tpl<Scalar, Options, axis> *); \
@@ -367,7 +373,7 @@ namespace jiminy
 
 #undef IS_PINOCCHIO_JOINT_DETAILS
 #define IS_PINOCCHIO_JOINT_DETAILS(type, name)                       \
-    namespace details::is_pinocchio_joint_##name                     \
+    namespace internal::is_pinocchio_joint_##name                    \
     {                                                                \
         template<typename T>                                         \
         std::true_type test(const pinocchio::JointModel##type<T> *); \
@@ -380,7 +386,7 @@ namespace jiminy
 
 #undef IS_PINOCCHIO_JOINT_DETAILS
 #define IS_PINOCCHIO_JOINT_DETAILS(type, name)                                              \
-    namespace details::is_pinocchio_joint_##name                                            \
+    namespace internal::is_pinocchio_joint_##name                                           \
     {                                                                                       \
         template<typename Scalar,                                                           \
                  int Options,                                                               \
diff --git a/core/include/jiminy/core/utilities/geometry.h b/core/include/jiminy/core/utilities/geometry.h
new file mode 100644
index 000000000..4b5df6fbc
--- /dev/null
+++ b/core/include/jiminy/core/utilities/geometry.h
@@ -0,0 +1,28 @@
+#ifndef JIMINY_GEOMETRY_H
+#define JIMINY_GEOMETRY_H
+
+#include <vector>  // `std::vector`
+
+#include "jiminy/core/fwd.h"  // `HeightmapFunctor`
+
+#include "hpp/fcl/fwd.hh"  // `hpp::fcl::BVHModelPtr_t`
+
+
+namespace jiminy
+{
+    HeightmapFunctor JIMINY_DLLAPI sumHeightmaps(std::vector<HeightmapFunctor> heightmaps);
+
+    HeightmapFunctor JIMINY_DLLAPI mergeHeightmaps(std::vector<HeightmapFunctor> heightmaps);
+
+    hpp::fcl::CollisionGeometryPtr_t JIMINY_DLLAPI discretizeHeightmap(
+        const HeightmapFunctor & heightmap,
+        double x_min,
+        double x_max,
+        double x_unit,
+        double y_min,
+        double y_max,
+        double y_unit,
+        bool must_simplify = false);
+}
+
+#endif  // JIMINY_GEOMETRY_H
\ No newline at end of file
diff --git a/core/include/jiminy/core/utilities/helpers.h b/core/include/jiminy/core/utilities/helpers.h
index 0a0de2f5f..cbb1b9b41 100644
--- a/core/include/jiminy/core/utilities/helpers.h
+++ b/core/include/jiminy/core/utilities/helpers.h
@@ -1,5 +1,5 @@
-#ifndef JIMINY_UTILITIES_H
-#define JIMINY_UTILITIES_H
+#ifndef JIMINY_HELPERS_H
+#define JIMINY_HELPERS_H
 
 #include <chrono>
 #include <memory>
@@ -12,7 +12,7 @@ namespace jiminy
 {
     struct LogData;
 
-    // *************** Local Mutex/Lock mechanism ******************
+    // ******************************* Local Mutex/Lock mechanism ****************************** //
 
     class LockGuardLocal;
 
@@ -50,34 +50,34 @@ namespace jiminy
         std::shared_ptr<bool> mutexFlag_;
     };
 
-    // ************************ Timer *******************************
+    // ***************************************** Timer ***************************************** //
 
     class JIMINY_DLLAPI Timer
     {
-        using Time = std::chrono::high_resolution_clock;
+        using clock = std::chrono::high_resolution_clock;
 
     public:
         explicit Timer() noexcept;
 
         void tic() noexcept;
-        void toc() noexcept;
 
-    public:
-        std::chrono::time_point<Time> t0{};
-        std::chrono::time_point<Time> tf{};
-        double dt{0.0};
+        template<typename Period = std::ratio<1>>
+        double toc() const noexcept;
+
+    private:
+        std::chrono::time_point<clock> t0_{};
     };
 
     // ****************************** Generic template utilities ******************************* //
 
     template<class F, class... Args>
     std::enable_if_t<!(... && !std::is_same_v<std::invoke_result_t<F, Args>, void>)>
-    do_for(F f, Args &&... args);
+    do_for(F func, Args &&... args);
 
     template<class F, class... Args>
     std::enable_if_t<(... && !std::is_same_v<std::invoke_result_t<F, Args>, void>),
                      std::tuple<std::invoke_result_t<F, Args>...>>
-    do_for(F f, Args &&... args);
+    do_for(F func, Args &&... args);
 
     // ******************************** enable_shared_from_this ******************************** //
 
@@ -91,26 +91,26 @@ namespace jiminy
     template<typename T>
     inline std::shared_ptr<T> shared_from(T * derived);
 
-    // ************* IO file and Directory utilities ****************
+    // **************************** IO file and Directory utilities **************************** //
 
     std::string JIMINY_DLLAPI getUserDirectory();
 
-    // ******************* Telemetry utilities **********************
+    // ********************************** Telemetry utilities ********************************** //
 
-    bool endsWith(const std::string & text, const std::string & ending);
+    bool endsWith(const std::string & str, const std::string & substr);
 
     std::string addCircumfix(std::string fieldname,  // Make a copy
                              const std::string_view & prefix = {},
                              const std::string_view & suffix = {},
                              const std::string_view & delimiter = {});
-    std::vector<std::string> addCircumfix(const std::vector<std::string> & fieldnamesIn,
+    std::vector<std::string> addCircumfix(const std::vector<std::string> & fieldnames,
                                           const std::string_view & prefix = {},
                                           const std::string_view & suffix = {},
                                           const std::string_view & delimiter = {});
 
     std::string removeSuffix(std::string fieldname,  // Make a copy
                              const std::string & suffix);
-    std::vector<std::string> removeSuffix(const std::vector<std::string> & fieldnamesIn,
+    std::vector<std::string> removeSuffix(const std::vector<std::string> & fieldnames,
                                           const std::string & suffix);
 
     /// \brief Value of a single logged variable (by copy).
@@ -122,41 +122,50 @@ namespace jiminy
     Eigen::VectorXd JIMINY_DLLAPI getLogVariable(const LogData & logData,
                                                  const std::string & fieldname);
 
-    // ********************** Math utilities *************************
-
-    template<typename DerivedType1, typename DerivedType2, typename DerivedType3>
-    Eigen::MatrixBase<DerivedType1> clamp(const Eigen::MatrixBase<DerivedType1> & data,
-                                          const Eigen::MatrixBase<DerivedType2> & minThr,
-                                          const Eigen::MatrixBase<DerivedType2> & maxThr);
+    // ************************************* Math utilities ************************************ //
 
     template<typename... Args>
-    double minClipped(double val1, double val2, Args... vs);
+    std::enable_if_t<std::conjunction_v<std::is_same<Args, double>...>, const double &>
+    minClipped(const double & value1, const double & value2, const Args &... values);
 
     template<typename... Args>
-    std::tuple<bool, double> isGcdIncluded(Args... values);
+    std::enable_if_t<std::conjunction_v<std::is_same<Args, double>...>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(const Args &... values);
 
     template<typename InputIt, typename UnaryFunction>
-    std::tuple<bool, double> isGcdIncluded(InputIt first, InputIt last, UnaryFunction f);
+    std::enable_if_t<std::is_invocable_r_v<const double &,
+                                           UnaryFunction,
+                                           typename std::iterator_traits<InputIt>::reference>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(InputIt first, InputIt last, const UnaryFunction & func);
 
     template<typename InputIt, typename UnaryFunction, typename... Args>
-    std::tuple<bool, double>
-    isGcdIncluded(InputIt first, InputIt last, UnaryFunction f, Args... values);
+    std::enable_if_t<std::is_invocable_r_v<const double &,
+                                           UnaryFunction,
+                                           typename std::iterator_traits<InputIt>::reference> &&
+                     std::conjunction_v<std::is_same<Args, double>...>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(InputIt first, InputIt last, const UnaryFunction & func, const Args &... values);
 
-    // ********************* Std::vector helpers **********************
+    // ********************************** Std::vector helpers ********************************** //
 
-    template<typename T, typename A>
-    bool checkDuplicates(const std::vector<T, A> & vect);
+    template<typename T>
+    std::enable_if_t<is_vector_v<T>, bool> checkDuplicates(const T & vect);
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    bool checkIntersection(const std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2);
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, bool> checkIntersection(const T1 & vec1,
+                                                                                 const T2 & vec2);
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    bool checkInclusion(const std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2);
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, bool> checkInclusion(const T1 & vec1,
+                                                                              const T2 & vec2);
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    void eraseVector(std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2);
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, void> eraseVector(const T1 & vec1,
+                                                                           const T2 & vec2);
 
-    // *********************** Miscellaneous **************************
+    // ************************************* Miscellaneous ************************************* //
 
     template<typename Derived>
     void swapMatrixRows(const Eigen::MatrixBase<Derived> & matrixIn,
@@ -168,4 +177,4 @@ namespace jiminy
 
 #include "jiminy/core/utilities/helpers.hxx"
 
-#endif  // JIMINY_UTILITIES_H
+#endif  // JIMINY_HELPERS_H
diff --git a/core/include/jiminy/core/utilities/helpers.hxx b/core/include/jiminy/core/utilities/helpers.hxx
index 81e3234df..1a9e96714 100644
--- a/core/include/jiminy/core/utilities/helpers.hxx
+++ b/core/include/jiminy/core/utilities/helpers.hxx
@@ -1,24 +1,36 @@
+#ifndef JIMINY_HELPERS_HXX
+#define JIMINY_HELPERS_HXX
+
 #include <algorithm>
 #include <numeric>
 
 
 namespace jiminy
 {
+    // ***************************************** Timer ***************************************** //
+
+    template<typename Period>
+    double Timer::toc() const noexcept
+    {
+        const typename clock::duration dt_{clock::now() - t0_};
+        return std::chrono::duration_cast<std::chrono::duration<double, Period>>(dt_).count();
+    }
+
     // ****************************** Generic template utilities ******************************* //
 
     template<class F, class... Args>
     std::enable_if_t<!(... && !std::is_same_v<std::invoke_result_t<F, Args>, void>)>
-    do_for(F f, Args &&... args)
+    do_for(F func, Args &&... args)
     {
-        (f(std::forward<Args>(args)), ...);
+        (func(std::forward<Args>(args)), ...);
     }
 
     template<class F, class... Args>
     std::enable_if_t<(... && !std::is_same_v<std::invoke_result_t<F, Args>, void>),
                      std::tuple<std::invoke_result_t<F, Args>...>>
-    do_for(F f, Args &&... args)
+    do_for(F func, Args &&... args)
     {
-        return std::tuple{f(std::forward<Args>(args))...};
+        return std::tuple{func(std::forward<Args>(args))...};
     }
 
     // ******************************** enable_shared_from_this ******************************** //
@@ -42,108 +54,111 @@ namespace jiminy
         return std::static_pointer_cast<T>(shared_from_base(derived));
     }
 
-    // *************************** Math *****************************
-
-    template<typename DerivedType>
-    auto clamp(const Eigen::MatrixBase<DerivedType> & data, double minThr, double maxThr)
-    {
-        return data.unaryExpr([&minThr, &maxThr](double x) -> double
-                              { return std::clamp(x, minThr, maxThr); });
-    }
-
-    template<typename DerivedType1, typename DerivedType2, typename DerivedType3>
-    Eigen::MatrixBase<DerivedType1> clamp(const Eigen::MatrixBase<DerivedType1> & data,
-                                          const Eigen::MatrixBase<DerivedType2> & minThr,
-                                          const Eigen::MatrixBase<DerivedType2> & maxThr)
-    {
-        return data.array().max(minThr).min(maxThr);
-    }
+    // ************************************* Math utilities ************************************ //
 
-    inline double minClipped()
+    inline const double & minClipped()
     {
         return INF;
     }
 
-    inline double minClipped(double val)
+    inline const double & minClipped(const double & val)
     {
         if (val > EPS)
         {
-            return std::forward<double>(val);
+            return val;
         }
         return INF;
     }
 
     template<typename... Args>
-    double minClipped(double val1, double val2, Args... vs)
+    std::enable_if_t<std::conjunction_v<std::is_same<Args, double>...>, const double &>
+    minClipped(const double & value1, const double & value2, const Args &... values)
     {
-        const bool isValid1 = val1 > EPS;
-        const bool isValid2 = val2 > EPS;
+        const bool isValid1 = value1 > EPS;
+        const bool isValid2 = value2 > EPS;
         if (isValid1 && isValid2)
         {
-            return minClipped(std::min(val1, val2), std::forward<Args>(vs)...);
+            return minClipped(std::min(value1, value2), values...);
         }
         else if (isValid2)
         {
-            return minClipped(val2, std::forward<Args>(vs)...);
+            return minClipped(value2, values...);
         }
         else if (isValid1)
         {
-            return minClipped(val1, std::forward<Args>(vs)...);
+            return minClipped(value1, values...);
         }
-        return minClipped(std::forward<Args>(vs)...);
+        return minClipped(values...);
     }
 
     template<typename... Args>
-    std::tuple<bool, double> isGcdIncluded(Args... values)
+    std::enable_if_t<std::conjunction_v<std::is_same<Args, double>...>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(const Args &... values)
     {
-        const double minValue = minClipped(std::forward<Args>(values)...);
+        const double & minValue = minClipped(values...);
         if (!std::isfinite(minValue))
         {
             return {true, INF};
         }
-        auto lambda = [&minValue](double value)
-        {
-            if (value < EPS)
+        /* FIXME: In some cases, order of evaluation is not always respected with MSVC, although
+           they pretend it has been fixed but it. As a result, 'isIncluded' must be explicitly
+           computed first. For reference, see:
+           https://devblogs.microsoft.com/cppblog/compiler-improvements-in-vs-2015-update-2/#order-of-initializer-list
+        */
+        bool isIncluded = (
+            [&minValue](double value)
             {
-                return true;
-            }
-            return std::fmod(value, minValue) < EPS;
-        };
-        // Taking advantage of C++17 "fold expression"
-        return {(... && lambda(values)), minValue};
+                if (value < EPS)
+                {
+                    return true;
+                }
+                return std::fmod(value, minValue) < EPS;
+            }(values) && ...);
+        return {isIncluded, minValue};
     }
 
     template<typename InputIt, typename UnaryFunction>
-    std::tuple<bool, double> isGcdIncluded(InputIt first, InputIt last, UnaryFunction f)
+    std::enable_if_t<std::is_invocable_r_v<const double &,
+                                           UnaryFunction,
+                                           typename std::iterator_traits<InputIt>::reference>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(InputIt first, InputIt last, const UnaryFunction & func)
     {
-        const double minValue = std::transform_reduce(first, last, INF, minClipped<>, f);
+        const double & minValue = std::transform_reduce(first, last, INF, minClipped<>, func);
         if (!std::isfinite(minValue))
         {
             return {true, INF};
         }
-        auto lambda = [&minValue, &f](const auto & elem)
+        auto lambda = [minValue, &func](const auto & elem)
         {
-            const double value = f(elem);
+            const double value = func(elem);
             if (value < EPS)
             {
                 return true;
             }
             return std::fmod(value, minValue) < EPS;
         };
-        return {std::all_of(first, last, lambda), minValue};
+        // FIXME: Order of evaluation is not always respected with MSVC.
+        bool isIncluded = std::all_of(first, last, lambda);
+        return {isIncluded, minValue};
     }
 
     template<typename InputIt, typename UnaryFunction, typename... Args>
-    std::tuple<bool, double>
-    isGcdIncluded(InputIt first, InputIt last, UnaryFunction f, Args... values)
+    std::enable_if_t<std::is_invocable_r_v<const double &,
+                                           UnaryFunction,
+                                           typename std::iterator_traits<InputIt>::reference> &&
+                     std::conjunction_v<std::is_same<Args, double>...>,
+                     std::tuple<bool, const double &>>
+    isGcdIncluded(InputIt first, InputIt last, const UnaryFunction & func, const Args &... values)
     {
-        const auto [isIncluded1, value1] = isGcdIncluded(std::forward<Args>(values)...);
-        const auto [isIncluded2, value2] = isGcdIncluded(first, last, f);
+        auto && [isIncluded1, value1] = isGcdIncluded(values...);
+        auto && [isIncluded2, value2] = isGcdIncluded(first, last, func);
         if (!isIncluded1 || !isIncluded2)
         {
             return {false, INF};
         }
-        if (value1 < value2)  // inf < inf : false
+        if (value1 < value2)  // inf < inf := false
         {
             if (!std::isfinite(value2))
             {
@@ -165,14 +180,15 @@ namespace jiminy
         }
     }
 
-    // ******************** Std::vector helpers *********************
+    // ********************************** Std::vector helpers ********************************** //
 
-    template<typename T, typename A>
-    bool checkDuplicates(const std::vector<T, A> & vect)
+    template<typename T>
+    std::enable_if_t<is_vector_v<T>, bool> checkDuplicates(const T & vec)
     {
-        for (auto it = vect.begin(); it != vect.end(); ++it)
+        const auto vecEnd = vec.cend();
+        for (auto vecIt = vec.cbegin(); vecIt != vecEnd; ++vecIt)
         {
-            if (std::find(it + 1, vect.end(), *it) != vect.end())
+            if (std::find(std::next(vecIt), vecEnd, *vecIt) != vecEnd)
             {
                 return true;
             }
@@ -180,27 +196,30 @@ namespace jiminy
         return false;
     }
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    bool checkIntersection(const std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2)
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, bool> checkIntersection(const T1 & vec1,
+                                                                                 const T2 & vec2)
     {
-        auto vect2It = std::find_if(vect2.begin(),
-                                    vect2.end(),
-                                    [&vect1](const auto & elem2)
-                                    {
-                                        auto vect1It =
-                                            std::find(vect1.begin(), vect1.end(), elem2);
-                                        return (vect1It != vect1.end());
-                                    });
-        return (vect2It != vect2.end());
+        const auto vec2It =
+            std::find_if(vec2.cbegin(),
+                         vec2.cend(),
+                         [vec1Begin = vec1.cbegin(), vec1End = vec1.cend()](const auto & elem2)
+                         {
+                             auto vec1It = std::find(vec1Begin, vec1End, elem2);
+                             return (vec1It != vec1End);
+                         });
+        return (vec2It != vec2.cend());
     }
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    bool checkInclusion(const std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2)
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, bool> checkInclusion(const T1 & vec1,
+                                                                              const T2 & vec2)
     {
-        for (const auto & elem2 : vect2)
+        const auto vec1End = vec1.cend();
+        for (const auto & elem2 : vec2)
         {
-            auto vect1It = std::find(vect1.begin(), vect1.end(), elem2);
-            if (vect1It == vect1.end())
+            const auto vec1It = std::find(vec1.cbegin(), vec1End, elem2);
+            if (vec1It == vec1End)
             {
                 return false;
             }
@@ -208,22 +227,24 @@ namespace jiminy
         return true;
     }
 
-    template<typename T1, typename A1, typename T2, typename A2>
-    void eraseVector(std::vector<T1, A1> & vect1, const std::vector<T2, A2> & vect2)
+    template<typename T1, typename T2>
+    std::enable_if_t<is_vector_v<T1> && is_vector_v<T2>, void> eraseVector(T1 & vec1,
+                                                                           const T2 & vec2)
     {
-        vect1.erase(std::remove_if(vect1.begin(),
-                                   vect1.end(),
-                                   [&vect2](const auto & elem1)
-                                   {
-                                       auto vect2It = std::find(vect2.begin(), vect2.end(), elem1);
-                                       return (vect2It != vect2.end());
-                                   }),
-                    vect1.end());
+        vec1.erase(
+            std::remove_if(vec1.begin(),
+                           vec1.end(),
+                           [vec2Begin = vec2.cbegin(), vec2End = vec2.cend()](const auto & elem1)
+                           {
+                               auto vec2It = std::find(vec2Begin, vec2End, elem1);
+                               return (vec2It != vec2End);
+                           }),
+            vec1.end());
     }
 
-    // *********************** Miscellaneous **************************
+    // ************************************* Miscellaneous ************************************* //
 
-    /// \brief Swap two non-overlapping row-blocks of data in a matrix.
+    /// \brief Swap two disjoint row-blocks of data in a matrix.
     ///
     /// \details Let b1, b2 be two row-blocks of arbitrary sizes of a matrix B s.t.
     ///          B = (... b1 ... b2 ...).T. This function re-assigns B to (... b2 ... b1 ...).T.
@@ -266,4 +287,6 @@ namespace jiminy
         const Eigen::Index secondBlockEnd = secondBlockStart + secondBlockLength;  // Excluded
         matrix.middleRows(secondBlockEnd - firstBlockLength, firstBlockLength) = firstBlock;
     }
-}
\ No newline at end of file
+}
+
+#endif  // JIMINY_HELPERS_HXX
diff --git a/core/include/jiminy/core/utilities/json.hxx b/core/include/jiminy/core/utilities/json.hxx
index d2f722099..227a21b69 100644
--- a/core/include/jiminy/core/utilities/json.hxx
+++ b/core/include/jiminy/core/utilities/json.hxx
@@ -26,7 +26,7 @@ namespace jiminy
     }
 
     template<typename T>
-    Json::Value convertToJson(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> & value)
+    Json::Value convertToJson(const MatrixX<T> & value)
     {
         Json::Value mat(Json::arrayValue);
         if (value.rows() > 0)
@@ -55,14 +55,14 @@ namespace jiminy
     Json::Value convertToJson<HeightmapFunctor>(const HeightmapFunctor & value);
 
     template<typename T, typename A>
-    constexpr std::enable_if_t<!is_eigen_v<T>, const char *>
+    constexpr std::enable_if_t<!is_eigen_any_v<T>, const char *>
     getJsonVectorType(const std::vector<T, A> & /* value */)
     {
         return "unknown";
     }
 
     template<typename T, typename A>
-    constexpr std::enable_if_t<is_eigen_v<T>, const char *>
+    constexpr std::enable_if_t<is_eigen_any_v<T>, const char *>
     getJsonVectorType(const std::vector<T, A> & /* value */)
     {
         return "list(array)";
@@ -102,10 +102,8 @@ namespace jiminy
     // ************* Conversion from JSON utilities *****************
 
     template<typename T>
-    std::enable_if_t<!is_vector_v<T>, T> convertFromJson(const Json::Value & /* value */)
-    {
-        T::undefined_template_specialization_for_this_type;
-    }
+    std::enable_if_t<!is_vector_v<T> && !is_eigen_vector_v<T>, T>
+    convertFromJson(const Json::Value & /* value */) = delete;
 
     template<>
     std::string convertFromJson<std::string>(const Json::Value & value);
@@ -122,8 +120,20 @@ namespace jiminy
     template<>
     double convertFromJson<double>(const Json::Value & value);
 
-    template<>
-    Eigen::VectorXd convertFromJson<Eigen::VectorXd>(const Json::Value & value);
+    template<typename T>
+    std::enable_if_t<is_eigen_vector_v<T>, T> convertFromJson(const Json::Value & value)
+    {
+        T vec{};
+        if (value.size() > 0)
+        {
+            vec.resize(value.size());
+            for (auto it = value.begin(); it != value.end(); ++it)
+            {
+                vec[it.index()] = convertFromJson<typename T::Scalar>(*it);
+            }
+        }
+        return vec;
+    }
 
     template<>
     Eigen::MatrixXd convertFromJson<Eigen::MatrixXd>(const Json::Value & value);
diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h
index 64f8e9b2b..ec70a226a 100644
--- a/core/include/jiminy/core/utilities/random.h
+++ b/core/include/jiminy/core/utilities/random.h
@@ -1,86 +1,320 @@
 #ifndef JIMINY_RANDOM_H
 #define JIMINY_RANDOM_H
 
-#include <optional>
+#include <array>     // `std::array`
+#include <memory>    // `std::unique_ptr`
+#include <optional>  // `std::optional`
+#include <utility>   // `std::pair`, `std::declval`
+#include <vector>    // `std::vector`
 
-#include "jiminy/core/fwd.h"
+#include <type_traits>  // `std::enable_if_t`, `std::is_same_v`, `std::is_convertible_v`, `std::is_unsigned_v`
+
+#include "jiminy/core/fwd.h"  // `remove_cvref_t`
 
 
 namespace jiminy
 {
-    // ************ Random number generator utilities ***************
+    // ***************************** Uniform random bit generators ***************************** //
+
+    /// @brief 32-bits Permuted Congruential Generator (PCG) random number generator. This
+    ///        generator has excellent statistical quality while being much faster than Mersenne
+    ///        Twister (std::mt19937) and having acceptable period (2^62).
+    ///
+    /// @details The PCG random number generation scheme has been developed by Melissa O'Neill. The
+    ///          technical details can be found in his origin paper, "PCG: A Family of Simple Fast
+    ///          Space-Efficient Statistically Good Algorithms for Random Number Generation.",
+    ///          2014. For additional information, please visit: http://www.pcg-random.org
+    ///
+    /// @warning This implementation has not been vectorized by leveraging SIMD instructions. As
+    ///          such, all platforms are supported out-of-the-box and yield reproducible results,
+    ///          at the cost of running significantly slower to when it comes to sampling enough
+    ///          data at once (e.g. AVX512 operates on 16 floats at once). This use-case is largely
+    ///          irrelevant at the time being for this project, and as such, is out of its scope.
+    ///          For completeness, here is a another fully vectorized implementation:
+    ///          https://github.com/lemire/simdpcg
+    ///
+    /// @sa The proposed implementation is a minimal version of `pcg32_fast` released under the
+    ///     Apache License, Version 2.0: https://github.com/imneme/pcg-cpp
+    class JIMINY_DLLAPI PCG32
+    {
+    public:
+        using result_type = uint32_t;
+
+        explicit PCG32(uint64_t state = 0xcafef00dd15ea5e5ULL) noexcept;
+        template<typename SeedSeq,
+                 typename = decltype(std::declval<SeedSeq &>().generate(
+                     std::declval<uint_least32_t *>(), std::declval<uint_least32_t *>())),
+                 typename = std::enable_if_t<!std::is_same_v<remove_cvref_t<SeedSeq>, PCG32> &&
+                                             !std::is_convertible_v<SeedSeq, uint64_t> &&
+                                             std::is_unsigned_v<typename SeedSeq::result_type>>>
+        explicit PCG32(SeedSeq && seed_seq) noexcept;
+
+        explicit PCG32(PCG32 && other) = default;
+        explicit PCG32(PCG32 & other) = default;
+        PCG32 & operator=(const PCG32 & other) = default;
+
+        template<typename SeedSeq>
+        void seed(SeedSeq && seed_seq) noexcept;
+
+        result_type operator()() noexcept;
+
+        constexpr static result_type min() noexcept
+        {
+            return std::numeric_limits<result_type>::min();
+        }
+        constexpr static result_type max() noexcept
+        {
+            return std::numeric_limits<result_type>::max();
+        }
+
+    private:
+        uint64_t state_;
+    };
 
-    void JIMINY_DLLAPI resetRandomGenerators(
-        const std::optional<uint32_t> & seed = std::nullopt) noexcept;
+    /// \brief Lightweight non-owning wrapper around a random generator without seeding capability.
+    ///
+    /// \details It allows for type erasure of the underlying random generator, at the cost of
+    ///          having to enforce return_type and min/max bounds at compile time. This enables
+    ///          supporting passing any random generator to virtual methods of polymorphic classes.
+    ///          For convenience, the exposed 'operator()' is const-qualified, which implies that
+    ///          sampling from a 'const uniform_random_bit_generator_ref<T> &' is permitted.
+    ///
+    /// \note The user is responsible to adapt the return type of a random generator if it does not
+    ///       match the requirement of this wrapper. `std::independent_bits_engine` adaptor is the
+    ///       right way to do so if copying the original generator is not an issue.
+    ///
+    /// \sa For technical reference about type-erasure for random generators:
+    ///     https://stackoverflow.com/a/77809228/4820605
+    template<typename ResultType,
+             ResultType min_ = std::numeric_limits<ResultType>::min(),
+             ResultType max_ = std::numeric_limits<ResultType>::max()>
+    class uniform_random_bit_generator_ref : private function_ref<ResultType()>
+    {
+    public:
+        using result_type = ResultType;
+
+        template<typename F
+                 //  ,typename = std::enable_if_t<std::bool_constant<
+                 //      std::decay_t<F>::min() == min_ && std::decay_t<F>::max() == max_>::value>
+                 >
+        constexpr uniform_random_bit_generator_ref(F && f) noexcept :
+        function_ref<result_type()>(f)
+        {
+        }
 
-    hresult_t JIMINY_DLLAPI getRandomSeed(uint32_t & seed);
+        using function_ref<result_type()>::operator();
 
-    double JIMINY_DLLAPI randUniform(double lo = 0.0, double hi = 1.0);
+        static constexpr result_type min() noexcept { return min_; }
+        static constexpr result_type max() noexcept { return max_; }
+    };
+
+    // ****************************** Random number distributions ****************************** //
+
+    /// \brief Lightweight wrapper around random number distributions and generators to make them
+    ///        appear as functors compatible with `Eigen::CwiseNullaryOp` for generating random
+    ///        expressions procedurally.
+    ///
+    /// \details It extends the capability of the original `Eigen::internal::scalar_random_op`
+    ///          functor to support specifying a custom distribution along with the accompanying
+    ///          random number generator.
+    ///
+    /// \sa For technical reference about saving packed arguments for later use:
+    ///     https://stackoverflow.com/q/7858817/4820605
+    template<typename F, typename... StackedArgs>
+    class scalar_random_op;
 
-    double JIMINY_DLLAPI randNormal(double mean = 0.0, double std = 1.0);
+    template<typename R, typename... DerivedArgs, typename... StackedArgs>
+    class scalar_random_op<R(DerivedArgs...), StackedArgs...>
+    {
+    public:
+        // FIXME: Add 'noexcept' specifier once Eigen>=3.4.0 is enforced as strict requirement
+        constexpr scalar_random_op(
+            const scalar_random_op<R(DerivedArgs...), StackedArgs...> & rhs) = default;
+
+        template<typename... StackedArgsIn>
+        constexpr scalar_random_op(R (*f)(DerivedArgs...), StackedArgsIn &&... args) :
+        fun_(f),
+        args_(std::forward<StackedArgsIn>(args)...)
+        {
+        }
 
-    Eigen::VectorXd JIMINY_DLLAPI randVectorNormal(uint32_t size, double mean, double std);
+        constexpr scalar_random_op<R(DerivedArgs...), StackedArgs...> & operator=(
+            const scalar_random_op<R(DerivedArgs...), StackedArgs...> & rhs) = default;
 
-    Eigen::VectorXd JIMINY_DLLAPI randVectorNormal(uint32_t size, double std);
+        template<typename F,
+                 typename = std::enable_if_t<!std::is_same_v<std::decay_t<F>, scalar_random_op>>>
+        scalar_random_op<R(DerivedArgs...), StackedArgs...> & operator=(F && f) = delete;
 
-    Eigen::VectorXd JIMINY_DLLAPI randVectorNormal(const Eigen::VectorXd & std);
+        R operator()(Eigen::Index i, Eigen::Index j = 0) const
+        {
+            return coeff(i, j, std::index_sequence_for<DerivedArgs...>{});
+        }
 
-    Eigen::VectorXd JIMINY_DLLAPI randVectorNormal(const Eigen::VectorXd & mean,
-                                                   const Eigen::VectorXd & std);
+    private:
+        template<std::size_t... Is>
+        R coeff(Eigen::Index i, Eigen::Index j, std::integer_sequence<std::size_t, Is...>) const
+        {
+            // Arguments bound by value are passed with const qualifier. The const qualifier is not
+            // removed and therefore must honored when being forwarded to the callable, which must
+            // accept such arguments either by value or const reference. This behavior is desirable
+            // as it would be error-prone to cast-away constness, otherwise the user may expect the
+            // original object to be modified while actually only the copy will be.
+            // FIXME: Replacing `std::integral_constant` by template-lambda when moving to C++20
+            return fun_(
+                [&](auto I) -> decltype(auto)
+                {
+                    // Extract the right stacked argument
+                    auto & arg = std::get<I>(args_);
+
+                    // Coeff-wise dispatching when forwarding stacked arguments to the callable for
+                    // those that are not implicitly convertible to the original type.
+                    using StackedArg = std::tuple_element_t<I, decltype(args_)>;
+                    using DerivedArg = std::tuple_element_t<I, std::tuple<DerivedArgs...>>;
+                    if constexpr (std::is_convertible_v<StackedArg, DerivedArg>)
+                    {
+                        // if constexpr (!std::is_lvalue_reference_v<StackedArg>)
+                        // {
+                        //     return const_cast<std::add_lvalue_reference_t<StackedArg>>(arg);
+                        // }
+                        return arg;
+                    }
+                    else
+                    {
+                        return arg(i, j);
+                    }
+                }(std::integral_constant<std::size_t, Is>{})...);
+        }
 
-    void JIMINY_DLLAPI shuffleIndices(std::vector<uint32_t> & vector);
+    private:
+        R (*fun_)(DerivedArgs...);
+        std::conditional_t<
+            (sizeof...(StackedArgs) > 0),
+            std::tuple<StackedArgs...>,
+            std::tuple<std::conditional_t<std::is_arithmetic_v<std::decay_t<DerivedArgs>>,
+                                          MatrixX<std::decay_t<DerivedArgs>>,
+                                          DerivedArgs>...>>
+            args_;
+    };
+}
 
-    // ************ Continuous 1D Perlin processes ***************
+namespace Eigen::internal
+{
+    template<typename R, typename Generator, typename... DerivedArgs, typename... StackedArgs>
+    struct functor_traits<jiminy::scalar_random_op<R(DerivedArgs...), Generator, StackedArgs...>>
+    {
+        enum
+        {
+            Cost = 5 * Eigen::NumTraits<std::decay_t<R>>::MulCost,
+            PacketAccess = false,
+            IsRepeatable = false
+        };
+    };
+}
 
-    namespace details
+namespace jiminy
+{
+    float JIMINY_DLLAPI uniform(const uniform_random_bit_generator_ref<uint32_t> & g);
+
+    float JIMINY_DLLAPI uniform(
+        const uniform_random_bit_generator_ref<uint32_t> & g, float lo, float hi);
+
+    template<typename Generator, typename Derived1, typename Derived2>
+    std::enable_if_t<
+        (is_eigen_any_v<Derived1> ||
+         is_eigen_any_v<Derived2>)&&(!std::is_arithmetic_v<std::decay_t<Derived1>> ||
+                                     !std::is_arithmetic_v<std::decay_t<Derived2>>),
+        Eigen::CwiseNullaryOp<
+            scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                             Generator &,
+                             Derived1,
+                             Derived2>,
+            Eigen::MatrixXf>>
+    uniform(Generator & g, Derived1 && lo, Derived2 && hi);
+
+    template<typename Generator>
+    Eigen::CwiseNullaryOp<
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         const Eigen::MatrixXf::ConstantReturnType,
+                         const Eigen::MatrixXf::ConstantReturnType>,
+        Eigen::MatrixXf>
+    uniform(
+        Eigen::Index nrows, Eigen::Index ncols, Generator & g, float lo = 0.0F, float hi = 1.0F);
+
+    /// @details The original Ziggurat algorithm for single-precision floating-point scalars is
+    ///          used internally. This method is known to be about x4 times faster than the
+    ///          Box–Muller transform but significantly more complex to implement and more
+    ///          notably to vectorize using SIMD instructions. For details about these methods:
+    ///          https://en.wikipedia.org/wiki/Ziggurat_algorithm
+    ///          https://en.wikipedia.org/wiki/Box–Muller_transform
+    ///
+    /// @warning This implementation has been optimized for sampling individual scalar here and
+    ///          there, rather than all at once in a vector. The proposed PCG32 random number
+    ///          generator is consistent with this design choice. Fully vectorized implementations
+    ///          of Ziggurat algorithm and Box-Muller transform that supports both x86 and
+    ///          Arm64-Neon intrinsics are publicly available here:
+    ///          https://github.com/lfarizav/pseudorandomnumbergenerators
+    ///          It you are looking for fully vectorized implementation of some other statistical
+    ///          distributions, have a look to this project: https://github.com/bab2min/EigenRand
+    ///
+    /// @sa Based on the original implementation by Marsaglia and Tsang (JSS, 2000):
+    ///     https://people.sc.fsu.edu/~jburkardt/cpp_src/ziggurat/ziggurat.html
+    ///     This implementation is known to fail some standard statistical tests as described
+    ///     by Doornik (2005). This is not an issue for most applications. For reference:
+    ///     https://www.doornik.com/research/ziggurat.pdf
+    float JIMINY_DLLAPI normal(const uniform_random_bit_generator_ref<uint32_t> & g,
+                               float mean = 0.0F,
+                               float stddev = 1.0F);
+
+    /// \details Enforcing a common interface to all scalar statistical distribution by means of
+    /// `uniform_random_bit_generator_ref` does not incur any overhead when compiling with
+    /// optimizations enabled (level 01 is enough), probably due to inlining.
+    template<typename Generator, typename Derived1, typename Derived2>
+    std::enable_if_t<
+        (is_eigen_any_v<Derived1> ||
+         is_eigen_any_v<Derived2>)&&(!std::is_arithmetic_v<std::decay_t<Derived1>> ||
+                                     !std::is_arithmetic_v<std::decay_t<Derived2>>),
+        Eigen::CwiseNullaryOp<
+            scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                             Generator &,
+                             Derived1,
+                             Derived2>,
+            Eigen::MatrixXf>>
+    normal(Generator & g, Derived1 && mean, Derived2 && stddev);
+
+    template<typename Generator>
+    Eigen::CwiseNullaryOp<
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         const Eigen::MatrixXf::ConstantReturnType,
+                         const Eigen::MatrixXf::ConstantReturnType>,
+        Eigen::MatrixXf>
+    normal(Eigen::Index nrows,
+           Eigen::Index ncols,
+           Generator & g,
+           float mean = 0.0F,
+           float stddev = 1.0F);
+
+    // **************************** Continuous 1D Gaussian processes *************************** //
+
+    namespace internal
     {
         /// \brief Lower Cholesky factor of a Toeplitz positive semi-definite matrix having 1.0 on
-        /// its
-        ///        main diagonal.
+        ///        its main diagonal.
         ///
         /// \details In practice, it is advisable to combine this algorithm with Tikhonov
         ///          regularization of relative magnitude 1e-9 to avoid numerical instabilities
         ///          because of double machine precision.
         ///
-        /// \see Michael Stewart, Cholesky factorization of semi-definite Toeplitz matrices. Linear
-        ///      Algebra and its Applications, Volume 254, pages 497-525, 1997.
-        ///
-        /// \see
-        /// https://people.sc.fsu.edu/~jburkardt/cpp_src/toeplitz_cholesky/toeplitz_cholesky.html
+        /// \sa Michael Stewart, Cholesky factorization of semi-definite Toeplitz matrices. Linear
+        ///     Algebra and its Applications, Volume 254, pages 497-525, 1997.
+        ///     The original implementation is available here:
+        ///     https://people.sc.fsu.edu/~jburkardt/cpp_src/toeplitz_cholesky/toeplitz_cholesky.html
         ///
-        /// \param[in] a First row of the matrix to decompose.
-        template<typename DerivedType>
-        MatrixX<typename DerivedType::Scalar>
-        standardToeplitzCholeskyLower(const Eigen::MatrixBase<DerivedType> & coeffs)
-        {
-            using Scalar = typename DerivedType::Scalar;
-
-            // Initialize lower Cholesky factor
-            const Eigen::Index n = coeffs.size();
-            MatrixX<Scalar> l{n, n};
-
-            /* Compute compressed representation of the matrix.
-               It coincides with the Schur generator for Toepliz matrices. */
-            Eigen::Matrix<Scalar, 2, Eigen::Dynamic> g{2, n};
-            g.rowwise() = coeffs.transpose();
-
-            // Run progressive Schur algorithm, adapted to Toepliz matrices
-            l.col(0) = g.row(0);
-            g.row(0).tail(n - 1) = g.row(0).head(n - 1).eval();
-            Eigen::Matrix<Scalar, 2, 2> H{Eigen::Matrix<Scalar, 2, 2>::Ones()};
-            for (Eigen::Index i = 1; i < n; ++i)
-            {
-                const double rho = -g(1, i) / g(0, i);
-                // H << 1.0, rho,
-                //      rho, 1.0;
-                Eigen::Map<Eigen::Vector4d>(H.data()).template segment<2>(1).fill(rho);
-                g.rightCols(n - i) = H * g.rightCols(n - i) / std::sqrt((1.0 - rho) * (1.0 + rho));
-                l.col(i).tail(n - i) = g.row(0).tail(n - i);
-                g.row(0).tail(n - i - 1) = g.row(0).segment(i, n - i - 1).eval();
-            }
-
-            return l;
-        }
+        /// \param[in] coeffs First row of the matrix to decompose.
+        template<typename Derived>
+        MatrixX<typename Derived::Scalar>
+        standardToeplitzCholeskyLower(const Eigen::MatrixBase<Derived> & coeffs);
     }
 
     class JIMINY_DLLAPI PeriodicGaussianProcess
@@ -89,43 +323,41 @@ namespace jiminy
         DISABLE_COPY(PeriodicGaussianProcess)
 
     public:
-        explicit PeriodicGaussianProcess(
-            double wavelength, double period, double scale = 1.0) noexcept;
+        explicit PeriodicGaussianProcess(double wavelength, double period) noexcept;
 
-        void reset();
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept;
 
-        double operator()(const float & t);
+        double operator()(float t);
 
-        double getWavelength() const;
-        double getPeriod() const;
-        double getDt() const;
+        double wavelength() const noexcept;
+        double period() const noexcept;
 
     private:
         const double wavelength_;
         const double period_;
-        const double scale_;
+
         const double dt_{0.02 * wavelength_};
-        const int numTimes_{static_cast<int>(std::ceil(period_ / dt_))};
+        const Eigen::Index num_times_{static_cast<int>(std::ceil(period_ / dt_))};
 
-        Eigen::VectorXd values_{numTimes_};
         /// \brief Cholesky decomposition (LLT) of the covariance matrix.
         ///
         /// \details All decompositions are equivalent as the covariance matrix is symmetric,
-        ///          namely eigen-values, singular-values, Cholesky and Schur decompositions.
-        ///          Yet, Cholesky is by far the most efficient one.
-        ///          See: https://math.stackexchange.com/q/22825/375496
-        ///          Moreover, the covariance is a positive semi-definite Toepliz matrix, which
-        ///          means that the computational complexity can be reduced even further using an
-        ///          specialized Cholesky decomposition algorithm. */
-        Eigen::MatrixXd covSqrtRoot_{
-            details::standardToeplitzCholeskyLower(Eigen::VectorXd::NullaryExpr(
-                numTimes_,
-                [numTimes = numTimes_, wavelength = wavelength_](double i) {
+        ///          namely eigen-values, singular-values, Cholesky and Schur decompositions. Yet,
+        ///          Cholesky is by far the most efficient one. Moreover, the covariance is a
+        ///          positive semi-definite Toepliz matrix, which means that the computational
+        ///          complexity can be reduced even further using an specialized Cholesky
+        ///          decomposition algorithm. See: https://math.stackexchange.com/q/22825/375496
+        Eigen::MatrixXd cov_sqrt_root_{
+            internal::standardToeplitzCholeskyLower(Eigen::VectorXd::NullaryExpr(
+                num_times_,
+                [numTimes = static_cast<double>(num_times_), wavelength = wavelength_](double i) {
                     return std::exp(-2.0 *
                                     std::pow(std::sin(M_PI / numTimes * i) / wavelength, 2));
                 }))};
+        Eigen::VectorXd values_{num_times_};
     };
 
+    // **************************** Continuous 1D Fourier processes **************************** //
 
     /// \see Based on "Smooth random functions, random ODEs, and Gaussian processes":
     ///      https://hal.inria.fr/hal-01944992/file/random_revision2.pdf */
@@ -135,63 +367,65 @@ namespace jiminy
         DISABLE_COPY(PeriodicFourierProcess)
 
     public:
-        explicit PeriodicFourierProcess(
-            double wavelength, double period, double scale = 1.0) noexcept;
+        explicit PeriodicFourierProcess(double wavelength, double period) noexcept;
 
-        void reset();
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept;
 
-        double operator()(const float & t);
+        double operator()(float t);
 
-        double getWavelength() const;
-        double getPeriod() const;
-        int getNumHarmonics() const;
-        double getDt() const;
+        double wavelength() const noexcept;
+        double period() const noexcept;
 
     private:
         const double wavelength_;
         const double period_;
-        const double scale_;
+
         const double dt_{0.02 * wavelength_};
-        const int numTimes_{static_cast<int>(std::ceil(period_ / dt_))};
-        const int numHarmonics_{static_cast<int>(std::ceil(period_ / wavelength_))};
-
-        Eigen::VectorXd values_{numTimes_};
-        const Eigen::MatrixXd cosMat_{
-            Eigen::MatrixXd::NullaryExpr(numTimes_,
-                                         numHarmonics_,
-                                         [numTimes = numTimes_](double i, double j)
-                                         { return std::cos(2 * M_PI / numTimes * i * j); })};
-        const Eigen::MatrixXd sinMat_{
-            Eigen::MatrixXd::NullaryExpr(numTimes_,
-                                         numHarmonics_,
-                                         [numTimes = numTimes_](double i, double j)
-                                         { return std::sin(2 * M_PI / numTimes * i * j); })};
+        const Eigen::Index num_times_{static_cast<Eigen::Index>(std::ceil(period_ / dt_))};
+        const Eigen::Index num_harmonics_{
+            static_cast<Eigen::Index>(std::ceil(period_ / wavelength_))};
+
+        const Eigen::MatrixXd cos_mat_{Eigen::MatrixXd::NullaryExpr(
+            num_times_,
+            num_harmonics_,
+            [numTimes = static_cast<double>(num_times_)](double i, double j)
+            { return std::cos(2 * M_PI / numTimes * i * j); })};
+        const Eigen::MatrixXd sin_mat_{Eigen::MatrixXd::NullaryExpr(
+            num_times_,
+            num_harmonics_,
+            [numTimes = static_cast<double>(num_times_)](double i, double j)
+            { return std::sin(2 * M_PI / numTimes * i * j); })};
+        Eigen::VectorXd values_{num_times_};
     };
 
+    // ***************************** Continuous 1D Perlin processes **************************** //
+
     class JIMINY_DLLAPI AbstractPerlinNoiseOctave
     {
     public:
-        explicit AbstractPerlinNoiseOctave(double wavelength, double scale) noexcept;
+        explicit AbstractPerlinNoiseOctave(double wavelength);
         virtual ~AbstractPerlinNoiseOctave() = default;
 
-        virtual void reset();
+        virtual void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept;
 
         double operator()(double t) const;
 
-        double getWavelength() const;
-        double getScale() const;
+        double wavelength() const noexcept;
 
     protected:
-        // Copy on purpose
-        virtual double grad(int32_t knot, double delta) const = 0;
+        virtual double grad(int32_t knot, double delta) const noexcept = 0;
 
-        double fade(double delta) const;
-
-        double lerp(double ratio, double yLeft, double yRight) const;
+        /// @brief Improved Smoothstep function by Ken Perlin (aka Smootherstep).
+        ///
+        /// @details It has zero 1st and 2nd-order derivatives at dt = 0.0, and 1.0.
+        ///
+        /// @sa For reference, see:
+        ///     https://en.wikipedia.org/wiki/Smoothstep#Variations
+        static double fade(double delta) noexcept;
+        static double lerp(double ratio, double y_left, double y_right) noexcept;
 
     protected:
         const double wavelength_;
-        const double scale_;
 
         double shift_{0.0};
     };
@@ -199,28 +433,30 @@ namespace jiminy
     class JIMINY_DLLAPI RandomPerlinNoiseOctave : public AbstractPerlinNoiseOctave
     {
     public:
-        using AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave;
-        virtual ~RandomPerlinNoiseOctave() = default;
+        explicit RandomPerlinNoiseOctave(double wavelength);
+        ~RandomPerlinNoiseOctave() override = default;
 
-        virtual void reset() override final;
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept override;
 
     protected:
-        virtual double grad(int32_t knot, double delta) const override final;
+        double grad(int32_t knot, double delta) const noexcept override;
 
     private:
-        uint32_t seed_{0};
+        uint32_t seed_{0U};
     };
 
     class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : public AbstractPerlinNoiseOctave
     {
     public:
-        explicit PeriodicPerlinNoiseOctave(double wavelength, double period, double scale);
-        virtual ~PeriodicPerlinNoiseOctave() = default;
+        explicit PeriodicPerlinNoiseOctave(double wavelength, double period);
+        ~PeriodicPerlinNoiseOctave() override = default;
+
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept override;
 
-        virtual void reset() override final;
+        double period() const noexcept;
 
     protected:
-        virtual double grad(int32_t knot, double delta) const override final;
+        double grad(int32_t knot, double delta) const noexcept override;
 
     private:
         const double period_;
@@ -253,63 +489,56 @@ namespace jiminy
     public:
         DISABLE_COPY(AbstractPerlinProcess)
 
+        using OctaveScalePair =
+            std::pair<std::unique_ptr<AbstractPerlinNoiseOctave>, const double>;
+
     public:
-        void reset();
+        void reset(const uniform_random_bit_generator_ref<uint32_t> & g) noexcept;
 
-        double operator()(const float & t);
+        double operator()(float t);
 
-        double getWavelength() const noexcept;
-        std::size_t getNumOctaves() const noexcept;
-        double getScale() const noexcept;
+        double wavelength() const noexcept;
+        std::size_t num_octaves() const noexcept;
 
     protected:
         explicit AbstractPerlinProcess(
-            double scale,
-            std::vector<std::unique_ptr<AbstractPerlinNoiseOctave>> && octaves) noexcept;
+            std::vector<OctaveScalePair> && octave_scale_pairs) noexcept;
 
     protected:
-        const double scale_;
+        std::vector<OctaveScalePair> octave_scale_pairs_;
 
-        std::vector<std::unique_ptr<AbstractPerlinNoiseOctave>> octaves_;
+    private:
         double amplitude_{0.0};
-
-        double grad_{0.0};
     };
 
     class JIMINY_DLLAPI RandomPerlinProcess : public AbstractPerlinProcess
     {
     public:
-        explicit RandomPerlinProcess(
-            double wavelength, double scale = 1.0, std::size_t numOctaves = 6U);
+        explicit RandomPerlinProcess(double wavelength, std::size_t num_octaves = 6U);
     };
 
     class PeriodicPerlinProcess : public AbstractPerlinProcess
     {
     public:
         explicit PeriodicPerlinProcess(
-            double wavelength, double period, double scale = 1.0, std::size_t numOctaves = 6U);
+            double wavelength, double period, std::size_t num_octaves = 6U);
 
-        double getPeriod() const noexcept;
+        double period() const noexcept;
 
     private:
         const double period_;
     };
 
-    // ************ Random terrain generators ***************
-
-    HeightmapFunctor JIMINY_DLLAPI randomTileGround(const Eigen::Vector2d & size,
-                                                    double heightMax,
-                                                    const Eigen::Vector2d & interpDelta,
-                                                    uint32_t sparsity,
-                                                    double orientation,
-                                                    uint32_t seed);
+    // ******************************* Random terrain generators ******************************* //
 
-    HeightmapFunctor JIMINY_DLLAPI sumHeightmap(const std::vector<HeightmapFunctor> & heightmaps);
-    HeightmapFunctor JIMINY_DLLAPI mergeHeightmap(
-        const std::vector<HeightmapFunctor> & heightmaps);
-
-    Eigen::MatrixXd JIMINY_DLLAPI discretizeHeightmap(
-        const HeightmapFunctor & heightmap, double gridSize, double gridUnit);
+    HeightmapFunctor JIMINY_DLLAPI tiles(const Eigen::Vector2d & size,
+                                         double height_max,
+                                         const Eigen::Vector2d & interp_delta,
+                                         uint32_t sparsity,
+                                         double orientation,
+                                         uint32_t seed);
 }
 
+#include "jiminy/core/utilities/random.hxx"
+
 #endif  // JIMINY_RANDOM_H
diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx
new file mode 100644
index 000000000..8c31dc8d6
--- /dev/null
+++ b/core/include/jiminy/core/utilities/random.hxx
@@ -0,0 +1,189 @@
+#ifndef JIMINY_RANDOM_HXX
+#define JIMINY_RANDOM_HXX
+
+#include <utility>  // `std::forward`
+
+#include "jiminy/core/fwd.h"
+
+
+namespace jiminy
+{
+    // ***************************** Uniform random bit generators ***************************** //
+
+    namespace internal
+    {
+        template<std::size_t I = 0UL, typename SeedSeq>
+        inline uint64_t generateState(SeedSeq && seed_seq) noexcept
+        {
+            constexpr std::size_t kN = I + 1UL;
+            constexpr std::size_t kDestSize = sizeof(uint64_t);
+            constexpr std::size_t kGenSize = sizeof(uint32_t);
+            constexpr std::size_t kSrcBits = kGenSize * 8;
+            constexpr std::size_t kScale = (kDestSize + kGenSize - 1) / kGenSize;
+            constexpr std::size_t kFromElems = kN * kScale;
+
+            std::array<uint64_t, kN> result;
+            std::array<uint32_t, kFromElems> buffer;
+            seed_seq.generate(buffer.begin(), buffer.end());
+            auto buffer_it = buffer.cbegin();
+            for (uint64_t & dest : result)
+            {
+                uint64_t value{0UL};
+                uint32_t shift{0U};
+                for (std::size_t j = 0; j < kScale; ++j)
+                {
+                    value |= static_cast<uint64_t>(*(buffer_it++)) << shift;
+                    shift += static_cast<uint32_t>(kSrcBits);
+                }
+                dest = value;
+            }
+            return result[I];
+        }
+    }
+
+    template<typename SeedSeq, typename, typename>
+    PCG32::PCG32(SeedSeq && seed_seq) noexcept :
+    PCG32(internal::generateState(seed_seq))
+    {
+    }
+
+    template<typename SeedSeq>
+    void PCG32::seed(SeedSeq && seed_seq) noexcept
+    {
+        new (this) PCG32(std::forward<SeedSeq>(seed_seq));
+    }
+
+    // ****************************** Random number distributions ****************************** //
+
+    template<typename Generator, typename Derived1, typename Derived2>
+    std::enable_if_t<
+        (is_eigen_any_v<Derived1> ||
+         is_eigen_any_v<Derived2>)&&(!std::is_arithmetic_v<std::decay_t<Derived1>> ||
+                                     !std::is_arithmetic_v<std::decay_t<Derived2>>),
+        Eigen::CwiseNullaryOp<
+            scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                             Generator &,
+                             Derived1,
+                             Derived2>,
+            Eigen::MatrixXf>>
+    uniform(Generator & g, Derived1 && lo, Derived2 && hi)
+    {
+        Eigen::Index nrows, ncols;
+        if constexpr (is_eigen_any_v<Derived1>)
+        {
+            nrows = lo.rows();
+            ncols = lo.cols();
+        }
+        else
+        {
+            nrows = hi.rows();
+            ncols = hi.cols();
+        }
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         Derived1,
+                         Derived2>
+            op{uniform, g, lo, hi};
+        return Eigen::MatrixXf::NullaryExpr(nrows, ncols, op);
+    }
+
+    template<typename Generator>
+    Eigen::CwiseNullaryOp<
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         const Eigen::MatrixXf::ConstantReturnType,
+                         const Eigen::MatrixXf::ConstantReturnType>,
+        Eigen::MatrixXf>
+    uniform(Eigen::Index nrows, Eigen::Index ncols, Generator & g, float lo, float hi)
+    {
+        return uniform(g,
+                       Eigen::MatrixXf::Constant(nrows, ncols, lo),
+                       Eigen::MatrixXf::Constant(nrows, ncols, hi));
+    }
+
+    template<typename Generator, typename Derived1, typename Derived2>
+    std::enable_if_t<
+        (is_eigen_any_v<Derived1> ||
+         is_eigen_any_v<Derived2>)&&(!std::is_arithmetic_v<std::decay_t<Derived1>> ||
+                                     !std::is_arithmetic_v<std::decay_t<Derived2>>),
+        Eigen::CwiseNullaryOp<
+            scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                             Generator &,
+                             Derived1,
+                             Derived2>,
+            Eigen::MatrixXf>>
+    normal(Generator & g, Derived1 && mean, Derived2 && stddev)
+    {
+        Eigen::Index nrows, ncols;
+        if constexpr (is_eigen_any_v<Derived1>)
+        {
+            nrows = mean.rows();
+            ncols = mean.cols();
+        }
+        else
+        {
+            nrows = stddev.rows();
+            ncols = stddev.cols();
+        }
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         Derived1,
+                         Derived2>
+            op{normal, g, mean, stddev};
+        return Eigen::MatrixXf::NullaryExpr(nrows, ncols, op);
+    }
+
+    template<typename Generator>
+    Eigen::CwiseNullaryOp<
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         Generator &,
+                         const Eigen::MatrixXf::ConstantReturnType,
+                         const Eigen::MatrixXf::ConstantReturnType>,
+        Eigen::MatrixXf>
+    normal(Eigen::Index nrows, Eigen::Index ncols, Generator & g, float mean, float stddev)
+    {
+        return normal(g,
+                      Eigen::MatrixXf::Constant(nrows, ncols, mean),
+                      Eigen::MatrixXf::Constant(nrows, ncols, stddev));
+    }
+
+    // **************************** Continuous 1D Gaussian processes *************************** //
+
+    namespace internal
+    {
+        template<typename Derived>
+        MatrixX<typename Derived::Scalar>
+        standardToeplitzCholeskyLower(const Eigen::MatrixBase<Derived> & coeffs)
+        {
+            using Scalar = typename Derived::Scalar;
+
+            // Initialize lower Cholesky factor
+            const Eigen::Index n = coeffs.size();
+            MatrixX<Scalar> l{n, n};
+
+            /* Compute compressed representation of the matrix.
+               It coincides with the Schur generator for Toepliz matrices. */
+            Eigen::Matrix<Scalar, 2, Eigen::Dynamic> g{2, n};
+            g.rowwise() = coeffs.transpose();
+
+            // Run progressive Schur algorithm, adapted to Toepliz matrices
+            l.col(0) = g.row(0);
+            g.row(0).tail(n - 1) = g.row(0).head(n - 1).eval();
+            Eigen::Matrix<Scalar, 2, 2> H{Eigen::Matrix<Scalar, 2, 2>::Ones()};
+            for (Eigen::Index i = 1; i < n; ++i)
+            {
+                const double rho = -g(1, i) / g(0, i);
+                // H << 1.0, rho,
+                //      rho, 1.0;
+                Eigen::Vector4d::Map(H.data()).template segment<2>(1).fill(rho);
+                g.rightCols(n - i) = H * g.rightCols(n - i) / std::sqrt((1.0 - rho) * (1.0 + rho));
+                l.col(i).tail(n - i) = g.row(0).tail(n - i);
+                g.row(0).tail(n - i - 1) = g.row(0).segment(i, n - i - 1).eval();
+            }
+
+            return l;
+        }
+    }
+}
+
+#endif  // JIMINY_RANDOM_HXX
diff --git a/core/src/constraints/frame_constraint.cc b/core/src/constraints/frame_constraint.cc
index c5d80d04b..fd6979be6 100644
--- a/core/src/constraints/frame_constraint.cc
+++ b/core/src/constraints/frame_constraint.cc
@@ -124,7 +124,27 @@ namespace jiminy
         // Assuming the model still exists.
         auto model = model_.lock();
 
-        // Get jacobian in local frame
+        /* Get jacobian in local frame:
+
+           In general, for the angular part we have:
+               p_c = log3(R_f),
+               v_c = J_log3 * v_f = J_log3 * J_f * dq,
+               a_c = d(J_log3)/dt * v_f + J_log3 * a_f
+                   = d(J_log3)/dt * v_f + J_log3 * a_f^0 + (J_log3 * J_f) * ddq
+                     where a_f^0 = d(J_f)/dt * dq
+           It means:
+               jac = J_log3 * J_f ,
+               drift = d(J_log3)/dt * v_f + J_log3 * a_f^0
+           Yet, we have the identity:
+               d(log3)/dt = J_log3 * v_f = v_f
+           It follows:
+               d(J_log3)/dt * v_f = 0,
+               J_log3 * a_f^0 = a_f^0
+           Hence, it yields:
+               jac = J_f,
+               drift = a_f^0
+
+           For reference, see: https://github.com/duburcqa/jiminy/pull/603 */
         const pinocchio::SE3 & framePose = model->pncData_.oMf[frameIdx_];
         const pinocchio::SE3 transformLocal(rotationLocal_, framePose.translation());
         const pinocchio::Frame & frame = model->pncModel_.frames[frameIdx_];
diff --git a/core/src/control/abstract_controller.cc b/core/src/control/abstract_controller.cc
index eadf09d44..6a57590fa 100644
--- a/core/src/control/abstract_controller.cc
+++ b/core/src/control/abstract_controller.cc
@@ -178,7 +178,8 @@ namespace jiminy
             registeredVariables,
         bool isTelemetryConfigured,
         const std::vector<std::string> & fieldnames,
-        const Eigen::Ref<Eigen::Matrix<Scalar, -1, 1>, 0, Eigen::InnerStride<>> & values)
+        const Eigen::Ref<VectorX<Scalar>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
+            values)
     {
         if (isTelemetryConfigured)
         {
@@ -207,7 +208,8 @@ namespace jiminy
 
     hresult_t AbstractController::registerVariable(
         const std::vector<std::string> & fieldnames,
-        const Eigen::Ref<Eigen::Matrix<double, -1, 1>, 0, Eigen::InnerStride<>> & values)
+        const Eigen::Ref<VectorX<double>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
+            values)
     {
         return registerVariableImpl<double>(
             registeredVariables_, isTelemetryConfigured_, fieldnames, values);
@@ -215,7 +217,8 @@ namespace jiminy
 
     hresult_t AbstractController::registerVariable(
         const std::vector<std::string> & fieldnames,
-        const Eigen::Ref<Eigen::Matrix<int64_t, -1, 1>, 0, Eigen::InnerStride<>> & values)
+        const Eigen::Ref<VectorX<int64_t>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
+            values)
     {
         return registerVariableImpl<int64_t>(
             registeredVariables_, isTelemetryConfigured_, fieldnames, values);
diff --git a/core/src/engine/engine_multi_robot.cc b/core/src/engine/engine_multi_robot.cc
index 74666fe93..1c054ed9f 100644
--- a/core/src/engine/engine_multi_robot.cc
+++ b/core/src/engine/engine_multi_robot.cc
@@ -27,9 +27,7 @@
 
 #include "jiminy/core/telemetry/fwd.h"
 #include "jiminy/core/hardware/fwd.h"
-#include "jiminy/core/utilities/helpers.h"
 #include "jiminy/core/utilities/pinocchio.h"
-#include "jiminy/core/utilities/random.h"
 #include "jiminy/core/utilities/json.h"
 #include "jiminy/core/io/file_device.h"
 #include "jiminy/core/io/serialization.h"
@@ -59,6 +57,7 @@ namespace jiminy
     inline constexpr uint32_t PGS_MAX_ITERATIONS{100U};
 
     EngineMultiRobot::EngineMultiRobot() noexcept :
+    generator_{std::seed_seq{std::random_device{}()}},
     telemetrySender_{std::make_unique<TelemetrySender>()},
     telemetryData_{std::make_shared<TelemetryData>()},
     telemetryRecorder_{std::make_unique<TelemetryRecorder>()}
@@ -211,7 +210,7 @@ namespace jiminy
         if (returnCode == hresult_t::SUCCESS)
         {
             // Get the system index
-            std::ptrdiff_t systemIdx;
+            std::ptrdiff_t systemIdx{};
             getSystemIdx(systemName, systemIdx);  // Cannot fail at this point
 
             // Update the systems' indices for the remaining coupling forces
@@ -900,13 +899,14 @@ namespace jiminy
         // Reset the random number generators
         if (resetRandomNumbers)
         {
-            resetRandomGenerators(engineOptions_->stepper.randomSeed);
+            VectorX<uint32_t> seedSeq = engineOptions_->stepper.randomSeedSeq;
+            generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size()));
         }
 
         // Reset the internal state of the robot and controller
         for (auto & system : systems_)
         {
-            system.robot->reset();
+            system.robot->reset(generator_);
             system.controller->reset();
         }
 
@@ -1592,6 +1592,9 @@ namespace jiminy
                         return hresult_t::ERROR_GENERIC;
                     }
 
+                    // Compute all external terms including joints accelerations and forces
+                    computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);
+
                     // Compute the sensor data with the updated effort and acceleration
                     systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
 
@@ -1618,7 +1621,7 @@ namespace jiminy
                 isFirstIter = false;
             }
 
-            // Update sensor data one last time to take into account the actual acceleration
+            // Update sensor data one last time to take into account the actual motor effort
             systemIt = systems_.begin();
             systemDataIt = systemsDataHolder_.begin();
             for (; systemIt != systems_.end(); ++systemIt, ++systemDataIt)
@@ -1631,9 +1634,6 @@ namespace jiminy
                 systemIt->robot->setSensorsData(t, q, v, a, uMotor, fext);
             }
 
-            // Compute all external terms including joints accelerations and forces if requested
-            computeAllExtraTerms(systems_, systemsDataHolder_, fPrev_);
-
             // Backend the updated joint accelerations and forces
             syncAllAccelerationsAndForces(systems_, contactForcesPrev_, fPrev_, aPrev_);
 
@@ -1968,7 +1968,7 @@ namespace jiminy
         bool hasDynamicsChanged = false;
 
         // Start the timer used for timeout handling
-        timer_->tic();
+        timer_.tic();
 
         // Perform the integration. Do not simulate extremely small time steps.
         while ((tEnd - t >= STEPPER_MIN_TIMESTEP) && (returnCode == hresult_t::SUCCESS))
@@ -2204,9 +2204,8 @@ namespace jiminy
                     }
 
                     // Break the loop in case of timeout. The error code will be set later.
-                    timer_->toc();
                     if (EPS < engineOptions_->stepper.timeout &&
-                        engineOptions_->stepper.timeout < timer_->dt)
+                        engineOptions_->stepper.timeout < timer_.toc())
                     {
                         break;
                     }
@@ -2470,9 +2469,8 @@ namespace jiminy
                             "integrate physics further in time. Aborting integration.");
                 returnCode = hresult_t::ERROR_GENERIC;
             }
-            timer_->toc();
             if (EPS < engineOptions_->stepper.timeout &&
-                engineOptions_->stepper.timeout < timer_->dt)
+                engineOptions_->stepper.timeout < timer_.toc())
             {
                 PRINT_ERROR("Step computation timeout. Aborting integration.");
                 returnCode = hresult_t::ERROR_GENERIC;
@@ -2610,26 +2608,28 @@ namespace jiminy
     }
 
     template<typename... Args>
-    std::tuple<bool, double>
-    isGcdIncluded(const vector_aligned_t<systemDataHolder_t> & systemsDataHolder, Args... values)
+    std::tuple<bool, const double &> isGcdIncluded(
+        const vector_aligned_t<systemDataHolder_t> & systemsDataHolder, const Args &... values)
     {
         if (systemsDataHolder.empty())
         {
-            return isGcdIncluded(std::forward<Args>(values)...);
+            return isGcdIncluded(values...);
         }
 
-        double minValue = INF;
-        auto lambda = [&minValue, &values...](const systemDataHolder_t & systemData)
+        const double * minValuePtr = &INF;
+        auto lambda = [&minValuePtr, &values...](const systemDataHolder_t & systemData)
         {
-            auto [isIncluded, value] = isGcdIncluded(
-                systemData.forcesProfile.begin(),
-                systemData.forcesProfile.end(),
-                [](ForceProfile const & force) { return force.updatePeriod; },
-                std::forward<Args>(values)...);
-            minValue = minClipped(minValue, value);
+            auto && [isIncluded, value] = isGcdIncluded(
+                systemData.forcesProfile.cbegin(),
+                systemData.forcesProfile.cend(),
+                [](ForceProfile const & force) -> const double & { return force.updatePeriod; },
+                values...);
+            minValuePtr = &(minClipped(*minValuePtr, value));
             return isIncluded;
         };
-        return {std::all_of(systemsDataHolder.begin(), systemsDataHolder.end(), lambda), minValue};
+        // FIXME: Order of evaluation is not always respected with MSVC.
+        bool isIncluded = std::all_of(systemsDataHolder.begin(), systemsDataHolder.end(), lambda);
+        return {isIncluded, *minValuePtr};
     }
 
     hresult_t EngineMultiRobot::registerForceProfile(const std::string & systemName,
@@ -2964,10 +2964,12 @@ namespace jiminy
 
         /* Reset random number generators if setOptions is called for the first time, or if the
            desired random seed has changed. */
-        const uint32_t randomSeed = boost::get<uint32_t>(stepperOptions.at("randomSeed"));
-        if (!engineOptions_ || randomSeed != engineOptions_->stepper.randomSeed)
+        const VectorX<uint32_t> & seedSeq =
+            boost::get<VectorX<uint32_t>>(stepperOptions.at("randomSeedSeq"));
+        if (!engineOptions_ || seedSeq.size() != engineOptions_->stepper.randomSeedSeq.size() ||
+            (seedSeq.array() != engineOptions_->stepper.randomSeedSeq.array()).any())
         {
-            resetRandomGenerators(randomSeed);
+            generator_.seed(std::seed_seq(seedSeq.data(), seedSeq.data() + seedSeq.size()));
         }
 
         // Update the internal options
@@ -3305,14 +3307,14 @@ namespace jiminy
         const pinocchio::SE3 & transformFrameInWorld = data.oMf[frameIdx];
 
         // Compute the ground normal and penetration depth at the contact point
+        double heightGround;
+        Eigen::Vector3d normalGround;
         const Eigen::Vector3d & posFrame = transformFrameInWorld.translation();
-        auto ground = engineOptions_->world.groundProfile(posFrame);
-        const double zGround = std::get<double>(ground);
-        Eigen::Vector3d & nGround = std::get<Eigen::Vector3d>(ground);
-        nGround.normalize();  // Make sure the ground normal is normalized
+        engineOptions_->world.groundProfile(posFrame.head<2>(), heightGround, normalGround);
+        normalGround.normalize();  // Make sure the ground normal is normalized
 
         // First-order projection (exact assuming no curvature)
-        const double depth = (posFrame[2] - zGround) * nGround[2];
+        const double depth = (posFrame[2] - heightGround) * normalGround[2];
 
         // Only compute the ground reaction force if the penetration depth is negative
         if (depth < 0.0)
@@ -3328,7 +3330,7 @@ namespace jiminy
 
                 // Compute the ground reaction force in world frame (local world aligned)
                 const pinocchio::Force fextAtContactInGlobal =
-                    computeContactDynamics(nGround, depth, vContactInWorld);
+                    computeContactDynamics(normalGround, depth, vContactInWorld);
 
                 // Deduce the ground reaction force in joint frame
                 fextLocal =
@@ -3362,13 +3364,13 @@ namespace jiminy
         {
             auto & frameConstraint = static_cast<FrameConstraint &>(*constraint.get());
             frameConstraint.setReferenceTransform(
-                {transformFrameInWorld.rotation(), posFrame - depth * nGround});
-            frameConstraint.setNormal(nGround);
+                {transformFrameInWorld.rotation(), posFrame - depth * normalGround});
+            frameConstraint.setNormal(normalGround);
         }
     }
 
     pinocchio::Force EngineMultiRobot::computeContactDynamics(
-        const Eigen::Vector3d & nGround,
+        const Eigen::Vector3d & normalGround,
         double depth,
         const Eigen::Vector3d & vContactInWorld) const
     {
@@ -3381,15 +3383,15 @@ namespace jiminy
             const contactOptions_t & contactOptions_ = engineOptions_->contacts;
 
             // Compute the penetration speed
-            const double vDepth = vContactInWorld.dot(nGround);
+            const double vDepth = vContactInWorld.dot(normalGround);
 
             // Compute normal force
             const double fextNormal = -std::min(
                 contactOptions_.stiffness * depth + contactOptions_.damping * vDepth, 0.0);
-            fextInWorld.noalias() = fextNormal * nGround;
+            fextInWorld.noalias() = fextNormal * normalGround;
 
             // Compute friction forces
-            const Eigen::Vector3d vTangential = vContactInWorld - vDepth * nGround;
+            const Eigen::Vector3d vTangential = vContactInWorld - vDepth * normalGround;
             const double vRatio =
                 std::min(vTangential.norm() / contactOptions_.transitionVelocity, 1.0);
             const double fextTangential = contactOptions_.friction * vRatio * fextNormal;
@@ -4314,7 +4316,7 @@ namespace jiminy
             [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data)
                 -> herr_t
             {
-                auto [_numInt, _numFloat] =
+                auto & [_numInt, _numFloat] =
                     *static_cast<std::pair<int64_t &, int64_t &> *>(op_data);
                 H5::Group fieldGroup = H5::Group(group).openGroup(name);
                 const H5::DataSet valueDataset = fieldGroup.openDataSet("value");
@@ -4334,15 +4336,13 @@ namespace jiminy
         // Pre-allocate memory
         logData.integerValues.resize(numInt, numData);
         logData.floatValues.resize(numFloat, numData);
-        Eigen::Matrix<int64_t, Eigen::Dynamic, 1> intVector(numData);
-        Eigen::Matrix<double, Eigen::Dynamic, 1> floatVector(numData);
+        VectorX<int64_t> intVector(numData);
+        VectorX<double> floatVector(numData);
         logData.variableNames.reserve(1 + numInt + numFloat);
         logData.variableNames.emplace_back(GLOBAL_TIME);
 
         // Read all variables while preserving ordering
-        using opDataT = std::tuple<LogData &,
-                                   Eigen::Matrix<int64_t, Eigen::Dynamic, 1> &,
-                                   Eigen::Matrix<double, Eigen::Dynamic, 1> &>;
+        using opDataT = std::tuple<LogData &, VectorX<int64_t> &, VectorX<double> &>;
         opDataT opData{logData, intVector, floatVector};
         H5Literate(
             variablesGroup.getId(),
@@ -4352,7 +4352,7 @@ namespace jiminy
             [](hid_t group, const char * name, const H5L_info_t * /* oinfo */, void * op_data)
                 -> herr_t
             {
-                auto [_logData, _intVector, _floatVector] = *static_cast<opDataT *>(op_data);
+                auto & [_logData, _intVector, _floatVector] = *static_cast<opDataT *>(op_data);
                 const Eigen::Index varIdx = _logData.variableNames.size() - 1;
                 const int64_t _numInt = _logData.integerValues.rows();
                 H5::Group fieldGroup = H5::Group(group).openGroup(name);
@@ -4457,8 +4457,8 @@ namespace jiminy
         }
 
         // Temporary contiguous storage for variables
-        Eigen::Matrix<int64_t, Eigen::Dynamic, 1> intVector;
-        Eigen::Matrix<double, Eigen::Dynamic, 1> floatVector;
+        VectorX<int64_t> intVector;
+        VectorX<double> floatVector;
 
         // Get the number of integer and float variables
         const Eigen::Index numInt = logData->integerValues.rows();
diff --git a/core/src/hardware/abstract_sensor.cc b/core/src/hardware/abstract_sensor.cc
index 801a0e507..2c78beb0b 100644
--- a/core/src/hardware/abstract_sensor.cc
+++ b/core/src/hardware/abstract_sensor.cc
@@ -1,4 +1,3 @@
-#include "jiminy/core/utilities/random.h"
 #include "jiminy/core/telemetry/telemetry_sender.h"
 #include "jiminy/core/robot/robot.h"
 
@@ -8,6 +7,7 @@
 namespace jiminy
 {
     AbstractSensorBase::AbstractSensorBase(const std::string & name) noexcept :
+    generator_{std::seed_seq{std::random_device{}()}},
     name_{name},
     telemetrySender_{std::make_unique<TelemetrySender>()}
     {
@@ -71,7 +71,8 @@ namespace jiminy
         // Add white noise
         if (baseSensorOptions_->noiseStd.size())
         {
-            get() += randVectorNormal(baseSensorOptions_->noiseStd);
+            get() += normal(generator_, 0.0F, baseSensorOptions_->noiseStd.cast<float>())
+                         .cast<double>();
         }
 
         // Add bias
diff --git a/core/src/hardware/basic_sensors.cc b/core/src/hardware/basic_sensors.cc
index d9146d585..e1298af21 100644
--- a/core/src/hardware/basic_sensors.cc
+++ b/core/src/hardware/basic_sensors.cc
@@ -189,8 +189,8 @@ namespace jiminy
         // Add measurement white noise
         if (baseSensorOptions_->noiseStd.size())
         {
-            // Accel + gyroscope: simply apply additive noise
-            get() += randVectorNormal(baseSensorOptions_->noiseStd);
+            get() += normal(generator_, 0.0F, baseSensorOptions_->noiseStd.cast<float>())
+                         .cast<double>();
         }
 
         // Add measurement bias
diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc
index 9c36dd14f..6f76c1f00 100644
--- a/core/src/robot/model.cc
+++ b/core/src/robot/model.cc
@@ -32,7 +32,6 @@
 #include "jiminy/core/constraints/sphere_constraint.h"
 #include "jiminy/core/constraints/frame_constraint.h"
 #include "jiminy/core/utilities/pinocchio.h"
-#include "jiminy/core/utilities/random.h"
 #include "jiminy/core/utilities/helpers.h"
 
 #include "jiminy/core/robot/model.h"
@@ -261,7 +260,7 @@ namespace jiminy
         {
             // Assume the model is fully initialized at this point
             isInitialized_ = true;
-            returnCode = generateModelBiased();
+            returnCode = generateModelBiased(std::random_device{});
         }
 
         /* Add joint constraints.
@@ -326,7 +325,7 @@ namespace jiminy
         return returnCode;
     }
 
-    void Model::reset()
+    void Model::reset(const uniform_random_bit_generator_ref<uint32_t> & g)
     {
         /* Do NOT reset the constraints, since it will be handled by the engine.
            It is necessary, since the current model state is required to initialize the
@@ -340,7 +339,7 @@ namespace jiminy
             generateModelFlexible();
 
             // Update the biases added to the dynamics properties of the model
-            generateModelBiased();
+            generateModelBiased(g);
         }
     }
 
@@ -410,7 +409,7 @@ namespace jiminy
             /* One must re-generate the model after adding a frame.
                Note that, since the added frame being the "last" of the model, the proxies are
                still up-to-date and therefore it is unecessary to call 'reset'. */
-            generateModelBiased();
+            generateModelBiased(std::random_device{});
 
             // Restore the current rotor inertias and effort limits
             pncModel_.rotorInertia.swap(rotorInertia);
@@ -476,7 +475,7 @@ namespace jiminy
             pncDataOrig_ = pinocchio::Data(pncModelOrig_);
 
             // One must reset the model after removing a frame
-            reset();
+            reset(std::random_device{});
         }
 
         return returnCode;
@@ -1087,7 +1086,7 @@ namespace jiminy
         return returnCode;
     }
 
-    hresult_t Model::generateModelBiased()
+    hresult_t Model::generateModelBiased(const uniform_random_bit_generator_ref<uint32_t> & g)
     {
         hresult_t returnCode = hresult_t::SUCCESS;
 
@@ -1118,22 +1117,24 @@ namespace jiminy
                 const pinocchio::JointIndex jointIdx = pncModel_.getJointId(jointName);
 
                 // Add bias to com position
-                const double comBiasStd = mdlOptions_->dynamics.centerOfMassPositionBodiesBiasStd;
+                const float comBiasStd =
+                    static_cast<float>(mdlOptions_->dynamics.centerOfMassPositionBodiesBiasStd);
                 if (comBiasStd > EPS)
                 {
                     Eigen::Vector3d & comRelativePositionBody =
                         pncModel_.inertias[jointIdx].lever();
                     comRelativePositionBody.array() *=
-                        1.0 + randVectorNormal(3U, comBiasStd).array();
+                        normal(3, 1, g, 1.0F, comBiasStd).array().cast<double>();
                 }
 
                 /* Add bias to body mass.
                    It cannot be less than min(original mass, 1g) for numerical stability. */
-                const double massBiasStd = mdlOptions_->dynamics.massBodiesBiasStd;
+                const float massBiasStd =
+                    static_cast<float>(mdlOptions_->dynamics.massBodiesBiasStd);
                 if (massBiasStd > EPS)
                 {
                     double & massBody = pncModel_.inertias[jointIdx].mass();
-                    massBody = std::max(massBody * (1.0 + randNormal(0.0, massBiasStd)),
+                    massBody = std::max(massBody * normal(g, 1.0F, massBiasStd),
                                         std::min(massBody, 1.0e-3));
                 }
 
@@ -1144,18 +1145,20 @@ namespace jiminy
                    small rotation is applied to the principal axes based on a randomly generated
                    rotation axis. Finally, the biased inertia matrix is obtained doing
                    `A @ diag(M) @ A.T`. If no bias, the original inertia matrix is recovered. */
-                const double inertiaBiasStd = mdlOptions_->dynamics.inertiaBodiesBiasStd;
+                const float inertiaBiasStd =
+                    static_cast<float>(mdlOptions_->dynamics.inertiaBodiesBiasStd);
                 if (inertiaBiasStd > EPS)
                 {
                     pinocchio::Symmetric3 & inertiaBody = pncModel_.inertias[jointIdx].inertia();
                     Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(inertiaBody.matrix());
                     Eigen::Vector3d inertiaBodyMoments = solver.eigenvalues();
                     Eigen::Matrix3d inertiaBodyAxes = solver.eigenvectors();
-                    const Eigen::Vector3d randAxis = randVectorNormal(3U, inertiaBiasStd);
+                    const Eigen::Vector3d randAxis =
+                        normal(3, 1, g, 0.0F, inertiaBiasStd).cast<double>();
                     inertiaBodyAxes =
                         inertiaBodyAxes * Eigen::Quaterniond(pinocchio::exp3(randAxis));
                     inertiaBodyMoments.array() *=
-                        1.0 + randVectorNormal(3U, inertiaBiasStd).array();
+                        normal(3, 1, g, 1.0F, inertiaBiasStd).array().cast<double>();
                     inertiaBody =
                         pinocchio::Symmetric3((inertiaBodyAxes * inertiaBodyMoments.asDiagonal() *
                                                inertiaBodyAxes.transpose())
@@ -1163,14 +1166,14 @@ namespace jiminy
                 }
 
                 // Add bias to relative body position (rotation excluded !)
-                const double relativeBodyPosBiasStd =
-                    mdlOptions_->dynamics.relativePositionBodiesBiasStd;
+                const float relativeBodyPosBiasStd =
+                    static_cast<float>(mdlOptions_->dynamics.relativePositionBodiesBiasStd);
                 if (relativeBodyPosBiasStd > EPS)
                 {
                     Eigen::Vector3d & relativePositionBody =
                         pncModel_.jointPlacements[jointIdx].translation();
                     relativePositionBody.array() *=
-                        1.0 + randVectorNormal(3U, relativeBodyPosBiasStd).array();
+                        normal(3, 1, g, 1.0F, relativeBodyPosBiasStd).array().cast<double>();
                 }
             }
 
@@ -1767,7 +1770,7 @@ namespace jiminy
         if (areModelsInvalid)
         {
             // Trigger models regeneration
-            reset();
+            reset(std::random_device{});
         }
         else if (internalBuffersMustBeUpdated)
         {
diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc
index 4bb96c9b8..9d7fd39ec 100644
--- a/core/src/robot/robot.cc
+++ b/core/src/robot/robot.cc
@@ -53,10 +53,10 @@ namespace jiminy
         return Model::initialize(pncModel, collisionModel, visualModel);
     }
 
-    void Robot::reset()
+    void Robot::reset(const uniform_random_bit_generator_ref<uint32_t> & g)
     {
         // Reset the model
-        Model::reset();
+        Model::reset(g);
 
         // Reset the motors
         if (!motorsHolder_.empty())
@@ -69,7 +69,7 @@ namespace jiminy
         {
             if (!sensorsGroupItem.second.empty())
             {
-                (*sensorsGroupItem.second.begin())->resetAll();
+                (*sensorsGroupItem.second.begin())->resetAll(g());
             }
         }
 
diff --git a/core/src/utilities/geometry.cc b/core/src/utilities/geometry.cc
new file mode 100644
index 000000000..3fa5677f4
--- /dev/null
+++ b/core/src/utilities/geometry.cc
@@ -0,0 +1,748 @@
+#include "hpp/fcl/BVH/BVH_model.h"  // `hpp::fcl::CollisionGeometry`, `hpp::fcl::BVHModel`, `hpp::fcl::OBBRSS`
+#include "hpp/fcl/shape/geometric_shapes.h"  // `hpp::fcl::Halfspace`
+#include "hpp/fcl/hfield.h"                  // `hpp::fcl::HeightField`
+#include "jiminy/core/utilities/geometry.h"
+
+
+namespace jiminy
+{
+    namespace internal
+    {
+        const double TOL = 1e-8;
+
+        using Vector10d = Eigen::Matrix<double, 10, 1>;
+
+        class Symmetric4
+        {
+        public:
+            Symmetric4() = default;
+
+            template<typename Derived>
+            Symmetric4(const Eigen::MatrixBase<Derived> & data) :
+            data_(data){EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(Derived, 10)
+
+            }
+
+            Symmetric4(const Eigen::Vector3d & n, const double & d) :
+            data_()
+            {
+                // Make plane a x + b y + c z + d = 0, i.e. Kp = p * p.T with p = [a, b, c, d]
+                const double a = n[0], b = n[1], c = n[2];
+                data_ << a * a, a * b, a * c, a * d, b * b, b * c, b * d, c * c, c * d, d * d;
+            }
+
+            static Symmetric4 Zero() { return Symmetric4(Vector10d::Zero()); }
+
+            void setZero() { data_.setZero(); }
+
+            const Vector10d & data() const { return data_; }
+
+            Vector10d & data() { return data_; }
+
+            Symmetric4 operator+(const Symmetric4 & other) const
+            {
+                return Symmetric4(data_ + other.data_);
+            }
+
+            Symmetric4 & operator+=(const Symmetric4 & other)
+            {
+                data_ += other.data_;
+                return *this;
+            }
+
+            template<int i, int j>
+            double coeff() const
+            {
+                constexpr int k = (j >= i) ? 4 * i - (i * (i + 1)) / 2 + j :
+                                             4 * j - (j * (j + 1)) / 2 + i;
+                return data_[k];
+            }
+
+            Eigen::Matrix4d matrix() const
+            {
+                // clang-format off
+                return (Eigen::Matrix4d() <<
+                    coeff<0, 0>(), coeff<0, 1>(), coeff<0, 2>(), coeff<0, 3>(),
+                    coeff<1, 0>(), coeff<1, 1>(), coeff<1, 2>(), coeff<1, 3>(),
+                    coeff<2, 0>(), coeff<2, 1>(), coeff<2, 2>(), coeff<2, 3>(),
+                    coeff<3, 0>(), coeff<3, 1>(), coeff<3, 2>(), coeff<3, 3>()
+                ).finished();
+                // clang-format on
+            }
+
+            /// @brief Compute the minor defined as the determinant of the sub-matrix formed by
+            ///        deleting row i and col j
+            template<int i, int j>
+            double cofactor() const
+            {
+                // clang-format off
+                constexpr int i1 = (i + 1) % 4, i2 = (i + 2) % 4, i3 = (i + 3) % 4;
+                constexpr int j1 = (j + 1) % 4, j2 = (j + 2) % 4, j3 = (j + 3) % 4;
+                double minor =
+                    coeff<i1, j1>() * (coeff<i2, j2>() * coeff<i3, j3>() - coeff<i2, j3>() * coeff<i3, j2>()) +
+                    coeff<i1, j2>() * (coeff<i2, j3>() * coeff<i3, j1>() - coeff<i2, j1>() * coeff<i3, j3>()) +
+                    coeff<i1, j3>() * (coeff<i2, j1>() * coeff<i3, j2>() - coeff<i2, j2>() * coeff<i3, j1>());
+                if constexpr ((i + j) & 1)
+                {
+                    minor *= -1;
+                }
+                return minor;
+                // clang-format on
+            }
+
+            double vtQv(const Eigen::Vector3d & v) const
+            {
+                // clang-format off
+                // [v_x, v_y, v_z, 1].T * Q * [v_x, v_y, v_z, 1]
+                const double x = v[0], y = v[1], z = v[2];
+                return data_[0] * x * x + data_[4] * y * y + data_[7] * z * z + 2 * (
+                       data_[1] * x * y + data_[2] * x * z + data_[5] * y * z +
+                       data_[3] * x + data_[6] * y + data_[8] * z) + data_[9];
+                // clang-format on
+            }
+
+        protected:
+            Vector10d data_{};
+        };
+
+        struct Triangle
+        {
+            std::array<std::size_t, 3> v;
+            std::array<double, 4> err;
+            Eigen::Vector3d n;
+            bool is_deleted;
+            bool is_dirty;
+        };
+
+        struct Vertex
+        {
+            Eigen::Vector3d p;
+            Symmetric4 q;
+            bool is_border;
+            std::size_t t_start;
+            std::size_t t_count;
+        };
+
+        struct Ref
+        {
+            std::size_t t_id;
+            std::size_t t_vertex;
+        };
+
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wmaybe-uninitialized"
+        /// @brief Fast mesh simplification utility.
+        ///
+        /// @details The original algorithm has been developed by Michael Garland and Paul
+        ///          Heckbert. The technical details can be found in their paper, "Surface
+        ///          Simplification Using Quadric Error Metrics.", 1997:
+        ///          http://www.cs.cmu.edu/~garland/Papers/quadrics.pdf
+        ///
+        /// @sa The proposed implementation is based on code from Sven Forstmann released in 2014
+        ///     under the MIT License:
+        ///     https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification
+        class MeshSimplifier
+        {
+        public:
+            MeshSimplifier(const hpp::fcl::Matrixx3f & verts, const hpp::fcl::Matrixx3i & tris)
+            {
+                vertices.reserve(verts.rows());
+                for (Eigen::Index i = 0; i < verts.rows(); ++i)
+                {
+                    // FIXME: Use designated initializer when moving to c++20
+                    Vertex v;
+                    v.p = verts.row(i);
+                    vertices.push_back(std::move(v));
+                }
+                triangles.reserve(tris.cols());
+                for (Eigen::Index i = 0; i < tris.rows(); ++i)
+                {
+                    // FIXME: Use designated initializer when moving to c++20
+                    Triangle t;
+                    Vector3<std::size_t>::Map(t.v.data()) = tris.row(i).cast<std::size_t>();
+                    triangles.push_back(std::move(t));
+                }
+            }
+
+            /// @brief Compute error for one specific edge.
+            std::pair<double, Eigen::Vector3d> computeError(std::size_t id_v1, std::size_t id_v2)
+            {
+                // Extract the relevant vertices
+                const Vertex & v1 = vertices[id_v1];
+                const Vertex & v2 = vertices[id_v2];
+
+                // compute interpolated vertex
+                const Symmetric4 q = v1.q + v2.q;
+                if (!(v1.is_border && v2.is_border))
+                {
+                    const double det = q.cofactor<3, 3>();
+                    if (det > TOL)
+                    {
+                        // q_delta is invertible
+                        Eigen::Vector3d p;
+                        p << q.cofactor<3, 0>(), q.cofactor<3, 1>(), q.cofactor<3, 2>();
+                        p /= det;
+
+                        // Compute resulting error
+                        const double err = p[0] * q.coeff<3, 0>() + p[1] * q.coeff<3, 1>() +
+                                           p[2] * q.coeff<3, 2>() + q.coeff<3, 3>();
+
+                        return std::make_pair(err, std::move(p));
+                    }
+                }
+
+                // det = 0 -> try to find best result
+                const Eigen::Vector3d & p1 = v1.p;
+                const double err1 = q.vtQv(p1);
+                if (err1 < TOL)
+                {
+                    return {err1, p1};
+                }
+                const Eigen::Vector3d & p2 = v2.p;
+                const double err2 = q.vtQv(p2);
+                if (err2 < TOL)
+                {
+                    return {err2, p2};
+                }
+                const Eigen::Vector3d p3 = 0.5 * (p1 + p2);
+                const double err3 = q.vtQv(p3);
+                if (err3 < err2)
+                {
+                    if (err3 < err1)
+                    {
+                        return {err3, p3};
+                    }
+                    return {err1, p1};
+                }
+                if (err2 < err1)
+                {
+                    return {err2, p2};
+                }
+                return {err1, p1};
+            }
+
+            /// @brief Check if a triangle flips when this edge is removed.
+            bool flipped(const Eigen::Vector3d & p,
+                         std::size_t i1,
+                         const Vertex & v0,
+                         std::vector<bool> & is_deleted)
+            {
+                for (std::size_t k = 0; k < v0.t_count; ++k)
+                {
+                    const Triangle & t = triangles[refs[v0.t_start + k].t_id];
+                    if (t.is_deleted)
+                    {
+                        continue;
+                    }
+
+                    const std::size_t s = refs[v0.t_start + k].t_vertex;
+                    const std::size_t id1 = t.v[(s + 1) % 3];
+                    const std::size_t id2 = t.v[(s + 2) % 3];
+                    if (id1 == i1 || id2 == i1)  // delete ?
+                    {
+                        is_deleted[k] = true;
+                        continue;
+                    }
+                    is_deleted[k] = false;
+
+                    if ((vertices[id1].p - p).cross(vertices[id2].p - p).dot(t.n) < 0.0)
+                    {
+                        return true;
+                    }
+                }
+                return false;
+            }
+
+            /// @brief Update triangle connections and edge error after a edge is collapsed.
+            void collapseTriangles(const std::size_t & i0,
+                                   const Vertex & v,
+                                   std::vector<bool> & is_deleted,
+                                   int64_t & num_deleted)
+            {
+                for (std::size_t k = 0; k < v.t_count; ++k)
+                {
+                    const Ref & r = refs[v.t_start + k];
+                    Triangle & t = triangles[r.t_id];
+                    if (t.is_deleted)
+                    {
+                        continue;
+                    }
+                    if (is_deleted[k])
+                    {
+                        t.is_deleted = true;
+                        num_deleted++;
+                        continue;
+                    }
+                    t.v[r.t_vertex] = i0;
+                    t.is_dirty = true;
+                    for (std::size_t j = 0; j < 3; ++j)
+                    {
+                        std::tie(t.err[j], std::ignore) = computeError(t.v[j], t.v[(j + 1) % 3]);
+                    }
+                    t.err[3] = std::min(t.err[0], std::min(t.err[1], t.err[2]));
+                    refs.push_back(r);
+                }
+            }
+
+            void update_mesh(int64_t iter)
+            {
+                // compact triangles, compute edge error and build reference list
+                if (iter > 0)
+                {
+                    std::size_t dst = 0;
+                    for (Triangle & t : triangles)
+                    {
+                        if (!t.is_deleted)
+                        {
+                            std::swap(triangles[dst++], t);
+                        }
+                    }
+                    triangles.resize(dst);
+                }
+
+                // Init Reference ID list
+                for (Vertex & v : vertices)
+                {
+                    v.t_start = 0;
+                    v.t_count = 0;
+                }
+                for (const Triangle & t : triangles)
+                {
+                    for (const std::size_t j : t.v)
+                    {
+                        ++vertices[j].t_count;
+                    }
+                }
+                std::size_t t_start = 0;
+                for (Vertex & v : vertices)
+                {
+                    v.t_start = t_start;
+                    t_start += v.t_count;
+                    v.t_count = 0;
+                }
+
+                // Write References
+                refs.resize(triangles.size() * 3);
+                for (std::size_t i = 0; i < triangles.size(); ++i)
+                {
+                    const Triangle & t = triangles[i];
+                    for (std::size_t j = 0; j < 3; ++j)
+                    {
+                        Vertex & v = vertices[t.v[j]];
+                        Ref & ref = refs[v.t_start + v.t_count];
+                        ref.t_id = i;
+                        ref.t_vertex = j;
+                        v.t_count++;
+                    }
+                }
+
+                if (iter == 0)
+                {
+                    // Identify boundary : vertices[].is_border=0,1
+                    for (Vertex & v : vertices)
+                    {
+                        v.is_border = false;
+                    }
+                    std::vector<std::pair<std::size_t, std::size_t>> v_info;  // id, count
+                    for (Vertex & v : vertices)
+                    {
+                        v_info.clear();  // Defined outside and cleared inside to optimize cache
+                        for (std::size_t j = 0; j < v.t_count; ++j)
+                        {
+                            const Triangle & t = triangles[refs[v.t_start + j].t_id];
+                            for (const std::size_t id : t.v)
+                            {
+                                auto v_info_it = std::find_if(
+                                    v_info.begin(),
+                                    v_info.end(),
+                                    [id](const std::pair<std::size_t, std::size_t> & item) -> bool
+                                    { return item.first == id; });
+                                if (v_info_it == v_info.end())
+                                {
+                                    v_info.emplace_back(id, 1);
+                                    continue;
+                                }
+                                ++(v_info_it->second);
+                            }
+                        }
+                        for (const auto & [id, count] : v_info)
+                        {
+                            if (count == 1)
+                            {
+                                vertices[id].is_border = true;
+                            }
+                        }
+                    }
+
+                    // Init Quadrics by Plane & Edge Errors.
+                    // It is required at first iteration but not during the simplification.
+                    // Yet, doing it anyway tends to improve the result for closed meshes.
+                    for (Vertex & v : vertices)
+                    {
+                        v.q.setZero();
+                    }
+
+                    for (Triangle & t : triangles)
+                    {
+                        const Eigen::Vector3d & p0 = vertices[t.v[0]].p;
+                        const Eigen::Vector3d & p1 = vertices[t.v[1]].p;
+                        const Eigen::Vector3d & p2 = vertices[t.v[2]].p;
+                        t.n = (p1 - p0).cross(p2 - p0).normalized();
+                        const Symmetric4 plan(t.n, -t.n.dot(p0));
+                        for (const std::size_t j : t.v)
+                        {
+                            vertices[j].q += plan;
+                        }
+                        for (std::size_t j = 0; j < 3; ++j)
+                        {
+                            Vertex & v0 = vertices[t.v[j]];
+                            Vertex & v1 = vertices[t.v[(j + 1) % 3]];
+                            if (v0.is_border && v1.is_border)
+                            {
+                                const Eigen::Vector3d n = t.n.cross(v1.p - v0.p).normalized();
+                                const Symmetric4 border(n, -n.dot(v0.p));
+                                v0.q += border;
+                                v1.q += border;
+                            }
+                        }
+                    }
+                    for (Triangle & t : triangles)
+                    {
+                        // Calc Edge Error
+                        for (std::size_t j = 0; j < 3; ++j)
+                        {
+                            std::tie(t.err[j], std::ignore) =
+                                computeError(t.v[j], t.v[(j + 1) % 3]);
+                        }
+                        t.err[3] = std::min(t.err[0], std::min(t.err[1], t.err[2]));
+                    }
+                }
+            }
+
+            /// @brief Main simplification function.
+            void simplify(std::size_t mesh_update_rate = 5,
+                          double aggressiveness = 7,
+                          int64_t max_iter = 100,
+                          double alpha = 1e-9,
+                          int64_t K = 3,
+                          bool preserve_border = false,
+                          bool verbose = false)
+            {
+                // init
+                for (Triangle & t : triangles)
+                {
+                    t.is_deleted = false;
+                }
+
+                // main iteration loop
+                int64_t num_deleted = 0;
+                std::vector<bool> deleted0;
+                std::vector<bool> deleted1;
+                std::size_t triangle_count = triangles.size();
+                for (int64_t iter = 0; iter < max_iter; iter++)
+                {
+                    // update mesh once in a while
+                    if (iter % mesh_update_rate == 0)
+                    {
+                        update_mesh(iter);
+                    }
+
+                    // clear is_dirty flag
+                    for (Triangle & t : triangles)
+                    {
+                        t.is_dirty = false;
+                    }
+
+                    // All triangles with edges below the threshold will be removed.
+                    // The following numbers works well for most models.
+                    // If it does not, try to adjust the 3 parameters.
+                    double threshold =
+                        alpha * std::pow(static_cast<double>(iter + K), aggressiveness);
+
+                    // target number of triangles reached ? Then break
+                    if (verbose)
+                    {
+                        std::cout << "iteration " << iter << " - triangles "
+                                  << triangle_count - num_deleted << " - threshold " << threshold
+                                  << std::endl;
+                    }
+
+                    // remove vertices & mark is_deleted triangles
+                    for (Triangle & t : triangles)
+                    {
+                        if (t.err[3] > threshold || t.is_deleted || t.is_dirty)
+                        {
+                            continue;
+                        }
+
+                        for (std::size_t j = 0; j < 3; ++j)
+                        {
+                            if (t.err[j] < threshold)
+                            {
+                                const std::size_t i0 = t.v[j];
+                                const std::size_t i1 = t.v[(j + 1) % 3];
+                                Vertex & v0 = vertices[i0];
+                                const Vertex & v1 = vertices[i1];
+
+                                // Border check
+                                if (v0.is_border != v1.is_border)
+                                {
+                                    // base behaviour
+                                    continue;
+                                }
+                                if (preserve_border && v0.is_border && v1.is_border &&
+                                    t.err[j] > TOL)
+                                {
+                                    continue;
+                                }
+
+                                // Compute vertex to collapse to
+                                Eigen::Vector3d p;
+                                std::tie(std::ignore, p) = computeError(i0, i1);
+                                deleted0.resize(v0.t_count);  // normals temporarily
+                                deleted1.resize(v1.t_count);  // normals temporarily
+                                // don't remove if flipped
+                                if (flipped(p, i1, v0, deleted0))
+                                {
+                                    continue;
+                                }
+                                if (flipped(p, i0, v1, deleted1))
+                                {
+                                    continue;
+                                }
+
+                                // not flipped, so remove edge
+                                v0.p = p;
+                                v0.q += v1.q;
+                                const std::size_t t_start = refs.size();
+                                collapseTriangles(i0, v0, deleted0, num_deleted);
+                                collapseTriangles(i0, v1, deleted1, num_deleted);
+                                const std::size_t t_count = refs.size() - t_start;
+                                if (t_count <= v0.t_count)
+                                {
+                                    if (t_count)
+                                    {
+                                        // save ram
+                                        std::move(std::next(refs.begin(), t_start),
+                                                  std::next(refs.begin(), t_start + t_count),
+                                                  std::next(refs.begin(), v0.t_start));
+                                    }
+                                }
+                                else
+                                {
+                                    // append
+                                    v0.t_start = t_start;
+                                }
+                                v0.t_count = t_count;
+                                break;
+                            }
+                        }
+                    }
+                }
+
+                if (verbose)
+                {
+                    std::cout << "done: triangles " << triangle_count - num_deleted << std::endl;
+                }
+
+                // Finally compact mesh before exiting
+                for (Vertex & v : vertices)
+                {
+                    v.t_count = 0;
+                }
+                std::size_t dst = 0;
+                for (Triangle & t : triangles)
+                {
+                    if (!t.is_deleted)
+                    {
+                        for (const std::size_t j : t.v)
+                        {
+                            vertices[j].t_count = 1;
+                        }
+                        std::swap(triangles[dst++], t);
+                    }
+                }
+                triangles.resize(dst);
+                dst = 0;
+                for (Vertex & v : vertices)
+                {
+                    if (v.t_count)
+                    {
+                        v.t_start = dst;
+                        std::swap(vertices[dst++].p, v.p);
+                    }
+                }
+                vertices.resize(dst);
+                for (Triangle & t : triangles)
+                {
+                    for (std::size_t & j : t.v)
+                    {
+                        j = vertices[j].t_start;
+                    }
+                }
+            }
+
+        public:
+            std::vector<Triangle> triangles{};
+            std::vector<Vertex> vertices{};
+            std::vector<Ref> refs{};
+        };
+    }
+#pragma GCC diagnostic pop
+
+    hpp::fcl::CollisionGeometryPtr_t discretizeHeightmap(const HeightmapFunctor & heightmap,
+                                                         double x_min,
+                                                         double x_max,
+                                                         double x_unit,
+                                                         double y_min,
+                                                         double y_max,
+                                                         double y_unit,
+                                                         bool must_simplify)
+    {
+        // Allocate vertices on a regular grid
+        const Eigen::Index x_dim =
+            static_cast<Eigen::Index>(std::round((x_max - x_min) / x_unit)) + 1;
+        const Eigen::Index y_dim =
+            static_cast<Eigen::Index>(std::round((y_max - y_min) / y_unit)) + 1;
+        hpp::fcl::Matrixx3f vertices(x_dim * y_dim, 3);
+
+        /* Fill x and y query coordinates over the grid.
+           Taking advantage of row-major storage order by default. */
+        Eigen::MatrixXd::Map(vertices.col(0).data(), x_dim, y_dim).colwise() =
+            Eigen::VectorXd::LinSpaced(x_dim, x_min, x_max);
+        Eigen::MatrixXd::Map(vertices.col(1).data(), x_dim, y_dim).rowwise() =
+            Eigen::VectorXd::LinSpaced(y_dim, y_min, y_max).transpose();
+
+        // Evaluate z coordinate over the grid
+        for (Eigen::Index i = 0; i < vertices.rows(); ++i)
+        {
+            auto vertex = vertices.row(i);
+            Eigen::Vector3d normal;
+            heightmap(vertex.head<2>(), vertex[2], normal);
+        }
+
+        // Check if the heightmap is flat
+        if (((vertices.col(2).array() - vertices(0, 2)).abs() < EPS).all())
+        {
+            hpp::fcl::CollisionGeometryPtr_t mesh_ptr(
+                new hpp::fcl::Halfspace(Eigen::Vector3d::UnitZ(), vertices(0, 2)));
+            mesh_ptr->computeLocalAABB();
+            return mesh_ptr;
+        }
+
+        // Return heightfield if no simplification requested and grid is centered
+        if (abs(x_min + x_max) < 1e-6 && abs(y_min + y_max) < 1e-6 && !must_simplify)
+        {
+            auto heights = Eigen::MatrixXd::Map(vertices.col(2).data(), x_dim, y_dim);
+            hpp::fcl::CollisionGeometryPtr_t mesh_ptr(new hpp::fcl::HeightField<hpp::fcl::OBBRSS>(
+                x_max - x_min,
+                y_max - y_min,
+                heights.transpose().colwise().reverse(),
+                vertices.col(2).minCoeff()));
+            mesh_ptr->computeLocalAABB();
+            return mesh_ptr;
+        }
+
+        // Compute the face indices
+        hpp::fcl::Matrixx3i triangles(2 * (x_dim - 1) * (y_dim - 1), 3);
+        Eigen::Index tri_index = 0;
+        for (Eigen::Index i = 0; i < x_dim - 1; ++i)
+        {
+            for (Eigen::Index j = 0; j < y_dim - 1; ++j)
+            {
+                const Eigen::Index k = j * x_dim + i;
+                triangles.middleRows<2>(tri_index) << k, k + 1, k + x_dim, k + 1, k + 1 + x_dim,
+                    k + x_dim;
+                tri_index += 2;
+            }
+        }
+
+        // Simplify the mesh if requested
+        if (must_simplify)
+        {
+            // The border must be preserved to avoid changing the boundary of the surface
+            internal::MeshSimplifier mesh_simplifier(vertices, triangles);
+            mesh_simplifier.simplify(4, 4, 21, 3.0e-9, 3, true, false);
+            vertices.resize(mesh_simplifier.vertices.size(), Eigen::NoChange);
+            for (Eigen::Index i = 0; i < vertices.rows(); ++i)
+            {
+                vertices.row(i) = mesh_simplifier.vertices[i].p;
+            }
+            triangles.resize(mesh_simplifier.triangles.size(), Eigen::NoChange);
+            for (Eigen::Index i = 0; i < triangles.rows(); ++i)
+            {
+                triangles.row(i) = Vector3<std::size_t>::Map(mesh_simplifier.triangles[i].v.data())
+                                       .cast<Eigen::Index>();
+            }
+        }
+
+        // Wrap the vertices and triangles in a geometry object
+        hpp::fcl::BVHModelPtr_t mesh_ptr(new hpp::fcl::BVHModel<hpp::fcl::OBBRSS>);
+        mesh_ptr->beginModel();
+        mesh_ptr->addVertices(vertices);
+        mesh_ptr->addTriangles(triangles);
+        mesh_ptr->endModel();
+        mesh_ptr->computeLocalAABB();
+        return mesh_ptr;
+    }
+
+    HeightmapFunctor sumHeightmaps(std::vector<HeightmapFunctor> heightmaps)
+    {
+        if (heightmaps.size() == 1)
+        {
+            return heightmaps[0];
+        }
+        return [heightmaps](
+                   const Eigen::Vector2d & pos, double & height, Eigen::Vector3d & normal) -> void
+        {
+            thread_local static double height_i;
+            thread_local static Eigen::Vector3d normal_i;
+
+            height = 0.0;
+            normal.setZero();
+            for (HeightmapFunctor const & heightmap : heightmaps)
+            {
+                heightmap(pos, height_i, normal_i);
+                height += height_i;
+                normal += normal_i;
+            }
+            normal.normalize();
+        };
+    }
+
+    HeightmapFunctor mergeHeightmaps(std::vector<HeightmapFunctor> heightmaps)
+    {
+        if (heightmaps.size() == 1)
+        {
+            return heightmaps[0];
+        }
+        return [heightmaps](
+                   const Eigen::Vector2d & pos, double & height, Eigen::Vector3d & normal) -> void
+        {
+            thread_local static double height_i;
+            thread_local static Eigen::Vector3d normal_i;
+
+            height = -INF;
+            bool is_dirty = false;
+            for (HeightmapFunctor const & heightmap : heightmaps)
+            {
+                heightmap(pos, height_i, normal_i);
+                if (std::abs(height_i - height) < EPS)
+                {
+                    normal += normal_i;
+                    is_dirty = true;
+                }
+                else if (height_i > height)
+                {
+                    height = height_i;
+                    normal = normal_i;
+                    is_dirty = false;
+                }
+            }
+            if (is_dirty)
+            {
+                normal.normalize();
+            }
+        };
+    }
+}
\ No newline at end of file
diff --git a/core/src/utilities/helpers.cc b/core/src/utilities/helpers.cc
index 603e11762..7234145ce 100644
--- a/core/src/utilities/helpers.cc
+++ b/core/src/utilities/helpers.cc
@@ -13,7 +13,7 @@
 
 namespace jiminy
 {
-    // *************** Local Mutex/Lock mechanism ******************
+    // ******************************* Local Mutex/Lock mechanism ****************************** //
 
     MutexLocal::~MutexLocal()
     {
@@ -36,7 +36,7 @@ namespace jiminy
         *mutexFlag_ = false;
     }
 
-    // ************************* Timer **************************
+    // ***************************************** Timer ***************************************** //
 
     Timer::Timer() noexcept
     {
@@ -45,39 +45,34 @@ namespace jiminy
 
     void Timer::tic() noexcept
     {
-        t0 = Time::now();
-        dt = 0.0;
+        t0_ = clock::now();
     }
 
-    void Timer::toc() noexcept
-    {
-        tf = Time::now();
-        std::chrono::duration<double> timeDiff = tf - t0;
-        dt = timeDiff.count();
-    }
-
-    // ************ IO file and Directory utilities **************
+    // **************************** IO file and Directory utilities **************************** //
 
-#ifndef _WIN32
     std::string getUserDirectory()
     {
+#ifndef _WIN32
         struct passwd * pw = getpwuid(getuid());
-        return pw->pw_dir;
-    }
+        if (pw)
+        {
+            return {pw->pw_dir};
+        }
+        return {};
 #else
-    std::string getUserDirectory()
-    {
         return {getenv("USERPROFILE")};
-    }
 #endif
+    }
 
-    // ******************* Telemetry utilities **********************
+    // ********************************** Telemetry utilities ********************************** //
 
-    bool endsWith(const std::string & text, const std::string & ending)
+    bool endsWith(const std::string & str, const std::string & substr)
     {
-        if (text.length() >= ending.length())
+        const size_t strSize = str.length();
+        const size_t substrSize = substr.length();
+        if (strSize >= substrSize)
         {
-            return text.compare(text.length() - ending.length(), ending.length(), ending) == 0;
+            return str.compare(strSize - substrSize, substrSize, substr) == 0;
         }
         return false;
     }
diff --git a/core/src/utilities/json.cc b/core/src/utilities/json.cc
index 7cb264bb0..b95b98b3f 100644
--- a/core/src/utilities/json.cc
+++ b/core/src/utilities/json.cc
@@ -118,21 +118,6 @@ namespace jiminy
         return value.asDouble();
     }
 
-    template<>
-    Eigen::VectorXd convertFromJson<Eigen::VectorXd>(const Json::Value & value)
-    {
-        Eigen::VectorXd vec{};
-        if (value.size() > 0)
-        {
-            vec.resize(value.size());
-            for (auto it = value.begin(); it != value.end(); ++it)
-            {
-                vec[it.index()] = convertFromJson<double>(*it);
-            }
-        }
-        return vec;
-    }
-
     template<>
     Eigen::MatrixXd convertFromJson<Eigen::MatrixXd>(const Json::Value & value)
     {
@@ -164,10 +149,12 @@ namespace jiminy
     template<>
     HeightmapFunctor convertFromJson<HeightmapFunctor>(const Json::Value & /* value */)
     {
-        return {HeightmapFunctor(
-            [](const Eigen::Vector3d & /* pos */) -> std::pair<double, Eigen::Vector3d> {
-                return {0.0, Eigen::Vector3d::UnitZ()};
-            })};
+        return {
+            [](const Eigen::Vector2d & /* xy */, double & height, Eigen::Vector3d & normal) -> void
+            {
+                height = 0.0;
+                normal = Eigen::Vector3d::UnitZ();
+            }};
     }
 
     template<>
@@ -249,6 +236,10 @@ namespace jiminy
                     {
                         field = convertFromJson<Eigen::VectorXd>(*root);
                     }
+                    else if (it->isConvertibleTo(Json::uintValue))
+                    {
+                        field = convertFromJson<VectorX<uint32_t>>(*root);
+                    }
                     else if (it->type() == Json::arrayValue)
                     {
                         field = convertFromJson<Eigen::MatrixXd>(*root);
diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc
index 8b337d24b..8794f06b3 100644
--- a/core/src/utilities/random.cc
+++ b/core/src/utilities/random.cc
@@ -8,78 +8,131 @@ namespace jiminy
     static inline constexpr double PERLIN_NOISE_PERSISTENCE{1.50};
     static inline constexpr double PERLIN_NOISE_LACUNARITY{1.15};
 
-    // ***************** Random number generator_ *****************
+    // ***************************** Uniform random bit generators ***************************** //
 
-    // Based on Ziggurat generator by Marsaglia and Tsang (JSS, 2000):
-    // https://people.sc.fsu.edu/~jburkardt/cpp_src/ziggurat/ziggurat.html
+    PCG32::PCG32(uint64_t state) noexcept :
+    state_{state | 3ULL}
+    {
+    }
 
-    std::mt19937 generator_{};
-    std::uniform_real_distribution<float> distUniform_(0.0, 1.0);
-    bool isInitialized_{false};
-    uint32_t seed_{0U};
+    uint32_t PCG32::operator()() noexcept
+    {
+        constexpr uint8_t bits = static_cast<uint8_t>(std::numeric_limits<uint64_t>::digits);
+        constexpr uint8_t uint32Bits = static_cast<uint8_t>(std::numeric_limits<uint32_t>::digits);
+        constexpr uint8_t spareBits = bits - uint32Bits;
+        constexpr uint8_t opBits = spareBits - 5 >= 64 ? 5 :
+                                   spareBits - 4 >= 32 ? 4 :
+                                   spareBits - 3 >= 16 ? 3 :
+                                   spareBits - 2 >= 4  ? 2 :
+                                   spareBits - 1 >= 1  ? 1 :
+                                                         0;
+        constexpr uint8_t mask = (1 << opBits) - 1;
+        constexpr uint8_t maxRandShift = mask;
+        constexpr uint8_t topSpare = opBits;
+        constexpr uint8_t bottomSpare = spareBits - topSpare;
+        constexpr uint8_t xShift = topSpare + (uint32Bits + maxRandShift) / 2;
+
+        state_ *= 6364136223846793005ULL;
+        uint64_t state = state_;
+        uint8_t rshift = opBits ? static_cast<uint8_t>(state >> (bits - opBits)) & mask : 0U;
+        state ^= state >> xShift;
+        return static_cast<uint32_t>(state >> (bottomSpare - maxRandShift + rshift));
+    }
 
-    uint32_t kn[128];
-    float fn[128];
-    float wn[128];
+    // ****************************** Random number distributions ****************************** //
 
-    void r4_nor_setup() noexcept
+    float uniform(const uniform_random_bit_generator_ref<uint32_t> & g)
     {
-        const double m1 = 2147483648.0;
-        const double vn = 9.91256303526217e-03;
-        double dn = 3.442619855899;
-        double tn = dn;
+        return std::generate_canonical<float, std::numeric_limits<float>::digits>(g);
+    }
+
+    float uniform(const uniform_random_bit_generator_ref<uint32_t> & g, float lo, float hi)
+    {
+        return std::uniform_real_distribution<float>(lo, hi)(g);
+    }
 
-        double q = vn / exp(-0.5 * dn * dn);
+    namespace internal::ziggurat
+    {
+        /// \details "Advanced" cmath functions are required by the standard to be constexpr
+        ///          starting from C++26 instead of C++23 as initially expected (see P1383R2).
+        ///          Generic compile-time computation supported by all compilers is not possible
+        ///          without it, and must be postponed to import-time, which is still much better
+        ///          than run-time.
+        ///
+        ///  \sa For reference, see:
+        ///      https://stackoverflow.com/a/34465458/4820605
+        ///      https://stackoverflow.com/q/36497632/4820605
+        struct ZigguratNormalData
+        {
+            ZigguratNormalData()
+            {
+                constexpr double m1 = 2147483648.0;
+                constexpr double vn = 9.91256303526217e-03;
+                double dn = 3.442619855899;
+                double tn = dn;
 
-        kn[0] = static_cast<uint32_t>((dn / q) * m1);
-        kn[1] = 0;
+                const double q = vn / std::exp(-0.5 * dn * dn);
 
-        wn[0] = static_cast<float>(q / m1);
-        wn[127] = static_cast<float>(dn / m1);
+                kn[0] = static_cast<uint32_t>((dn / q) * m1);
+                kn[1] = 0;
 
-        fn[0] = 1.0f;
-        fn[127] = static_cast<float>(exp(-0.5 * dn * dn));
+                wn[0] = static_cast<float>(q / m1);
+                wn[127] = static_cast<float>(dn / m1);
 
-        for (uint8_t i = 126; 1 <= i; i--)
-        {
-            dn = sqrt(-2.0 * log(vn / dn + exp(-0.5 * dn * dn)));
-            kn[i + 1] = static_cast<uint32_t>((dn / tn) * m1);
-            tn = dn;
-            fn[i] = static_cast<float>(exp(-0.5 * dn * dn));
-            wn[i] = static_cast<float>(dn / m1);
-        }
-    }
+                fn[0] = 1.0F;
+                fn[127] = static_cast<float>(std::exp(-0.5 * dn * dn));
 
-    float r4_uni()
-    {
-        return distUniform_(generator_);
+                for (uint8_t i = 126; 1 <= i; i--)
+                {
+                    dn = std::sqrt(-2.0 * std::log(vn / dn + std::exp(-0.5 * dn * dn)));
+                    kn[i + 1] = static_cast<uint32_t>((dn / tn) * m1);
+                    tn = dn;
+                    fn[i] = static_cast<float>(std::exp(-0.5 * dn * dn));
+                    wn[i] = static_cast<float>(dn / m1);
+                }
+            }
+
+            std::array<uint32_t, 128> kn{};
+            std::array<float, 128> fn{};
+            std::array<float, 128> wn{};
+        };
+
+        static const ZigguratNormalData ZIGGURAT_NORMAL_DATA{};
+        static const std::array<uint32_t, 128> kn = ZIGGURAT_NORMAL_DATA.kn;
+        static const std::array<float, 128> fn = ZIGGURAT_NORMAL_DATA.fn;
+        static const std::array<float, 128> wn = ZIGGURAT_NORMAL_DATA.wn;
     }
 
-    float r4_nor()
+    namespace internal
     {
-        const float r = 3.442620f;
-        int32_t hz;
-        uint32_t iz;
-        float x;
-        float y;
-
-        hz = static_cast<int32_t>(generator_());
-        iz = (static_cast<uint32_t>(hz) & 127U);
 
-        if (fabs(hz) < kn[iz])
-        {
-            return static_cast<float>(hz) * wn[iz];
-        }
-        else
+        float normal(const uniform_random_bit_generator_ref<uint32_t> & g)
         {
+            using namespace internal::ziggurat;
+
+            constexpr float r = 3.442620F;
+
+            int32_t hz;
+            uint32_t iz;
+            float x;
+            float y;
+
+            hz = static_cast<int32_t>(g());
+            iz = (static_cast<uint32_t>(hz) & 127UL);
+
+            if (std::fabs(hz) < kn[iz])
+            {
+                return static_cast<float>(hz) * wn[iz];
+            }
+
             while (true)
             {
                 if (iz == 0)
                 {
                     while (true)
                     {
-                        x = -0.2904764f * log(r4_uni());
-                        y = -log(r4_uni());
+                        x = -0.2904764F * std::log(uniform(g));
+                        y = -std::log(uniform(g));
                         if (x * x <= y + y)
                         {
                             break;
@@ -90,23 +143,20 @@ namespace jiminy
                     {
                         return -r - x;
                     }
-                    else
-                    {
-                        return +r + x;
-                    }
+                    return r + x;
                 }
 
                 x = static_cast<float>(hz) * wn[iz];
 
-                if (fn[iz] + r4_uni() * (fn[iz - 1] - fn[iz]) < exp(-0.5f * x * x))
+                if (fn[iz] + uniform(g) * (fn[iz - 1] - fn[iz]) < std::exp(-0.5F * x * x))
                 {
                     return x;
                 }
 
-                hz = static_cast<int32_t>(generator_());
+                hz = static_cast<int32_t>(g());
                 iz = (hz & 127);
 
-                if (fabs(hz) < kn[iz])
+                if (std::fabs(hz) < kn[iz])
                 {
                     return static_cast<float>(hz) * wn[iz];
                 }
@@ -114,103 +164,42 @@ namespace jiminy
         }
     }
 
-    void resetRandomGenerators(const std::optional<uint32_t> & seed) noexcept
+    float normal(const uniform_random_bit_generator_ref<uint32_t> & g, float mean, float stddev)
     {
-        uint32_t newSeed = seed.value_or(seed_);
-        srand(newSeed);  // Eigen relies on srand for generating random numbers
-        generator_.seed(newSeed);
-        r4_nor_setup();
-        seed_ = newSeed;
-        isInitialized_ = true;
+        return internal::normal(g) * stddev + mean;
     }
 
-    hresult_t getRandomSeed(uint32_t & seed)
-    {
-        if (!isInitialized_)
-        {
-            PRINT_ERROR("Random number generator not initialized.");
-            return hresult_t::ERROR_GENERIC;
-        }
-
-        seed = seed_;
-
-        return hresult_t::SUCCESS;
-    }
-
-    double randUniform(double lo, double hi)
-    {
-        assert(isInitialized_ && "Random number genetors not initialized. "
-                                 "Please call `resetRandomGenerators` at least once.");
-        return lo + r4_uni() * (hi - lo);
-    }
-
-    double randNormal(double mean, double std)
-    {
-        assert(isInitialized_ && "Random number genetors not initialized. "
-                                 "Please call `resetRandomGenerators` at least once.");
-        return mean + r4_nor() * std;
-    }
-
-    Eigen::VectorXd randVectorNormal(uint32_t size, double mean, double std)
-    {
-        if (std > 0.0)
-        {
-            return Eigen::VectorXd::NullaryExpr(
-                size, [mean, std](Eigen::Index) -> double { return randNormal(mean, std); });
-        }
-        return Eigen::VectorXd::Constant(size, mean);
-    }
+    // **************************** Non-cryptographic hash function **************************** //
 
-    Eigen::VectorXd randVectorNormal(uint32_t size, double std)
-    {
-        return randVectorNormal(size, 0.0, std);
-    }
-
-    Eigen::VectorXd randVectorNormal(const Eigen::VectorXd & mean, const Eigen::VectorXd & std)
-    {
-        return Eigen::VectorXd::NullaryExpr(std.size(),
-                                            [&mean, &std](Eigen::Index i) -> double
-                                            { return randNormal(mean[i], std[i]); });
-    }
-
-    Eigen::VectorXd randVectorNormal(const Eigen::VectorXd & std)
-    {
-        return Eigen::VectorXd::NullaryExpr(
-            std.size(), [&std](Eigen::Index i) -> double { return randNormal(0, std[i]); });
-    }
-
-    void shuffleIndices(std::vector<uint32_t> & vector)
-    {
-        std::shuffle(vector.begin(), vector.end(), generator_);
-    }
-
-    //-----------------------------------------------------------------------------
-    // MurmurHash3 was written by Austin Appleby, and is placed in the public
-    // domain. The author hereby disclaims copyright to this source code:
-    // https://github.com/aappleby/smhasher/blob/master/src/MurmurHash3.cpp
-
-    inline uint32_t rotl32(uint32_t x, int8_t r) noexcept
+    static uint32_t rotl32(uint32_t x, int8_t r) noexcept
     {
         return (x << r) | (x >> (32 - r));
     }
 
-    uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept
+    /// @brief MurmurHash3 is a non-cryptographic hash function initially designed
+    ///        for hash-based lookup.
+    ///
+    /// @sa It was written by Austin Appleby, and is placed in the public domain.
+    ///     The author hereby disclaims copyright to this source code:
+    ///     https://github.com/aappleby/smhasher/blob/master/src/MurmurHash3.cpp
+    static uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept
     {
         // Define some internal constants
-        const uint32_t c1 = 0xcc9e2d51;
-        const uint32_t c2 = 0x1b873593;
-        const uint32_t c3 = 0xe6546b64;
+        constexpr uint32_t c1 = 0xcc9e2d51;
+        constexpr uint32_t c2 = 0x1b873593;
+        constexpr uint32_t c3 = 0xe6546b64;
 
         // Initialize has to seed value
         uint32_t h1 = seed;
 
         // Extract bytes from key
-        const uint8_t * data = reinterpret_cast<const uint8_t *>(key);
-        const int32_t nblocks = len / 4;  // len in bytes, so 32-bits blocks
+        const auto * data = static_cast<const uint8_t *>(key);
+        const int32_t num_blocks = len / 4;  // len in bytes, so 32-bits blocks
 
         // Body
-        const uint32_t * blocks = reinterpret_cast<const uint32_t *>(data + nblocks * 4);
-        for (int32_t i = -nblocks; i; ++i)
+        const auto * blocks =
+            reinterpret_cast<const uint32_t *>(data + static_cast<std::ptrdiff_t>(num_blocks * 4));
+        for (int32_t i = -num_blocks; i; ++i)
         {
             uint32_t k1 = blocks[i];
             k1 *= c1;
@@ -222,7 +211,8 @@ namespace jiminy
         }
 
         // Tail
-        const uint8_t * tail = reinterpret_cast<const uint8_t *>(data + nblocks * 4);
+        const auto * tail =
+            reinterpret_cast<const uint8_t *>(data + static_cast<std::ptrdiff_t>(num_blocks * 4));
         uint32_t k1 = 0U;
         switch (len & 3)
         {
@@ -255,27 +245,27 @@ namespace jiminy
         return h1;
     }
 
-    //-----------------------------------------------------------------------------
+    // **************************** Continuous 1D Gaussian processes *************************** //
 
-    PeriodicGaussianProcess::PeriodicGaussianProcess(
-        double wavelength, double period, double scale) noexcept :
+    PeriodicGaussianProcess::PeriodicGaussianProcess(double wavelength, double period) noexcept :
     wavelength_{wavelength},
-    period_{period},
-    scale_{scale}
+    period_{period}
     {
+        reset(std::random_device{});
     }
 
-    void PeriodicGaussianProcess::reset()
+    void PeriodicGaussianProcess::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
     {
         // Sample normal vector
-        const Eigen::VectorXd normalVec =
-            Eigen::VectorXd::NullaryExpr(numTimes_, [](Eigen::Index) { return randNormal(); });
+        auto normalVec = normal(num_times_, 1, g);
 
         // Compute discrete periodic gaussian process values
-        values_.noalias() = covSqrtRoot_.triangularView<Eigen::Lower>() * normalVec;
+        values_.noalias() =
+            cov_sqrt_root_.triangularView<Eigen::Lower>() * normalVec.cast<double>();
     }
 
-    double PeriodicGaussianProcess::operator()(const float & t)
+    double PeriodicGaussianProcess::operator()(float t)
     {
         // Wrap requested time in gaussian process period
         double tWrap = std::fmod(t, period_);
@@ -285,51 +275,47 @@ namespace jiminy
         }
 
         // Compute closest left and right indices
-        const int32_t tLeftIdx = static_cast<int32_t>(std::floor(tWrap / dt_));
-        const int32_t tRightIdx = (tLeftIdx + 1) % numTimes_;
+        const Eigen::Index tLeftIdx = static_cast<Eigen::Index>(std::floor(tWrap / dt_));
+        const Eigen::Index tRightIdx = (tLeftIdx + 1) % num_times_;
 
         // Perform First order interpolation
-        const double ratio = tWrap / dt_ - tLeftIdx;
-        return scale_ * (values_[tLeftIdx] + ratio * (values_[tRightIdx] - values_[tLeftIdx]));
+        const double ratio = tWrap / dt_ - static_cast<double>(tLeftIdx);
+        return values_[tLeftIdx] + ratio * (values_[tRightIdx] - values_[tLeftIdx]);
     }
 
-    double PeriodicGaussianProcess::getWavelength() const
+    double PeriodicGaussianProcess::wavelength() const noexcept
     {
         return wavelength_;
     }
 
-    double PeriodicGaussianProcess::getPeriod() const
+    double PeriodicGaussianProcess::period() const noexcept
     {
         return period_;
     }
 
-    double PeriodicGaussianProcess::getDt() const
-    {
-        return dt_;
-    }
+    // **************************** Continuous 1D Fourier processes **************************** //
 
-    PeriodicFourierProcess::PeriodicFourierProcess(
-        double wavelength, double period, double scale) noexcept :
+    PeriodicFourierProcess::PeriodicFourierProcess(double wavelength, double period) noexcept :
     wavelength_{wavelength},
-    period_{period},
-    scale_{scale}
+    period_{period}
     {
+        reset(std::random_device{});
     }
 
-    void PeriodicFourierProcess::reset()
+    void PeriodicFourierProcess::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
     {
         // Sample normal vectors
-        Eigen::VectorXd normalVec1 =
-            Eigen::VectorXd::NullaryExpr(numHarmonics_, [](Eigen::Index) { return randNormal(); });
-        Eigen::VectorXd normalVec2 =
-            Eigen::VectorXd::NullaryExpr(numHarmonics_, [](Eigen::Index) { return randNormal(); });
+        auto normalVec1 = normal(num_harmonics_, 1, g);
+        auto normalVec2 = normal(num_harmonics_, 1, g);
 
         // Compute discrete periodic gaussian process values
-        values_ = M_SQRT2 / std::sqrt(2 * numHarmonics_ + 1) *
-                  (cosMat_ * normalVec1 + sinMat_ * normalVec2);
+        const double scale = M_SQRT2 / std::sqrt(2 * num_harmonics_ + 1);
+        values_ = scale * cos_mat_ * normalVec1.cast<double>();
+        values_.noalias() += scale * sin_mat_ * normalVec2.cast<double>();
     }
 
-    double PeriodicFourierProcess::operator()(const float & t)
+    double PeriodicFourierProcess::operator()(float t)
     {
         // Wrap requested time in guassian process period
         double tWrap = std::fmod(t, period_);
@@ -339,45 +325,38 @@ namespace jiminy
         }
 
         // Compute closest left and right indices
-        const int32_t tLeftIdx = static_cast<int32_t>(std::floor(tWrap / dt_));
-        const int32_t tRightIdx = (tLeftIdx + 1) % numTimes_;
+        const Eigen::Index tLeftIdx = static_cast<Eigen::Index>(std::floor(tWrap / dt_));
+        const Eigen::Index tRightIdx = (tLeftIdx + 1) % num_times_;
 
         // Perform First order interpolation
-        const double ratio = tWrap / dt_ - tLeftIdx;
-        return scale_ * (values_[tLeftIdx] + ratio * (values_[tRightIdx] - values_[tLeftIdx]));
+        const double ratio = tWrap / dt_ - static_cast<double>(tLeftIdx);
+        return values_[tLeftIdx] + ratio * (values_[tRightIdx] - values_[tLeftIdx]);
     }
 
-    double PeriodicFourierProcess::getWavelength() const
+    double PeriodicFourierProcess::wavelength() const noexcept
     {
         return wavelength_;
     }
 
-    double PeriodicFourierProcess::getPeriod() const
+    double PeriodicFourierProcess::period() const noexcept
     {
         return period_;
     }
 
-    int32_t PeriodicFourierProcess::getNumHarmonics() const
-    {
-        return numHarmonics_;
-    }
+    // ***************************** Continuous 1D Perlin processes **************************** //
 
-    double PeriodicFourierProcess::getDt() const
+    AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) :
+    wavelength_{wavelength}
     {
-        return dt_;
+        assert(wavelength_ > 0 && "wavelength must be strictly larger than 0.0.");
+        shift_ = uniform(std::random_device{});
     }
 
-    AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength, double scale) noexcept
-    :
-    wavelength_{wavelength},
-    scale_{scale}
-    {
-    }
-
-    void AbstractPerlinNoiseOctave::reset()
+    void AbstractPerlinNoiseOctave::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
     {
         // Sample random phase shift
-        shift_ = randUniform();
+        shift_ = uniform(g);
     }
 
     double AbstractPerlinNoiseOctave::operator()(double t) const
@@ -398,20 +377,15 @@ namespace jiminy
            at current phase.*/
         const double yLeft = grad(phaseIdxLeft, dtLeft);
         const double yRight = grad(phaseIdxRight, dtRight);
-        return scale_ * lerp(ratio, yLeft, yRight);
+        return lerp(ratio, yLeft, yRight);
     }
 
-    double AbstractPerlinNoiseOctave::getWavelength() const
+    double AbstractPerlinNoiseOctave::wavelength() const noexcept
     {
         return wavelength_;
     }
 
-    double AbstractPerlinNoiseOctave::getScale() const
-    {
-        return scale_;
-    }
-
-    double AbstractPerlinNoiseOctave::fade(double delta) const
+    double AbstractPerlinNoiseOctave::fade(double delta) noexcept
     {
         /* Improved Smoothstep function by Ken Perlin (aka Smootherstep).
            It has zero 1st and 2nd-order derivatives at dt = 0.0, and 1.0:
@@ -419,21 +393,28 @@ namespace jiminy
         return std::pow(delta, 3) * (delta * (delta * 6.0 - 15.0) + 10.0);
     }
 
-    double AbstractPerlinNoiseOctave::lerp(double ratio, double yLeft, double yRight) const
+    double AbstractPerlinNoiseOctave::lerp(double ratio, double yLeft, double yRight) noexcept
     {
         return yLeft + ratio * (yRight - yLeft);
     }
 
-    void RandomPerlinNoiseOctave::reset()
+    RandomPerlinNoiseOctave::RandomPerlinNoiseOctave(double wavelength) :
+    AbstractPerlinNoiseOctave(wavelength)
+    {
+        seed_ = std::random_device{}();
+    }
+
+    void RandomPerlinNoiseOctave::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
     {
         // Call base implementation
-        AbstractPerlinNoiseOctave::reset();
+        AbstractPerlinNoiseOctave::reset(g);
 
         // Sample new random seed for MurmurHash
-        seed_ = static_cast<uint32_t>(generator_());
+        seed_ = g();
     }
 
-    double RandomPerlinNoiseOctave::grad(int32_t knot, double delta) const
+    double RandomPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept
     {
         // Get hash of knot
         const uint32_t hash = MurmurHash3(&knot, sizeof(int32_t), seed_);
@@ -449,28 +430,42 @@ namespace jiminy
         return 2.0 * grad * delta;
     }
 
-    PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(
-        double wavelength, double period, double scale) :
-    AbstractPerlinNoiseOctave(wavelength, scale),
+    namespace internal
+    {
+        template<typename Generator, typename T>
+        void randomizePermutationVector(Generator && g, T & vec)
+        {
+            // Re-Initialize the permutation vector with values from 0 to size
+            std::iota(vec.begin(), vec.end(), 0);
+
+            // Shuffle the permutation vector
+            std::shuffle(vec.begin(), vec.end(), g);
+        }
+    }
+
+    PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(double wavelength, double period) :
+    AbstractPerlinNoiseOctave(wavelength),
     period_{period}
     {
         // Make sure the wavelength is multiple of the period
-        assert(std::abs(period_ - period) < 1e-6);
+        assert(std::abs(std::round(period / wavelength) * wavelength - period) < 1e-6 &&
+               "wavelength must be multiple of period.");
+
+        // Initialize the permutation vector with values from 0 to 255 and shuffle it
+        internal::randomizePermutationVector(std::random_device{}, perm_);
     }
 
-    void PeriodicPerlinNoiseOctave::reset()
+    void PeriodicPerlinNoiseOctave::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
     {
         // Call base implementation
-        AbstractPerlinNoiseOctave::reset();
-
-        // Initialize the permutation vector with values from 0 to 255
-        std::iota(perm_.begin(), perm_.end(), 0);
+        AbstractPerlinNoiseOctave::reset(g);
 
-        // Shuffle the permutation vector
-        std::shuffle(perm_.begin(), perm_.end(), generator_);
+        // Re-Initialize the permutation vector with values from 0 to 255
+        internal::randomizePermutationVector(g, perm_);
     }
 
-    double PeriodicPerlinNoiseOctave::grad(int32_t knot, double delta) const
+    double PeriodicPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept
     {
         // Wrap knot is period interval
         knot %= static_cast<uint32_t>(period_ / wavelength_);
@@ -486,161 +481,158 @@ namespace jiminy
     }
 
     AbstractPerlinProcess::AbstractPerlinProcess(
-        double scale, std::vector<std::unique_ptr<AbstractPerlinNoiseOctave>> && octaves) noexcept
-    :
-    scale_{scale},
-    octaves_(std::move(octaves))
-    {
-    }
-
-    void AbstractPerlinProcess::reset()
+        std::vector<OctaveScalePair> && octave_scale_pairs) noexcept :
+    octave_scale_pairs_(std::move(octave_scale_pairs))
     {
-        // Reset every octave successively
-        for (auto & octave : octaves_)
+        // Compute the scaling factor to keep values within range [-1.0, 1.0]
+        double amplitude_squared = 0.0;
+        for (const OctaveScalePair & octave_scale : octave_scale_pairs_)
         {
-            octave->reset();
+            const double scale = std::get<1>(octave_scale);
+            amplitude_squared += std::pow(scale, 2);
         }
+        amplitude_ = std::sqrt(amplitude_squared);
+    }
 
-        // Compute scaling factor to get values in range [-1.0, 1.0]
-        double amplitudeSquared = 0.0;
-        for (const auto & octave : octaves_)
+    void AbstractPerlinProcess::reset(
+        const uniform_random_bit_generator_ref<uint32_t> & g) noexcept
+    {
+        // Reset octaves
+        for (OctaveScalePair & octave_scale : octave_scale_pairs_)
         {
-            amplitudeSquared += std::pow(octave->getScale(), 2);
+            std::unique_ptr<AbstractPerlinNoiseOctave> & octave = std::get<0>(octave_scale);
+            octave->reset(g);
         }
-        amplitude_ = scale_ * std::sqrt(amplitudeSquared);
     }
 
-    double AbstractPerlinProcess::operator()(const float & t)
+    double AbstractPerlinProcess::operator()(float t)
     {
         // Compute sum of octaves' values
         double value = 0.0;
-        for (const auto & octave : octaves_)
+        for (const auto & [octave, scale] : octave_scale_pairs_)
         {
-            value += (*octave)(t);
+            value += scale * (*octave)(t);
         }
 
-        // Return scaled value by total amplitude
+        // Scale sum by maximum amplitude
         return value / amplitude_;
     }
 
-    double AbstractPerlinProcess::getWavelength() const noexcept
+    double AbstractPerlinProcess::wavelength() const noexcept
     {
-        return std::transform_reduce(
-            octaves_.cbegin(),
-            octaves_.cend(),
-            INF,
-            std::less<double>(),
-            [](const std::unique_ptr<AbstractPerlinNoiseOctave> & octave) -> double
-            { return octave->getWavelength(); });
-    }
-
-    std::size_t AbstractPerlinProcess::getNumOctaves() const noexcept
-    {
-        return octaves_.size();
+        double wavelength = INF;
+        for (const OctaveScalePair & octave_scale : octave_scale_pairs_)
+        {
+            const std::unique_ptr<AbstractPerlinNoiseOctave> & octave = std::get<0>(octave_scale);
+            wavelength = std::min(wavelength, octave->wavelength());
+        }
+        return wavelength;
     }
 
-    double AbstractPerlinProcess::getScale() const noexcept
+    std::size_t AbstractPerlinProcess::num_octaves() const noexcept
     {
-        return scale_;
+        return octave_scale_pairs_.size();
     }
 
-    std::vector<std::unique_ptr<AbstractPerlinNoiseOctave>> buildPerlinNoiseOctaves(
+    std::vector<AbstractPerlinProcess::OctaveScalePair> buildPerlinNoiseOctaves(
         double wavelength,
-        std::size_t numOctaves,
-        std::function<std::unique_ptr<AbstractPerlinNoiseOctave>(double, double)> factory)
+        std::size_t num_octaves,
+        std::function<std::unique_ptr<AbstractPerlinNoiseOctave>(double)> factory)
     {
-        std::vector<std::unique_ptr<AbstractPerlinNoiseOctave>> octaves_;
-        octaves_.reserve(numOctaves);
+        std::vector<AbstractPerlinProcess::OctaveScalePair> octave_scale_pairs;
+        octave_scale_pairs.reserve(num_octaves);
         double scale = 1.0;
-        for (std::size_t i = 0; i < numOctaves; ++i)
+        for (std::size_t i = 0; i < num_octaves; ++i)
         {
-            octaves_.push_back(factory(wavelength, scale));
+            octave_scale_pairs.emplace_back(factory(wavelength), scale);
             wavelength *= PERLIN_NOISE_LACUNARITY;
             scale *= PERLIN_NOISE_PERSISTENCE;
         }
-        return octaves_;
+        return octave_scale_pairs;
     }
 
-    RandomPerlinProcess::RandomPerlinProcess(
-        double wavelength, double scale, std::size_t numOctaves) :
-    AbstractPerlinProcess(
-        scale,
-        buildPerlinNoiseOctaves(
-            wavelength,
-            numOctaves,
-            [](double wavelengthIn, double scaleIn) -> std::unique_ptr<AbstractPerlinNoiseOctave>
-            { return std::make_unique<RandomPerlinNoiseOctave>(wavelengthIn, scaleIn); }))
+    RandomPerlinProcess::RandomPerlinProcess(double wavelength, std::size_t num_octaves) :
+    AbstractPerlinProcess(buildPerlinNoiseOctaves(
+        wavelength,
+        num_octaves,
+        [](double wavelengthIn) -> std::unique_ptr<AbstractPerlinNoiseOctave>
+        { return std::make_unique<RandomPerlinNoiseOctave>(wavelengthIn); }))
     {
     }
 
     PeriodicPerlinProcess::PeriodicPerlinProcess(
-        double wavelength, double period, double scale, std::size_t numOctaves) :
-    AbstractPerlinProcess(
-        scale,
-        buildPerlinNoiseOctaves(
-            wavelength,
-            numOctaves,
-            [period](double wavelengthIn,
-                     double scaleIn) -> std::unique_ptr<AbstractPerlinNoiseOctave> {
-                return std::make_unique<PeriodicPerlinNoiseOctave>(wavelengthIn, period, scaleIn);
-            })),
+        double wavelength, double period, std::size_t num_octaves) :
+    AbstractPerlinProcess(buildPerlinNoiseOctaves(
+        wavelength,
+        num_octaves,
+        [period](double wavelengthIn) -> std::unique_ptr<AbstractPerlinNoiseOctave>
+        { return std::make_unique<PeriodicPerlinNoiseOctave>(wavelengthIn, period); })),
     period_{period}
     {
         // Make sure the period is larger than the wavelength
         assert(period_ >= wavelength && "Period must be larger than wavelength.");
     }
 
-    double PeriodicPerlinProcess::getPeriod() const noexcept
+    double PeriodicPerlinProcess::period() const noexcept
     {
         return period_;
     }
 
-    template<typename VectorLike>
-    std::enable_if_t<is_eigen_vector_v<VectorLike>, double> randomDouble(
-        const Eigen::MatrixBase<VectorLike> & key, int64_t sparsity, double scale, uint32_t seed)
+    // ******************************* Random terrain generators ******************************* //
+
+    template<typename Scalar>
+    static float uniformSparseFromStateImpl(
+        const MatrixX<Scalar> & state, int64_t sparsity, uint32_t seed) noexcept
     {
-        const int32_t keyLen = static_cast<int32_t>(sizeof(typename VectorLike::Scalar)) *
-                               static_cast<int32_t>(key.size());
-        const uint32_t hash = MurmurHash3(key.derived().data(), keyLen, seed);
+        const auto num_bytes = static_cast<int32_t>(sizeof(Scalar) * state.size());
+        const uint32_t hash = MurmurHash3(state.data(), num_bytes, seed);
         if (hash % sparsity == 0)
         {
-            double encoding(hash);
-            encoding /= std::numeric_limits<uint32_t>::max();
-            return scale * encoding;
+            return static_cast<float>(hash) /
+                   static_cast<float>(std::numeric_limits<uint32_t>::max());
         }
-        return 0.0;
+        return 0.0F;
     }
 
-    std::pair<double, double> tile2dInterp1d(Eigen::Vector2i & posIdx,
-                                             const Eigen::Vector2d & posRel,
+    template<typename Derived>
+    static float uniformSparseFromState(
+        const Eigen::MatrixBase<Derived> & state, int64_t sparsity, uint32_t seed) noexcept
+    {
+        return uniformSparseFromStateImpl<typename Derived::Scalar>(
+            state.derived(), sparsity, seed);
+    }
+
+    std::pair<double, double> tile2dInterp1d(Eigen::Vector2i & pos_indices,
+                                             const Eigen::Vector2d & pos_rel,
                                              uint32_t dim,
                                              const Eigen::Vector2d & size,
                                              int64_t sparsity,
-                                             double heightMax,
-                                             const Eigen::Vector2d & interpThreshold,
+                                             double height_max,
+                                             const Eigen::Vector2d & interp_thr,
                                              uint32_t seed)
     {
-        const double z = randomDouble(posIdx, sparsity, heightMax, seed);
         double height, dheight;
-        if (posRel[dim] < interpThreshold[dim])
+
+        const double z = height_max * uniformSparseFromState(pos_indices, sparsity, seed);
+        if (pos_rel[dim] < interp_thr[dim])
         {
-            posIdx[dim] -= 1;
-            const double z_m = randomDouble(posIdx, sparsity, heightMax, seed);
-            posIdx[dim] += 1;
+            pos_indices[dim] -= 1;
+            const double z_m = height_max * uniformSparseFromState(pos_indices, sparsity, seed);
+            pos_indices[dim] += 1;
 
-            const double ratio = (1.0 - posRel[dim] / interpThreshold[dim]) / 2.0;
+            const double ratio = (1.0 - pos_rel[dim] / interp_thr[dim]) / 2.0;
             height = z + (z_m - z) * ratio;
-            dheight = (z - z_m) / (2.0 * size[dim] * interpThreshold[dim]);
+            dheight = (z - z_m) / (2.0 * size[dim] * interp_thr[dim]);
         }
-        else if (1.0 - posRel[dim] < interpThreshold[dim])
+        else if (1.0 - pos_rel[dim] < interp_thr[dim])
         {
-            posIdx[dim] += 1;
-            const double z_p = randomDouble(posIdx, sparsity, heightMax, seed);
-            posIdx[dim] -= 1;
+            pos_indices[dim] += 1;
+            const double z_p = height_max * uniformSparseFromState(pos_indices, sparsity, seed);
+            pos_indices[dim] -= 1;
 
-            const double ratio = (1.0 + (posRel[dim] - 1.0) / interpThreshold[dim]) / 2.0;
+            const double ratio = (1.0 + (pos_rel[dim] - 1.0) / interp_thr[dim]) / 2.0;
             height = z + (z_p - z) * ratio;
-            dheight = (z_p - z) / (2.0 * size[dim] * interpThreshold[dim]);
+            dheight = (z_p - z) / (2.0 * size[dim] * interp_thr[dim]);
         }
         else
         {
@@ -651,184 +643,95 @@ namespace jiminy
         return {height, dheight};
     }
 
-    HeightmapFunctor randomTileGround(const Eigen::Vector2d & size,
-                                      double heightMax,
-                                      const Eigen::Vector2d & interpDelta,
-                                      uint32_t sparsity,
-                                      double orientation,
-                                      uint32_t seed)
+    HeightmapFunctor tiles(const Eigen::Vector2d & size,
+                           double height_max,
+                           const Eigen::Vector2d & interp_delta,
+                           uint32_t sparsity,
+                           double orientation,
+                           uint32_t seed)
     {
-        if ((0.01 <= interpDelta.array()).all() &&
-            (interpDelta.array() <= size.array() / 2.0).all())
+        if ((0.01 <= interp_delta.array()).all() &&
+            (interp_delta.array() <= size.array() / 2.0).all())
         {
-            PRINT_WARNING("'interpDelta' must be in range [0.01, 'size'/2.0].");
+            PRINT_WARNING("'interp_delta' must be in range [0.01, 'size'/2.0].");
         }
 
-        Eigen::Vector2d interpThreshold = interpDelta.cwiseMax(0.01).cwiseMin(size / 2.0);
-        interpThreshold.array() /= size.array();
+        Eigen::Vector2d interp_thr = interp_delta.cwiseMax(0.01).cwiseMin(size / 2.0);
+        interp_thr.array() /= size.array();
 
         const Eigen::Vector2d offset = Eigen::Vector2d::NullaryExpr(
-            [&size, seed](Eigen::Index i) -> double
-            {
-                Eigen::Matrix<Eigen::Index, 1, 1> key;
-                key[0] = i;
-                return randomDouble(key, 1, size[i], seed);
+            [&size, seed](Eigen::Index i) -> double {
+                return size[i] *
+                       uniformSparseFromState(Vector1<Eigen::Index>::Constant(i), 1, seed);
             });
 
-        Eigen::Rotation2D<double> rotationMat(orientation);
+        const Eigen::Rotation2D<double> rot_mat(orientation);
 
-        return
-            [size, heightMax, interpDelta, rotationMat, sparsity, interpThreshold, offset, seed](
-                const Eigen::Vector3d & pos3) -> std::pair<double, Eigen::Vector3d>
+        return [size, height_max, interp_delta, rot_mat, sparsity, interp_thr, offset, seed](
+                   const Eigen::Vector2d & pos, double & height, Eigen::Vector3d & normal) -> void
         {
             // Compute the tile index and relative coordinate
-            Eigen::Vector2d pos = rotationMat * (pos3.head<2>() + offset);
-            Eigen::Vector2d posRel = pos.array() / size.array();
-            Eigen::Vector2i posIdx = posRel.array().floor().cast<int32_t>();
-            posRel -= posIdx.cast<double>();
+            Eigen::Vector2d pos_rel = (rot_mat * (pos + offset)).array() / size.array();
+            Vector2<int32_t> pos_indices = pos_rel.array().floor().cast<int32_t>();
+            pos_rel -= pos_indices.cast<double>();
 
             // Interpolate height based on nearby tiles if necessary
-            double height;
-            Eigen::Vector3d normal;
-            Eigen::Matrix<bool, 2, 1> isEdge = (posRel.array() < interpThreshold.array()) ||
-                                               (1.0 - posRel.array() < interpThreshold.array());
-            if (isEdge[0] && !isEdge[1])
+            Vector2<bool> is_edge = (pos_rel.array() < interp_thr.array()) ||
+                                    (1.0 - pos_rel.array() < interp_thr.array());
+            if (is_edge[0] && !is_edge[1])
             {
                 double dheight_x;
                 std::tie(height, dheight_x) = tile2dInterp1d(
-                    posIdx, posRel, 0, size, sparsity, heightMax, interpThreshold, seed);
-                const double normInv = std::sqrt(dheight_x * dheight_x + 1.0);
-                normal << -dheight_x * normInv, 0.0, normInv;
+                    pos_indices, pos_rel, 0, size, sparsity, height_max, interp_thr, seed);
+                double const norm_inv = 1.0 / std::sqrt(dheight_x * dheight_x + 1.0);
+                normal << -dheight_x * norm_inv, 0.0, norm_inv;
             }
-            else if (!isEdge[0] && isEdge[1])
+            else if (!is_edge[0] && is_edge[1])
             {
                 double dheight_y;
                 std::tie(height, dheight_y) = tile2dInterp1d(
-                    posIdx, posRel, 1, size, sparsity, heightMax, interpThreshold, seed);
-                const double normInv = std::sqrt(dheight_y * dheight_y + 1.0);
-                normal << 0.0, -dheight_y * normInv, normInv;
+                    pos_indices, pos_rel, 1, size, sparsity, height_max, interp_thr, seed);
+                double const norm_inv = 1.0 / std::sqrt(dheight_y * dheight_y + 1.0);
+                normal << 0.0, -dheight_y * norm_inv, norm_inv;
             }
-            else if (isEdge[0] && isEdge[1])
+            else if (is_edge[0] && is_edge[1])
             {
-                const auto [height_0, dheight_x_0] = tile2dInterp1d(
-                    posIdx, posRel, 0, size, sparsity, heightMax, interpThreshold, seed);
-                if (posRel[1] < interpThreshold[1])
+                auto const [height_0, dheight_x_0] = tile2dInterp1d(
+                    pos_indices, pos_rel, 0, size, sparsity, height_max, interp_thr, seed);
+                if (pos_rel[1] < interp_thr[1])
                 {
-                    posIdx[1] -= 1;
-                    const auto [height_m, dheight_x_m] = tile2dInterp1d(
-                        posIdx, posRel, 0, size, sparsity, heightMax, interpThreshold, seed);
+                    pos_indices[1] -= 1;
+                    auto const [height_m, dheight_x_m] = tile2dInterp1d(
+                        pos_indices, pos_rel, 0, size, sparsity, height_max, interp_thr, seed);
 
-                    const double ratio = (1.0 - posRel[1] / interpThreshold[1]) / 2.0;
+                    double const ratio = (1.0 - pos_rel[1] / interp_thr[1]) / 2.0;
                     height = height_0 + (height_m - height_0) * ratio;
-                    const double dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio;
-                    const double dheight_y =
-                        (height_0 - height_m) / (2.0 * size[1] * interpThreshold[1]);
+                    double const dheight_x = dheight_x_0 + (dheight_x_m - dheight_x_0) * ratio;
+                    double const dheight_y =
+                        (height_0 - height_m) / (2.0 * size[1] * interp_thr[1]);
                     normal << -dheight_x, -dheight_y, 1.0;
                     normal.normalize();
                 }
                 else
                 {
-                    posIdx[1] += 1;
-                    const auto [height_p, dheight_x_p] = tile2dInterp1d(
-                        posIdx, posRel, 0, size, sparsity, heightMax, interpThreshold, seed);
+                    pos_indices[1] += 1;
+                    auto const [height_p, dheight_x_p] = tile2dInterp1d(
+                        pos_indices, pos_rel, 0, size, sparsity, height_max, interp_thr, seed);
 
-                    const double ratio = (1.0 + (posRel[1] - 1.0) / interpThreshold[1]) / 2.0;
+                    double const ratio = (1.0 + (pos_rel[1] - 1.0) / interp_thr[1]) / 2.0;
                     height = height_0 + (height_p - height_0) * ratio;
-                    const double dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio;
-                    const double dheight_y =
-                        (height_p - height_0) / (2.0 * size[1] * interpThreshold[1]);
+                    double const dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio;
+                    double const dheight_y =
+                        (height_p - height_0) / (2.0 * size[1] * interp_thr[1]);
                     normal << -dheight_x, -dheight_y, 1.0;
                     normal.normalize();
                 }
             }
             else
             {
-                height = randomDouble(posIdx, sparsity, heightMax, seed);
+                height = height_max * uniformSparseFromState(pos_indices, sparsity, seed);
                 normal = Eigen::Vector3d::UnitZ();
             }
-
-            return std::make_pair(height, std::move(normal));
-        };
-    }
-
-    HeightmapFunctor sumHeightmap(const std::vector<HeightmapFunctor> & heightmaps)
-    {
-        if (heightmaps.size() == 1)
-        {
-            return heightmaps[0];
-        }
-        return [heightmaps](const Eigen::Vector3d & pos3) -> std::pair<double, Eigen::Vector3d>
-        {
-            double height = 0.0;
-            Eigen::Vector3d normal = Eigen::Vector3d::Zero();
-            for (const HeightmapFunctor & heightmap : heightmaps)
-            {
-                const auto [height_i, normal_i] = heightmap(pos3);
-                height += height_i;
-                normal += normal_i;
-            }
-            normal.normalize();
-            return std::make_pair(height, std::move(normal));
-        };
-    }
-
-    HeightmapFunctor mergeHeightmap(const std::vector<HeightmapFunctor> & heightmaps)
-    {
-        if (heightmaps.size() == 1)
-        {
-            return heightmaps[0];
-        }
-        return [heightmaps](const Eigen::Vector3d & pos3) -> std::pair<double, Eigen::Vector3d>
-        {
-            double heightmax = -INF;
-            Eigen::Vector3d normal;  // It will be initialized to `normal_i`
-            bool isDirty = false;
-            for (const HeightmapFunctor & heightmap : heightmaps)
-            {
-                const auto [height, normal_i] = heightmap(pos3);
-                if (std::abs(height - heightmax) < EPS)
-                {
-                    normal += normal_i;
-                    isDirty = true;
-                }
-                else if (height > heightmax)
-                {
-                    heightmax = height;
-                    normal = normal_i;
-                    isDirty = false;
-                }
-            }
-            if (isDirty)
-            {
-                normal.normalize();
-            }
-            return std::make_pair(heightmax, std::move(normal));
         };
     }
-
-    Eigen::MatrixXd discretizeHeightmap(
-        const HeightmapFunctor & heightmap, double gridSize, double gridUnit)
-    {
-        // Allocate empty discrete grid
-        uint32_t gridDim = static_cast<int32_t>(std::ceil(gridSize / gridUnit)) + 1U;
-        Eigen::MatrixXd heightGrid(gridDim * gridDim, 6);
-
-        // Fill x and y discrete grid coordinates
-        const Eigen::VectorXd values =
-            (Eigen::VectorXd::LinSpaced(gridDim, 0, gridDim - 1) * gridUnit).array() -
-            (gridDim - 1) * (gridUnit / 2.0);
-        Eigen::Map<Eigen::MatrixXd>(heightGrid.col(0).data(), gridDim, gridDim).colwise() = values;
-        Eigen::Map<Eigen::MatrixXd>(heightGrid.col(1).data(), gridDim, gridDim).rowwise() =
-            values.transpose();
-
-        // Fill discrete grid
-        for (uint32_t i = 0; i < heightGrid.rows(); ++i)
-        {
-            auto result = heightmap(heightGrid.block<1, 3>(i, 0));
-            heightGrid(i, 2) = std::get<double>(result);
-            heightGrid.block<1, 3>(i, 3) = std::get<Eigen::Vector3d>(result);
-        }
-
-        return heightGrid;
-    }
-}
+}
\ No newline at end of file
diff --git a/core/unit/miscellaneous.cc b/core/unit/miscellaneous.cc
index 70580a5d1..4e0ebb94f 100644
--- a/core/unit/miscellaneous.cc
+++ b/core/unit/miscellaneous.cc
@@ -1,7 +1,9 @@
+#include <gmock/gmock.h>
 #include <gtest/gtest.h>
 
 #include "jiminy/core/fwd.h"
 #include "jiminy/core/utilities/helpers.h"
+#include "jiminy/core/utilities/random.h"
 
 
 using namespace jiminy;
@@ -55,3 +57,66 @@ TEST(Miscellaneous, swapMatrixRows)
                      matrixOut);
     }
 }
+
+
+TEST(Miscellaneous, MatrixRandom)
+{
+    using generator_t =
+        std::independent_bits_engine<std::mt19937, std::numeric_limits<uint32_t>::digits, uint32_t>;
+
+    generator_t gen32{0};
+    uniform_random_bit_generator_ref<uint32_t> gen32_ref = gen32;
+
+    float mean = 5.0;
+    float stddev = 2.0;
+
+    auto mean_vec = Eigen::MatrixXf::Constant(1, 2, mean);
+    auto stddev_vec = Eigen::MatrixXf::Constant(1, 2, stddev);
+    float value1 = normal(gen32, mean, stddev);
+    float value2 = normal(gen32, mean, stddev);
+
+    {
+        gen32.seed(0);
+        scalar_random_op<float(generator_t &, float, float)> op{
+            [](auto & g, float _mean, float _stddev) -> float
+            { return normal(g, _mean, _stddev); },
+            gen32,
+            mean_vec,
+            stddev_vec};
+        ASSERT_FLOAT_EQ(op(0, 0), value1);
+        ASSERT_FLOAT_EQ(op(0, 1), value2);
+        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        scalar_random_op<float(const uniform_random_bit_generator_ref<uint32_t> &, float, float),
+                         uniform_random_bit_generator_ref<uint32_t>,
+                         float,
+                         float>
+            op{normal, gen32, mean, stddev};
+        ASSERT_FLOAT_EQ(op(0, 0), value1);
+        ASSERT_FLOAT_EQ(op(10, 10), value2);
+        ASSERT_THAT(op(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(gen32_ref, mean_vec, stddev_vec);
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(1, 2, gen32, mean, stddev);
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(0, 1), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+    {
+        gen32.seed(0);
+        auto mat_expr = normal(gen32_ref, mean, stddev_vec.transpose());
+        ASSERT_FLOAT_EQ(mat_expr(0, 0), value1);
+        ASSERT_FLOAT_EQ(mat_expr(1, 0), value2);
+        ASSERT_THAT(mat_expr(0, 0), testing::Not(testing::FloatEq(value1)));
+    }
+}
diff --git a/data/bipedal_robots/atlas/atlas_v4_options.toml b/data/bipedal_robots/atlas/atlas_v4_options.toml
index af6076f69..63d5a9b6f 100644
--- a/data/bipedal_robots/atlas/atlas_v4_options.toml
+++ b/data/bipedal_robots/atlas/atlas_v4_options.toml
@@ -6,7 +6,7 @@ odeSolver = "euler_explicit"
 sensorsUpdatePeriod = 0.005
 controllerUpdatePeriod = 0.005
 logInternalStepperSteps = false
-randomSeed = 0
+randomSeedSeq = [0,]
 
 # ============== Contact dynamics ===============
 
diff --git a/data/bipedal_robots/cassie/cassie_options.toml b/data/bipedal_robots/cassie/cassie_options.toml
index ae54cb88f..4ed0ded67 100644
--- a/data/bipedal_robots/cassie/cassie_options.toml
+++ b/data/bipedal_robots/cassie/cassie_options.toml
@@ -6,7 +6,7 @@ odeSolver = "euler_explicit"
 sensorsUpdatePeriod = 0.005
 controllerUpdatePeriod = 0.005
 logInternalStepperSteps = false
-randomSeed = 0
+randomSeedSeq = [0,]
 
 # ============== Contact dynamics ===============
 
diff --git a/data/bipedal_robots/digit/digit_options.toml b/data/bipedal_robots/digit/digit_options.toml
index 30982827f..6c1a540fa 100644
--- a/data/bipedal_robots/digit/digit_options.toml
+++ b/data/bipedal_robots/digit/digit_options.toml
@@ -6,7 +6,7 @@ odeSolver = "euler_explicit"
 sensorsUpdatePeriod = 0.005
 controllerUpdatePeriod = 0.005
 logInternalStepperSteps = false
-randomSeed = 0
+randomSeedSeq = [0,]
 
 # ============== Ground dynamics ===============
 
diff --git a/data/quadrupedal_robots/anymal/anymal_options.toml b/data/quadrupedal_robots/anymal/anymal_options.toml
index bf8984224..e16cb97f8 100644
--- a/data/quadrupedal_robots/anymal/anymal_options.toml
+++ b/data/quadrupedal_robots/anymal/anymal_options.toml
@@ -6,7 +6,7 @@ odeSolver = "euler_explicit"
 sensorsUpdatePeriod = 0.005
 controllerUpdatePeriod = 0.005
 logInternalStepperSteps = false
-randomSeed = 0
+randomSeedSeq = [0,]
 
 # ============== Ground dynamics ===============
 
diff --git a/data/toys_models/ant/ant_options.toml b/data/toys_models/ant/ant_options.toml
index 5640a27f1..0051ad794 100644
--- a/data/toys_models/ant/ant_options.toml
+++ b/data/toys_models/ant/ant_options.toml
@@ -6,7 +6,7 @@ odeSolver = "runge_kutta_4"
 sensorsUpdatePeriod = 0.01
 controllerUpdatePeriod = 0.01
 logInternalStepperSteps = false
-randomSeed = 0
+randomSeedSeq = [0,]
 
 # ============== Contact dynamics ===============
 
diff --git a/docs/api/jiminy/utilities.rst b/docs/api/jiminy/utilities.rst
index bbb3a8d54..a712640ad 100644
--- a/docs/api/jiminy/utilities.rst
+++ b/docs/api/jiminy/utilities.rst
@@ -25,22 +25,49 @@ Conversion from/to JSON
 .. doxygenfunction:: jiminy::convertFromJson(Json::Value const &value)
    :project: jiminy
 
+Geometry
+--------
+
+.. doxygenfunction:: jiminy::sumHeightmaps
+   :project: jiminy
+
+.. doxygenfunction:: jiminy::mergeHeightmaps
+   :project: jiminy
+
+.. doxygenfunction:: jiminy::discretizeHeightmap
+   :project: jiminy
 
 Random number generation
 ------------------------
 
-.. doxygenfunction:: jiminy::resetRandomGenerators
+.. doxygenfunction:: jiminy::PCG32
+   :project: jiminy
+   :members:
+
+.. doxygenfunction:: jiminy::uniform
    :project: jiminy
 
-.. doxygenfunction:: jiminy::randUniform
+.. doxygenfunction:: jiminy::normal
    :project: jiminy
 
-.. doxygenfunction:: jiminy::randNormal
+.. doxygenfunction:: jiminy::PeriodicGaussianProcess
    :project: jiminy
+   :members:
 
-.. doxygenfunction:: jiminy::randVectorNormal(uint32_t const &size, double const &mean, double const &std)
+.. doxygenfunction:: jiminy::PeriodicFourierProcess
    :project: jiminy
+   :members:
+
+.. doxygenfunction:: jiminy::RandomPerlinProcess
+   :project: jiminy
+   :members:
 
+.. doxygenfunction:: jiminy::PeriodicPerlinProcess
+   :project: jiminy
+   :members:
+
+.. doxygenfunction:: jiminy::tiles
+   :project: jiminy
 
 Telemetry
 ---------
diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py
index 62469d012..a9ee6cae5 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/bases/generic_bases.py
@@ -155,8 +155,6 @@ def compute_reward(self,
 
         :returns: Aggregated reward for the current step.
         """
-        # pylint: disable=unused-argument
-
         raise NotImplementedError
 
 
diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py
index dc37269fc..bd99870aa 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/bases/pipeline_bases.py
@@ -73,8 +73,6 @@ def __init__(self,
         """
         :param kwargs: Extra keyword arguments for multiple inheritance.
         """
-        # pylint: disable=unused-argument
-
         # Initialize some proxies for fast lookup
         self.simulator = env.simulator
         self.robot = env.robot
diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
index 5bd48413b..1a1da1165 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py
@@ -199,9 +199,7 @@ def __init__(self,
             self._registered_variables, 0)
 
         # Internal buffers for physics computations
-        self._seed: List[np.uint32] = []
-        self.np_random = np.random.Generator(
-            np.random.SFC64(np.random.SeedSequence()))
+        self.np_random = np.random.Generator(np.random.SFC64())
         self.log_path: Optional[str] = None
 
         # Whether evaluation mode is active
@@ -529,7 +527,7 @@ def _initialize_action_space(self) -> None:
         self.action_space = spaces.Box(
             low=-action_scale, high=action_scale, dtype=np.float64)
 
-    def _initialize_seed(self, seed: Optional[int] = None) -> List[np.uint32]:
+    def _initialize_seed(self, seed: Optional[int] = None) -> None:
         """Specify the seed of the environment.
 
         .. note::
@@ -545,20 +543,20 @@ def _initialize_seed(self, seed: Optional[int] = None) -> List[np.uint32]:
 
         :returns: Updated seed of the environment
         """
-        # Generate a sequence of 3 bytes uint32 seeds
-        self._seed = list(np.random.SeedSequence(seed).generate_state(3))
+        # Generate distinct sequences of 3 bytes uint32 seeds for the engine
+        # and environment.
+        engine_seed = np.random.SeedSequence(seed).generate_state(3)
+        np_seed = np.random.SeedSequence(engine_seed).generate_state(3)
 
         # Re-initialize the low-level bit generator based on the provided seed
-        self.np_random.bit_generator.state = np.random.SFC64(self._seed).state
+        self.np_random.bit_generator.state = np.random.SFC64(np_seed).state
 
         # Reset the seed of the action and observation spaces
         self.observation_space.seed(seed)
         self.action_space.seed(seed)
 
         # Reset the seed of Jiminy Engine
-        self.simulator.seed(self._seed[0])
-
-        return self._seed
+        self.simulator.seed(engine_seed)
 
     def register_variable(self,
                           name: str,
@@ -1486,8 +1484,6 @@ def compute_command(self, action: ActT) -> np.ndarray:
 
         :param action: High-level target to achieve by means of the command.
         """
-        # pylint: disable=unused-argument
-
         # Check if the action is out-of-bounds, in debug mode only
         if self.debug and not self._contains_action():
             LOGGER.warning("The action is out-of-bounds.")
diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py
index 4345c09db..046fb9d00 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py
@@ -314,7 +314,7 @@ def _setup(self) -> None:
 
             # Schedule a single periodic force profile applied on PelvisLink
             for func in self._f_xy_profile:
-                func.reset()
+                func.reset(self.np_random)
             self.simulator.register_force_profile(
                 frame_name, self._force_external_profile)
 
diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py b/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py
index ed2fab218..af7da36cb 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py
@@ -112,7 +112,6 @@ def register_variables(controller: jiminy.AbstractController,
 
     :returns: Whether the registration has been successful.
     """
-    # pylint: disable=cell-var-from-loop
     for fieldname, value in zip(
             tree.flatten_up_to(data, fieldnames),
             tree.flatten(data)):
diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py b/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py
index f740e8db9..f39b2c25b 100644
--- a/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py
+++ b/python/gym_jiminy/common/gym_jiminy/common/utils/spaces.py
@@ -7,7 +7,7 @@
 from typing import (
     Any, Dict, Optional, Union, Sequence as SequenceT, Mapping as MappingT,
     Iterable as IterableT, Tuple, Literal, SupportsFloat, TypeVar, Type,
-    Callable, no_type_check, cast)
+    Callable, Protocol, no_type_check, cast)
 
 import numba as nb
 import numpy as np
@@ -37,6 +37,15 @@
 ArrayOrScalarT = TypeVar('ArrayOrScalarT', bound=ArrayOrScalar)
 
 
+class RandomDistribution(Protocol):
+    """Protocol to be satisfied when passing generic statistical distribution
+    callable to `sample` method.
+    """
+    def __call__(self, rg: np.random.Generator, *args: Any, **kwargs: Any
+                 ) -> ArrayOrScalar:
+        ...
+
+
 @no_type_check
 @nb.jit(nopython=True, cache=True)
 def _array_clip(value: np.ndarray,
@@ -88,9 +97,10 @@ def _array_contains(value: np.ndarray,
         tol_nd = np.full_like(value, tol_abs)
         if low is not None and high is not None and tol_rel > 0.0:
             tol_nd = np.maximum((high - low) * tol_rel, tol_nd)
-        if low is not None and (low - tol_nd > value).all():
+        # Reversed bound check because 'all' is always true for empty arrays
+        if low is not None and not (low - tol_nd <= value).all():
             return False
-        if high is not None and (value > high + tol_nd).all():
+        if high is not None and not (value <= high + tol_nd).all():
             return False
         return True
     tol_0d = tol_abs
@@ -140,7 +150,7 @@ def get_bounds(space: gym.Space
 
 def sample(low: Union[float, np.ndarray] = -1.0,
            high: Union[float, np.ndarray] = 1.0,
-           dist: str = 'uniform',
+           dist: Union[str, RandomDistribution] = 'uniform',
            scale: Union[float, np.ndarray] = 1.0,
            enable_log_scale: bool = False,
            shape: Optional[SequenceT[int]] = None,
@@ -160,8 +170,15 @@ def sample(low: Union[float, np.ndarray] = -1.0,
     :param high: Upper value for bounded distribution, positive-side standard
                  deviation otherwise.
                  Optional: 1.0 by default.
-    :param dist: Name of the statistical distribution from which to draw
-                 samples. It must be a member function for `np.random`.
+    :param dist: The statistical from which to draw samples, either provided as
+                 a pre-defined string or a callable. For strings, then it must
+                 be a member function of `np.random.Generator` (only 'uniform'
+                 and 'normal' are supported for now). For callables, it must
+                 corresponds to a standardized distribution and satisfying
+                 `gym_jiminy.common.utils.RandomDistribution` protocol. This is
+                 especially useful for specifying custom parameters of complex
+                 distributions such as Beta. Using `functools.partial` is
+                 recommended, eg `partial(np.random.Generator.Beta, a=1, b=8)`.
                  Optional: 'uniform' by default.
     :param scale: Shrink the standard deviation of the distribution around the
                   mean by this factor.
@@ -173,12 +190,7 @@ def sample(low: Union[float, np.ndarray] = -1.0,
     :param rg: Custom random number generator from which to draw samples.
                Optional: Default to `np.random`.
     """
-    # Make sure the distribution is supported
-    if dist not in ('uniform', 'normal'):
-        raise NotImplementedError(
-            f"'{dist}' distribution type is not supported for now.")
-
-    # Extract mean and deviation from min/max
+    # Compute mean and deviation from low and high arguments
     mean = 0.5 * (low + high)
     dev = 0.5 * scale * (high - low)
 
@@ -193,25 +205,33 @@ def sample(low: Union[float, np.ndarray] = -1.0,
                 np.broadcast(np.empty(shape, dtype=[]), dev)
             except ValueError as e:
                 raise ValueError(
-                    f"'shape' {shape} must be broadcastable with 'low', "
+                    f"'shape' {shape} must be broadcast-able with 'low', "
                     f"'high' and 'scale' {dev.shape} if specified.") from e
 
-    # Sample from normalized distribution.
-    # Note that some distributions are not normalized by default.
-    distrib_fn = getattr(rg or GLOBAL_RNG, dist)
-    if dist == 'uniform':
-        value = distrib_fn(low=-1.0, high=1.0, size=shape)
+    # Define "standardized" distribution callable if only its name was provided
+    if isinstance(dist, str):
+        if dist not in ('uniform', 'normal'):
+            raise NotImplementedError(
+                f"'{dist}' distribution type is not supported for now.")
+        dist_fn = getattr(np.random.Generator, dist)
+        if dist == 'uniform':
+            # The uniform distribution is NOT standardized by default
+            dist_fn = partial(dist_fn, low=-1.0, high=1.0)
     else:
-        value = distrib_fn(size=shape)
+        dist_fn = dist
+
+    # Generate samples from distribution.
+    # Make sure that the result is always returned as np.ndarray.
+    value = np.asarray(dist_fn(rg or GLOBAL_RNG, size=shape))
 
-    # Set mean and deviation
+    # Apply mean and standard deviation transformation
     value = mean + dev * value
 
-    # Revert log scale if appropriate
+    # Revert log scale if requested
     if enable_log_scale:
         value = 10 ** value
 
-    return np.asarray(value)
+    return value
 
 
 def is_bounded(space_nested: gym.Space) -> bool:
diff --git a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py
index bb240af5d..d950fb41a 100644
--- a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py
+++ b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/qhull.py
@@ -5,7 +5,7 @@
 import numpy as np
 import numba as nb
 from numba.np.extensions import cross2d
-from scipy.spatial.qhull import _qhull
+from scipy.spatial import _qhull
 
 from .generic import squared_norm_2
 
diff --git a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/spline.py b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/spline.py
index 30c1dfc4e..de55e73c0 100644
--- a/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/spline.py
+++ b/python/gym_jiminy/toolbox/gym_jiminy/toolbox/math/spline.py
@@ -13,13 +13,13 @@
 from torch import nn
 
 
-coefs_deriv = torch.tensor([[1, 1, 1, 1],  # pylint: disable=not-callable
+coefs_deriv = torch.tensor([[1, 1, 1, 1],
                             [0, 1, 2, 3],
                             [0, 0, 2, 6],
                             [0, 0, 0, 6],
                             [0, 0, 0, 0]],
                            dtype=torch.float32)
-A = torch.tensor([[1, -3,  3, -1],  # pylint: disable=not-callable
+A = torch.tensor([[1, -3,  3, -1],
                   [0,  3, -6,  3],
                   [0,  0,  3, -3],
                   [0,  0,  0,  1]],
diff --git a/python/gym_jiminy/toolbox/setup.py b/python/gym_jiminy/toolbox/setup.py
index a561a83d4..db22695aa 100644
--- a/python/gym_jiminy/toolbox/setup.py
+++ b/python/gym_jiminy/toolbox/setup.py
@@ -36,7 +36,8 @@
         # No wheel is distributed on pypi for PyPy, and pip requires to install
         # `libatlas-base-dev` system dependency to build it from source.
         # 1.8.0: `scipy.spatial.qhull` low-level API changes.
-        "scipy>1.8.0"
+        # 1.9.2: First release to support Python 3.11
+        "scipy>=1.9.2"
     ],
     zip_safe=False
 )
diff --git a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py
index 59fc751ca..1e590fd25 100644
--- a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py
+++ b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py
@@ -4,7 +4,7 @@
 import numpy as np
 
 from jiminy_py.simulator import Simulator
-from jiminy_py.core import random_tile_ground, sum_heightmap, merge_heightmap
+from jiminy_py.core import random_tile_ground, sum_heightmaps, merge_heightmaps
 
 import pinocchio as pin
 
@@ -24,44 +24,44 @@
 TILE_ORIENTATION = [0.0, np.pi / 4.0, 0.0]
 TILE_SEED = range(3)
 
+if __name__ == '__main__':
+    # Create a gym environment for a simple cube
+    urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/box_collision_mesh.urdf"
+    simulator = Simulator.build(urdf_path, has_freeflyer=True)
 
-# Create a gym environment for a simple cube
-urdf_path = f"{MODULE_DIR}/../../jiminy_py/unit_py/data/box_collision_mesh.urdf"
-simulator = Simulator.build(urdf_path, has_freeflyer=True)
+    # Enable constraint contact model
+    engine_options = simulator.engine.get_options()
+    engine_options['contacts']['model'] = 'constraint'
+    engine_options['contacts']['stabilizationFreq'] = 20.0
+    engine_options["constraints"]["regularization"] = 0.0
 
-# Enable constraint contact model
-engine_options = simulator.engine.get_options()
-engine_options['contacts']['model'] = 'constraint'
-engine_options['contacts']['stabilizationFreq'] = 20.0
-engine_options["constraints"]["regularization"] = 0.0
+    # Configure integrator
+    engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5'
+    engine_options['stepper']['dtMax'] = 1.0e-3
 
-# Configure integrator
-engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5'
-engine_options['stepper']['dtMax'] = 1.0e-3
+    # Set the ground contact options
+    engine_options['contacts']['friction'] = 1.0
+    engine_options['contacts']['torsion'] = 0.0
+    simulator.engine.set_options(engine_options)
 
-# Set the ground contact options
-engine_options['contacts']['friction'] = 1.0
-engine_options['contacts']['torsion'] = 0.0
-simulator.engine.set_options(engine_options)
+    # Generate random ground profile
+    ground_params = list(starmap(random_tile_ground, zip(
+        TILE_SIZE, TILE_HEIGHT_MAX, TILE_INTERP_DELTA, TILE_SPARSITY,
+        TILE_ORIENTATION, TILE_SEED)))
+    engine_options["world"]["groundProfile"] = sum_heightmaps([
+        ground_params[0], merge_heightmaps(ground_params[1:])])
+    simulator.engine.set_options(engine_options)
 
-# Generate random ground profile
-ground_params = list(starmap(random_tile_ground, zip(
-    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:])])
-simulator.engine.set_options(engine_options)
+    # Sample the initial state
+    qpos = pin.neutral(simulator.system.robot.pinocchio_model)
+    qvel = np.zeros(simulator.system.robot.nv)
+    qpos[2] += 1.5
+    qvel[0] = 2.0
+    qvel[3] = 1.0
+    qvel[5] = 2.0
 
-# Sample the initial state
-qpos = pin.neutral(simulator.system.robot.pinocchio_model)
-qvel = np.zeros(simulator.system.robot.nv)
-qpos[2] += 1.5
-qvel[0] = 2.0
-qvel[3] = 1.0
-qvel[5] = 2.0
+    # Run a simulation
+    simulator.simulate(5.0, qpos, qvel)
 
-# Run a simulation
-simulator.simulate(5.0, qpos, qvel)
-
-# Replay the simulation
-simulator.replay(enable_travelling=False, display_contact_frames=True)
+    # Replay the simulation
+    simulator.replay(enable_travelling=False, display_contact_frames=True)
diff --git a/python/jiminy_py/examples/double_pendulum.py b/python/jiminy_py/examples/double_pendulum.py
index 8432ea221..5dcaa969d 100644
--- a/python/jiminy_py/examples/double_pendulum.py
+++ b/python/jiminy_py/examples/double_pendulum.py
@@ -63,7 +63,7 @@ def internalDynamics(t, q, v, sensors_data, u_custom):
 engine_options["stepper"]["sensorsUpdatePeriod"] = 1.0e-3
 engine_options["stepper"]["controllerUpdatePeriod"] = 1.0e-3
 engine_options["stepper"]["logInternalStepperSteps"] = False
-engine_options["stepper"]["randomSeed"] = 0
+engine_options["stepper"]["randomSeedSeq"] = np.array([0,], dtype=np.uint32)
 engine_options['contacts']['model'] = "spring_damper"
 engine_options['contacts']['stiffness'] = 1.0e6
 engine_options['contacts']['damping'] = 2000.0
diff --git a/python/jiminy_py/setup.py b/python/jiminy_py/setup.py
index ffe9aa933..1e2b75ecd 100644
--- a/python/jiminy_py/setup.py
+++ b/python/jiminy_py/setup.py
@@ -153,7 +153,7 @@ def finalize_options(self) -> None:
             # Check PEP8 conformance of Python native code
             "flake8",
             # Python linter
-            "pylint>=2.16.0",
+            "pylint>=3.0",
             # Python static type checker
             "mypy>=1.5.0",
             # Dependency for documentation generation
diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py
index 71d48e56c..d84ce82dd 100644
--- a/python/jiminy_py/src/jiminy_py/dynamics.py
+++ b/python/jiminy_py/src/jiminy_py/dynamics.py
@@ -423,7 +423,8 @@ def compute_transform_contact(
         contact_ground_transform = []
         ground_pos = np.zeros(3)
         for frame_transform in contact_frames_transform:
-            ground_pos[2], normal = ground_profile(frame_transform.translation)
+            ground_pos[2], normal = ground_profile(
+                frame_transform.translation[:2])
             ground_rot = pin.Quaternion.FromTwoVectors(
                 np.array([0.0, 0.0, 1.0]), normal).matrix()
             contact_ground_transform.append(pin.SE3(ground_rot, ground_pos))
@@ -611,7 +612,7 @@ def compute_freeflyer_state_from_fixed_body(
         if ground_profile is not None:
             ground_translation = np.zeros(3)
             ground_translation[2], normal = ground_profile(
-                ff_M_fixed_body.translation)
+                ff_M_fixed_body.translation[:2])
             ground_rotation = pin.Quaternion.FromTwoVectors(
                 np.array([0.0, 0.0, 1.0]), normal).matrix()
             w_M_ground = pin.SE3(ground_rotation, ground_translation)
diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py
index 4735e4d7a..f4fdffb3d 100644
--- a/python/jiminy_py/src/jiminy_py/simulator.py
+++ b/python/jiminy_py/src/jiminy_py/simulator.py
@@ -294,28 +294,23 @@ def _callback(self,
         """
         out[()] = True
 
-    def seed(self, seed: np.uint32) -> None:
+    def seed(self, seed: Union[np.uint32, np.ndarray]) -> None:
         """Set the seed of the simulation and reset the simulation.
 
         .. warning::
             It also resets the low-level jiminy Engine. Therefore one must call
             the `reset` method manually afterward.
 
-        :param seed: Desired seed (Unsigned integer 32 bits).
+        :param seed: Desired seed as a sequence of unsigned integers 32 bits.
         """
-        assert isinstance(seed, np.uint32), "'seed' must have type np.uint32."
+        assert seed.dtype == np.uint32, "'seed' must have dtype np.uint32."
 
         # Make sure no simulation is running before setting the seed
         self.engine.stop()
 
-        # 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.
+        # Set the seed in engine options
         engine_options = self.engine.get_options()
-        engine_options["stepper"]["randomSeed"] = \
-            np.array(seed, dtype=np.dtype('uint32'))
+        engine_options["stepper"]["randomSeedSeq"] = np.asarray(seed)
         self.engine.set_options(engine_options)
 
         # It is expected by OpenAI Gym API to reset env after setting the seed
@@ -540,7 +535,7 @@ def render(self,
 
         # Enable the ground profile is requested and available
         assert self.viewer is not None and self.viewer.backend is not None
-        if self.viewer.backend.startswith('panda3d') and update_ground_profile:
+        if update_ground_profile:
             engine_options = self.engine.get_options()
             ground_profile = engine_options["world"]["groundProfile"]
             Viewer.update_floor(ground_profile, show_meshes=False)
diff --git a/python/jiminy_py/src/jiminy_py/viewer/geometry.py b/python/jiminy_py/src/jiminy_py/viewer/geometry.py
new file mode 100644
index 000000000..13ac0285a
--- /dev/null
+++ b/python/jiminy_py/src/jiminy_py/viewer/geometry.py
@@ -0,0 +1,54 @@
+""" TODO: Write documentation.
+"""
+from typing import Tuple
+
+import numpy as np
+
+import hppfcl
+
+
+def extract_vertices_and_faces_from_geometry(
+        geom: hppfcl.CollisionGeometry) -> Tuple[np.ndarray, np.ndarray]:
+    """Extract vertices and faces from a triangle-based collision geometry
+
+    :param geom: Collision geometry from which to extract data.
+
+    :returns: Tuple containing the vertices and faces as np.ndarray objects
+              in this exact order.
+    """
+    if isinstance(geom, hppfcl.HeightFieldOBBRSS):
+        x_grid, y_grid = geom.getXGrid(), geom.getYGrid()
+        x_dim, y_dim = len(x_grid), len(y_grid)
+        vertices = np.stack((
+            np.tile(x_grid, y_dim),
+            np.repeat(y_grid, x_dim),
+            geom.getHeights().flat
+        ), axis=1, dtype=np.float32)
+
+        num_faces = 2 * (x_dim - 1) * (y_dim - 1)
+        faces = np.empty((num_faces, 3), dtype=np.uint32)
+        tri_index = 0
+        for i in range(x_dim - 1):
+            for j in range(y_dim - 1):
+                k = j * x_dim + i
+                faces[tri_index:(tri_index + 2)].flat[:] = (
+                    k, k + x_dim + 1, k + 1,
+                    k, k + x_dim, k + x_dim + 1)
+                tri_index += 2
+    else:
+        if isinstance(geom, hppfcl.Convex):
+            vertices = geom.points()
+            num_faces, get_faces = geom.num_polygons, geom.polygons
+        elif isinstance(geom, hppfcl.BVHModelBase):
+            vertices = geom.vertices().astype(np.float32)
+            num_faces, get_faces = geom.num_tris, geom.tri_indices
+        else:
+            raise ValueError(f"CollisionGeometry '{geom}' is not supported.")
+
+        faces = np.empty((num_faces, 3), dtype=np.uint32)
+        for i in range(num_faces):
+            tri = get_faces(i)
+            for j in range(3):
+                faces[i, j] = tri[j]
+
+    return vertices, faces
diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py
index 97bdf7726..340b3c069 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py
@@ -12,6 +12,7 @@
 
 import meshcat
 import meshcat.path
+import meshcat.commands
 from meshcat.geometry import (
     ReferenceSceneElement, Geometry, TriangularMeshGeometry)
 
@@ -20,6 +21,8 @@
 from pinocchio.utils import npToTuple
 from pinocchio.visualize import BaseVisualizer
 
+from ..geometry import extract_vertices_and_faces_from_geometry
+
 
 MsgType = Dict[str, Union[str, bytes, bool, float, 'MsgType']]
 
@@ -170,6 +173,36 @@ def lower(self) -> Dict[str, Any]:
         return data
 
 
+def update_floor(viewer: meshcat.Visualizer,
+                 geom: Optional[hppfcl.CollisionGeometry] = None) -> None:
+    """Display a custom collision geometry as ground profile or a flat tile
+    ground floor in a given viewer instance.
+
+    :param viewer: Meshcat viewer instance.
+    :param geom: Ground profile as a collision geometry object, as provided by
+                 `jiminy.discretize_heightmap`. It renders a flat tile ground
+                 if not specified.
+                 Optional: None by default.
+    """
+    # Disable the existing custom ground if any and show original flat ground
+    if geom is None:
+        viewer["Ground"].delete()
+        viewer.window.send(meshcat.commands.SetProperty(
+            "visible", True, meshcat.path.Path(("Grid",))))
+        return
+
+    # Convert provided geometry in Meshcat-specific triangle-based geometry
+    vertices, faces = extract_vertices_and_faces_from_geometry(geom)
+    obj = TriangularMeshGeometry(vertices, faces)
+    material = meshcat.geometry.MeshLambertMaterial()
+    material.color = (255 << 16) + (255 << 8) + 255
+
+    # Disable newly created custom ground profile and hide original flat ground
+    viewer["Ground"].set_object(obj, material)
+    viewer.window.send(meshcat.commands.SetProperty(
+        "visible", False, meshcat.path.Path(("Grid",))))
+
+
 class MeshcatVisualizer(BaseVisualizer):
     """A Pinocchio display using Meshcat.
 
@@ -177,7 +210,7 @@ class MeshcatVisualizer(BaseVisualizer):
     Copyright (c) 2014-2020, CNRS
     Copyright (c) 2018-2020, INRIA
     """  # noqa: E501  # pylint: disable=line-too-long
-    def initViewer(self,  # pylint: disable=invalid-name,arguments-differ
+    def initViewer(self,  # pylint: disable=arguments-differ
                    viewer: meshcat.Visualizer = None,
                    loadModel: bool = False,
                    mustOpen: bool = False,
@@ -204,7 +237,7 @@ def initViewer(self,  # pylint: disable=invalid-name,arguments-differ
         if loadModel:
             self.loadViewerModel(root_node_name=self.model.name)
 
-    def getViewerNodeName(self,  # pylint: disable=invalid-name
+    def getViewerNodeName(self,
                           geometry_object: pin.GeometryObject,
                           geometry_type: pin.GeometryType) -> str:
         """Return the name of the geometry object inside the viewer.
@@ -239,29 +272,21 @@ def loadPrimitive(self,  # pylint: disable=invalid-name
             obj = meshcat.geometry.Sphere(geom.radius)
         elif isinstance(geom, hppfcl.Cone):
             obj = Cone(2. * geom.halfLength, geom.radius)
-        elif isinstance(geom, (hppfcl.Convex, hppfcl.BVHModelBase)):
-            # Extract vertices and faces from geometry
-            if isinstance(geom, hppfcl.Convex):
-                vertices = geom.points()
-                num_faces, get_faces = geom.num_polygons, geom.polygons
-            else:
-                vertices = geom.vertices()
-                num_faces, get_faces = geom.num_tris, geom.tri_indices
-            faces = np.empty((num_faces, 3), dtype=np.int32)
-            for i in range(num_faces):
-                tri = get_faces(i)
-                for j in range(3):
-                    faces[i, j] = tri[j]
-
-            # Create primitive triangle geometry
-            obj = TriangularMeshGeometry(vertices, faces)
-            geometry_object.meshScale = np.ones(3)  # It is already at scale !
         else:
-            warnings.warn(
-                f"Unsupported geometry type for {geometry_object.name} "
-                f"({type(geom)})",
-                category=UserWarning, stacklevel=2)
-            obj = None
+            # Try extract raw vertices and faces from geometry
+            try:
+                vertices, faces = (
+                    extract_vertices_and_faces_from_geometry(geom))
+            except ValueError:
+                warnings.warn(
+                    f"Unsupported geometry type for {geometry_object.name} "
+                    f"({type(geom)})",
+                    category=UserWarning, stacklevel=2)
+                return None
+
+            # Create Meshcat-specific triangle-based geometry
+            obj = TriangularMeshGeometry(vertices, faces)
+            geometry_object.meshScale = np.ones(3)  # Already at scale !
 
         return obj
 
@@ -327,8 +352,8 @@ def loadViewerGeometryObject(  # pylint: disable=invalid-name
             mesh_color = geometry_object.meshColor
         else:
             mesh_color = color
-        material.color = (int(mesh_color[0] * 255) * 256 ** 2 +
-                          int(mesh_color[1] * 255) * 256 +
+        material.color = ((int(mesh_color[0] * 255) << 16) +
+                          (int(mesh_color[1] * 255) << 8) +
                           int(mesh_color[2] * 255))
 
         # Add transparency, if needed.
@@ -378,8 +403,8 @@ def loadViewerModel(self,  # pylint: disable=arguments-differ
     def display(self,  # pylint: disable=signature-differs
                 q: np.ndarray) -> None:
         """Display the robot at configuration q in the viewer by placing all
-        the bodies."""
-        # pylint: disable=invalid-name
+        the bodies.
+        """
         pin.forwardKinematics(self.model, self.data, q)
 
         if self.display_visuals:
diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py
index d84044b67..4489833a9 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py
@@ -203,7 +203,6 @@ def forward_comm_thread() -> None:
             asyncio.set_event_loop(loop)
             self.__ioloop = tornado.ioloop.IOLoop()
 
-            # pylint: disable=abstract-class-instantiated
             # Start comm socket
             context = zmq.Context()
             self.__comm_socket = context.socket(zmq.XREQ)
diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
index ce7f660cb..ca8320234 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py
@@ -53,6 +53,8 @@
 from pinocchio.utils import npToTuple
 from pinocchio.visualize import BaseVisualizer
 
+from ..geometry import extract_vertices_and_faces_from_geometry
+
 
 WINDOW_SIZE_DEFAULT = (600, 600)
 CAMERA_POS_DEFAULT = [(4.0, -4.0, 1.5), (0, 0, 0.5)]
@@ -134,7 +136,7 @@ def make_gradient_skybox(sky_color: Tuple4FType,
                    Optional: 0.0 by default.
     :param subdiv: Number of sub-division for the complete gradient.
                    Optional: 2 by default.
-    """  # noqa: E501  # pylint: disable=line-too-long
+    """  # noqa: E501
     # Check validity of arguments
     assert subdiv > 1, "Number of sub-division must be strictly larger than 1."
     assert 0.0 <= span <= 1.0, "Offset must be in [0.0, 1.0]."
@@ -363,44 +365,6 @@ def make_torus(minor_radius: float = 0.2, num_segments: int = 16) -> Geom:
     return geom
 
 
-def make_heightmap(heightmap: np.ndarray) -> Geom:
-    """Create a unit squared height map.
-
-    :param heightmap: Elevation map along x-and y-axes as a as a 2D `nd.array`
-                      of shape [N_X * N_Y, 6], where N_X, N_Y are the number
-                      of vertices on x and y axes respectively, while the last
-                      dimension corresponds to the position (x, y, z) and
-                      normal (n_x, n_y, nz) of the vertex in space.
-    """
-    # Deduce the number of vertices
-    num_vertices, _ = heightmap.shape
-    x_dim, y_dim = (int(math.sqrt(num_vertices)),) * 2
-
-    # Allocation vertex
-    vformat = GeomVertexFormat.get_v3n3()
-    vdata = GeomVertexData('vdata', vformat, Geom.UH_static)
-    vdata.unclean_set_num_rows(num_vertices)
-
-    # Set vertex data
-    vdata_view = memoryview(vdata.modify_array(0)).cast("B")
-    vdata_view[:] = array.array("f", heightmap.reshape((-1,))).tobytes()
-
-    # Make triangles
-    prim = GeomTriangles(Geom.UHStatic)
-    prim.reserve_num_vertices(2 * (x_dim - 1) * (y_dim - 1))
-    for i in range(x_dim - 1):
-        for j in range(y_dim - 1):
-            k = i * x_dim + j
-            prim.addVertices(k, k + 1, k + x_dim)
-            prim.addVertices(k + 1, k + 1 + x_dim, k + x_dim)
-
-    # Create geometry object
-    geom = Geom(vdata)
-    geom.add_primitive(prim)
-
-    return geom
-
-
 class Panda3dApp(panda3d_viewer.viewer_app.ViewerApp):
     """A Panda3D based application.
     """
@@ -961,12 +925,12 @@ def _make_axes(self) -> NodePath:
         return node
 
     def _make_floor(self,
-                    heightmap: Optional[np.ndarray] = None,
+                    geom: Optional[Geom] = None,
                     show_meshes: bool = False) -> NodePath:
         model = GeomNode('floor')
         node = self.render.attach_new_node(model)
 
-        if heightmap is None:
+        if geom is None:
             for xi in range(-10, 10):
                 for yi in range(-10, 10):
                     tile = GeomNode(f"tile-{xi}.{yi}")
@@ -978,7 +942,7 @@ def _make_floor(self,
                     else:
                         tile_path.set_color((0.14, 0.14, 0.21, 1.0))
         else:
-            model.add_geom(make_heightmap(heightmap))
+            model.add_geom(geom)
             node.set_color((0.75, 0.75, 0.85, 1.0))
             if show_meshes:
                 render_attrib = node.get_state().get_attrib_def(
@@ -1018,24 +982,20 @@ def show_floor(self, show: bool) -> None:
             self._floor.hide()
 
     def update_floor(self,
-                     heightmap: Optional[np.ndarray] = None,
+                     geom: Optional[Geom] = None,
                      show_meshes: bool = False) -> NodePath:
         """Update the floor.
 
-        :param heightmap: Height map of the ground, as a 2D nd.array of shape
-                          [N_X * N_Y, 6], where N_X, N_Y are the number of
-                          vertices on x and y axes respectively, while the
-                          last dimension corresponds to the position (x, y, z)
-                          and normal (n_x, n_y, nz) of the vertex in space. It
-                          renders a flat tile ground if not specified.
-                          Optional: None by default.
+        :param geom: Ground profile as a generic geometry object. If None, then
+                     a flat tile ground is rendered.
+                     Optional: None by default.
         """
         # Check if floor is currently hidden
         is_hidden = self._floor.isHidden()
 
         # Remove existing floor and create a new one
         self._floor.remove_node()
-        self._floor = self._make_floor(heightmap, show_meshes)
+        self._floor = self._make_floor(geom, show_meshes)
 
         # Hide the floor if is was previously hidden
         if is_hidden:
@@ -2017,6 +1977,50 @@ def __dir__(self) -> Iterable[str]:
         return chain(super().__dir__(), dir(self._app))
 
 
+def convert_bvh_collision_geometry_to_primitive(geom: hppfcl.CollisionGeometry
+                                                ) -> Optional[Geom]:
+    """Convert a triangle-based collision geometry associated to a primitive
+    geometry for rendering it with Panda3D.
+
+    :param geom: Collision geometry to convert.
+    """
+    # Extract vertices and faces from geometry
+    vertices, faces = extract_vertices_and_faces_from_geometry(geom)
+
+    # Return immediately if there is nothing to load
+    if len(faces) == 0:
+        return None
+
+    # Define normal to vertices as the average normal of adjacent triangles
+    fnormals = np.cross(vertices[faces[:, 2]] - vertices[faces[:, 1]],
+                        vertices[faces[:, 0]] - vertices[faces[:, 1]])
+    fnormals /= np.linalg.norm(fnormals, axis=0)
+    normals = np.zeros((len(vertices), 3), dtype=np.float32)
+    for i in range(3):
+        normals[faces[:, i]] += fnormals
+    normals /= np.linalg.norm(normals, axis=0)
+
+    # Create primitive triangle geometry
+    vformat = GeomVertexFormat()
+    vformat.addArray(GeomVertexArrayFormat(
+        "vertex", 3, Geom.NT_float32, Geom.C_point))
+    vformat.addArray(GeomVertexArrayFormat(
+        "normal", 3, Geom.NT_float32, Geom.C_normal))
+    vformat = GeomVertexFormat.registerFormat(vformat)
+    vdata = GeomVertexData('vdata', vformat, Geom.UHStatic)
+    vdata.modify_array_handle(0).copy_data_from(vertices)
+    vdata.modify_array_handle(1).copy_data_from(normals)
+    prim = GeomTriangles(Geom.UHStatic)
+    prim.set_index_type(GeomEnums.NTUint32)
+    prim.modify_vertices(-1).modify_handle().copy_data_from(faces)
+
+    # Create geometry object
+    geom = Geom(vdata)
+    geom.add_primitive(prim)
+
+    return geom
+
+
 class Panda3dVisualizer(BaseVisualizer):
     """A Pinocchio display using panda3d engine.
 
@@ -2024,7 +2028,7 @@ class Panda3dVisualizer(BaseVisualizer):
     Copyright (c) 2014-2020, CNRS
     Copyright (c) 2018-2020, INRIA
     """  # noqa: E501  # pylint: disable=line-too-long
-    def initViewer(self,  # pylint: disable=arguments-differ,unused-argument
+    def initViewer(self,  # pylint: disable=arguments-differ
                    viewer: Optional[Union[Panda3dViewer, Panda3dApp]] = None,
                    loadModel: bool = False,
                    **kwargs: Any) -> None:
@@ -2085,7 +2089,6 @@ def loadViewerGeometryObject(self,
 
         # Try to load mesh from path first, to take advantage of very effective
         # Panda3d model caching procedure.
-        is_success = True
         if os.path.exists(mesh_path):
             # append a mesh
             mesh_path = _sanitize_path(geometry_object.meshPath)
@@ -2110,66 +2113,23 @@ def loadViewerGeometryObject(self,
                 self.viewer.append_box(*node_name, 2.0 * geom.halfSide)
             elif isinstance(geom, hppfcl.Sphere):
                 self.viewer.append_sphere(*node_name, geom.radius)
-            elif isinstance(geom, (hppfcl.Convex, hppfcl.BVHModelBase)):
-                # Extract vertices and faces from geometry
-                if isinstance(geom, hppfcl.Convex):
-                    vertices = geom.points()
-                    num_faces, get_faces = geom.num_polygons, geom.polygons
-                else:
-                    vertices = geom.vertices()
-                    num_faces, get_faces = geom.num_tris, geom.tri_indices
-                faces = np.empty((num_faces, 3), dtype=np.int32)
-                for i in range(num_faces):
-                    tri = get_faces(i)
-                    for j in range(3):
-                        faces[i, j] = tri[j]
-
-                # Return immediately if there is nothing to load
-                if num_faces == 0:
+            else:
+                # Create primitive triangle geometry
+                try:
+                    obj = convert_bvh_collision_geometry_to_primitive(geom)
+                except ValueError:
+                    warnings.warn(
+                        "Unsupported geometry type for "
+                        f"{geometry_object.name} ({type(geom)})",
+                        category=UserWarning, stacklevel=2)
                     return
 
-                # Create primitive triangle geometry.
-                # Note that the redundant vertices are added for efficiency
-                # because avoiding duplication requires expensive search.
-                vformat = GeomVertexFormat.get_v3n3()
-                vdata = GeomVertexData('vdata', vformat, Geom.UHStatic)
-                vdata.unclean_set_num_rows(3 * len(faces))
-                vwriter = GeomVertexWriter(vdata, 'vertex')
-                nwriter = GeomVertexWriter(vdata, "normal")
-                normals = np.empty((len(faces), 3))
-                for i, face in enumerate(faces):
-                    pts = vertices[face]
-                    v1, v2 = pts[1] - pts[0], pts[2] - pts[1]
-                    normals[i] = np.cross(v1, v2)
-                    for pt in pts:
-                        vwriter.add_data3(*pt)
-                normals /= np.linalg.norm(normals, axis=0)
-                for normal in normals:
-                    for i in range(3):
-                        nwriter.add_data3(*normal)
-                prim = GeomTriangles(Geom.UHStatic)
-                prim.reserve_num_vertices(len(faces))
-                faces.flat[:] = np.arange(faces.size)
-                for face in faces:
-                    prim.addVertices(*face)
-                obj = Geom(vdata)
-                obj.addPrimitive(prim)
-
                 # Add the primitive geometry to the scene
                 geom_node = GeomNode(geometry_object.name)
                 geom_node.add_geom(obj)
                 node = NodePath(geom_node)
                 node.set_two_sided(True)
                 self.viewer.append_node(*node_name, node)
-            else:
-                is_success = False
-
-        # Early return if impossible to load the geometry for some reason
-        if not is_success:
-            warnings.warn(
-                f"Unsupported geometry type for {geometry_object.name} "
-                f"({type(geom)})", category=UserWarning, stacklevel=2)
-            return
 
         # Set material
         self.viewer.set_material(*node_name, color, texture_path)
diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py
index ebdacef95..5147660fc 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/replay.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py
@@ -1,7 +1,6 @@
 # mypy: disable-error-code="attr-defined, name-defined"
 """ TODO: Write documentation.
 """
-# pylint: disable=no-member
 import os
 import time
 import ctypes
@@ -144,7 +143,7 @@ def _with_lock(fun: Callable[..., Any]) -> Callable[..., Any]:
     @wraps(fun)
     def fun_safe(*args: Any, **kwargs: Any) -> Any:
         with viewer_lock:
-            return fun(*args, **kwargs)  # pylint: disable=not-callable
+            return fun(*args, **kwargs)
     return fun_safe
 
 
@@ -307,7 +306,6 @@ def play_trajectories(
 
     :returns: List of viewers used to play the trajectories.
     """
-    # pylint: disable=used-before-assignment
     # Make sure sequence arguments are list or tuple
     if isinstance(trajs_data, dict):
         trajs_data = [trajs_data]
diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py
index 9fe38e13e..501d79424 100644
--- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py
+++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py
@@ -33,6 +33,7 @@
 except ImportError:
     Process = type(None)  # type: ignore[assignment,misc]
 
+import hppfcl
 import pinocchio as pin
 from pinocchio import SE3, SE3ToXYZQUAT
 from pinocchio.rpy import (  # pylint: disable=import-error
@@ -47,7 +48,7 @@
 from .meshcat.utilities import interactive_mode
 from .panda3d.panda3d_visualizer import (
     Tuple3FType, Tuple4FType, ShapeType, Panda3dApp, Panda3dViewer,
-    Panda3dVisualizer)
+    Panda3dVisualizer, convert_bvh_collision_geometry_to_primitive)
 
 
 REPLAY_FRAMERATE = 30
@@ -275,7 +276,7 @@ def fun_safe(*args: Any, **kwargs: Any) -> Any:
             raise RuntimeError(
                 "No backend available. Please start one before calling "
                 f"'{fun.__name__}'.")
-        return fun(*args, **kwargs)  # pylint: disable=not-callable
+        return fun(*args, **kwargs)
     return fun_safe
 
 
@@ -287,7 +288,7 @@ def fun_safe(*args: Any, **kwargs: Any) -> Any:
             self = args[0]
         self = kwargs.get('self', self)
         with self._lock:
-            return fun(*args, **kwargs)  # pylint: disable=not-callable
+            return fun(*args, **kwargs)
     return fun_safe
 
 
@@ -1783,15 +1784,14 @@ def set_color(self,
     @_with_lock
     @_must_be_open
     def update_floor(ground_profile: Optional[jiminy.HeightmapFunctor] = None,
-                     grid_size: float = 20.0,
-                     grid_unit: float = 0.04,
+                     x_range: Tuple[float, float] = (-10.0, 10.0),
+                     y_range: Tuple[float, float] = (-10.0, 10.0),
+                     grid_unit:  Tuple[float, float] = (0.04, 0.04),
+                     simplify_meshes: bool = False,
                      show_meshes: bool = False) -> None:
         """Display a custom ground profile as a height map or the original tile
         ground floor.
 
-        .. note::
-            This method is only supported by Panda3d for now.
-
         :param ground_profile: `jiminy_py.core.HeightmapFunctor` associated
                                with the ground profile. It renders a flat tile
                                ground if not specified.
@@ -1803,30 +1803,37 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunctor] = None,
         :param show_meshes: Whether to highlight the meshes.
                             Optional: disabled by default.
         """
+        # pylint: disable=import-outside-toplevel
+
         # Assert(s) for type checker
         assert Viewer.backend is not None
         assert Viewer._backend_obj is not None
 
-        # Return early if this method is not supported by the current backend
-        if not Viewer.backend.startswith('panda3d'):
-            LOGGER.warning("This method is only supported by Panda3d.")
-            return
-
-        # Restore tile ground if heightmap is not specified
+        # Discretize ground profile if provided
+        geom = None
         if ground_profile is None:
-            Viewer._backend_obj.gui.update_floor()
-            return
-
-        # Discretize heightmap
-        grid = discretize_heightmap(ground_profile, grid_size, grid_unit)
-
-        # Make sure it is not flat ground
-        if np.unique(grid[:, 2:], axis=0).shape[0] == 1 and \
-                np.allclose(grid[0, 2:], [0.0, 0.0, 0.0, 1.0], atol=1e-3):
-            Viewer._backend_obj.gui.update_floor()
-            return
+            geom = discretize_heightmap(
+                ground_profile, *x_range, grid_unit[0], *y_range, grid_unit[1],
+                must_simplify=simplify_meshes)
+
+        # Render original flat tile ground if possible.
+        # TODO: Improve this check using LocalAABB box geometry instead.
+        if isinstance(geom, hppfcl.Halfspace):
+            if abs(geom.d) > 1e-6:
+                raise RuntimeError(
+                    "Rendering flat ground with non-zero height not supported")
+            geom = None
 
-        Viewer._backend_obj.gui.update_floor(grid, show_meshes)
+        # Render ground geometry
+        if Viewer.backend.startswith('panda3d'):
+            obj = None
+            if geom is not None:
+                obj = convert_bvh_collision_geometry_to_primitive(geom)
+            Viewer._backend_obj.gui.update_floor(obj, show_meshes)
+        else:
+            from .meshcat.meshcat_visualizer import (
+                update_floor as meshcat_update_floor)
+            meshcat_update_floor(Viewer._backend_obj.gui, geom)
 
     @staticmethod
     @_with_lock
diff --git a/python/jiminy_pywrap/include/jiminy/python/functors.h b/python/jiminy_pywrap/include/jiminy/python/functors.h
index 983cf0821..274574ea7 100644
--- a/python/jiminy_pywrap/include/jiminy/python/functors.h
+++ b/python/jiminy_pywrap/include/jiminy/python/functors.h
@@ -54,13 +54,13 @@ namespace jiminy::python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_v<T>, bp::handle<>> FctPyWrapperArgToPython(T & arg)
+    std::enable_if_t<is_eigen_object_v<T>, bp::handle<>> FctPyWrapperArgToPython(T & arg)
     {
         return bp::handle<>(getNumpyReference(arg));
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_v<T>, bp::handle<>> FctPyWrapperArgToPython(const T & arg)
+    std::enable_if_t<is_eigen_object_v<T>, bp::handle<>> FctPyWrapperArgToPython(const T & arg)
     {
         return bp::handle<>(getNumpyReference(arg));
     }
@@ -212,133 +212,34 @@ namespace jiminy::python
 
     struct HeightmapFunctorPyWrapper
     {
-    public:
-        // Disable copy-assignment
-        HeightmapFunctorPyWrapper & operator=(const HeightmapFunctorPyWrapper & other) = delete;
-
-    public:
         HeightmapFunctorPyWrapper(const bp::object & objPy, heightmapType_t objType) :
         heightmapType_{objType},
-        handlePyPtr_{objPy},
-        out1Ptr_(new double),
-        out2Ptr_(new Eigen::Vector3d),
-        out1PyPtr_{},
-        out2PyPtr_{}
-        {
-            if (heightmapType_ == heightmapType_t::CONSTANT)
-            {
-                *out1Ptr_ = bp::extract<double>(handlePyPtr_);
-                *out2Ptr_ = Eigen::Vector3d::UnitZ();
-            }
-            else if (heightmapType_ == heightmapType_t::STAIRS)
-            {
-                out1PyPtr_ = getNumpyReference(*out1Ptr_);
-                *out2Ptr_ = Eigen::Vector3d::UnitZ();
-            }
-            else if (heightmapType_ == heightmapType_t::GENERIC)
-            {
-                out1PyPtr_ = getNumpyReference(*out1Ptr_);
-                out2PyPtr_ = getNumpyReference(*out2Ptr_);
-            }
-        }
-
-        // Copy constructor, same as the normal constructor
-        HeightmapFunctorPyWrapper(const HeightmapFunctorPyWrapper & other) :
-        heightmapType_(other.heightmapType_),
-        handlePyPtr_(other.handlePyPtr_),
-        out1Ptr_(new double),
-        out2Ptr_(new Eigen::Vector3d),
-        out1PyPtr_{},
-        out2PyPtr_{}
-        {
-            *out1Ptr_ = *(other.out1Ptr_);
-            *out2Ptr_ = *(other.out2Ptr_);
-            out1PyPtr_ = getNumpyReference(*out1Ptr_);
-            out2PyPtr_ = getNumpyReference(*out2Ptr_);
-        }
-
-        // Move constructor, takes a rvalue reference &&
-        HeightmapFunctorPyWrapper(HeightmapFunctorPyWrapper && other) :
-        heightmapType_(other.heightmapType_),
-        handlePyPtr_(other.handlePyPtr_),
-        out1Ptr_{nullptr},
-        out2Ptr_{nullptr},
-        out1PyPtr_{nullptr},
-        out2PyPtr_{nullptr}
-        {
-            // Steal the resource from "other"
-            out1Ptr_ = other.out1Ptr_;
-            out2Ptr_ = other.out2Ptr_;
-            out1PyPtr_ = other.out1PyPtr_;
-            out2PyPtr_ = other.out2PyPtr_;
-
-            /* "other" will soon be destroyed and its destructor will do nothing because we null
-               out its resource here. */
-            other.out1Ptr_ = nullptr;
-            other.out2Ptr_ = nullptr;
-            other.out1PyPtr_ = nullptr;
-            other.out2PyPtr_ = nullptr;
-        }
-
-        // Destructor
-        ~HeightmapFunctorPyWrapper()
+        handlePyPtr_{objPy}
         {
-            Py_XDECREF(out1PyPtr_);
-            Py_XDECREF(out2PyPtr_);
-            delete out1Ptr_;
-            delete out2Ptr_;
         }
 
-        // Move assignment, takes a rvalue reference &&
-        HeightmapFunctorPyWrapper & operator=(HeightmapFunctorPyWrapper && other)
+        void operator()(
+            const Eigen::Vector2d & posFrame, double & height, Eigen::Vector3d & normal)
         {
-            /* "other" is soon going to be destroyed, so we let it destroy our current resource
-               instead and we take "other"'s current resource via swapping. */
-            std::swap(heightmapType_, other.heightmapType_);
-            std::swap(handlePyPtr_, other.handlePyPtr_);
-            std::swap(out1Ptr_, other.out1Ptr_);
-            std::swap(out2Ptr_, other.out2Ptr_);
-            std::swap(out1PyPtr_, other.out1PyPtr_);
-            std::swap(out2PyPtr_, other.out2PyPtr_);
-            return *this;
-        }
-
-        std::pair<double, Eigen::Vector3d> operator()(const Eigen::Vector3d & posFrame)
-        {
-            if (heightmapType_ == heightmapType_t::STAIRS)
-            {
-                *out1Ptr_ = qNAN;
-                bp::handle<> out1Py(bp::borrowed(out1PyPtr_));
-                handlePyPtr_(posFrame[0], posFrame[1], out1Py);
-            }
-            else if (heightmapType_ == heightmapType_t::GENERIC)
-            {
-                *out1Ptr_ = qNAN;
-                out2Ptr_->setConstant(qNAN);
-                bp::handle<> out1Py(bp::borrowed(out1PyPtr_));
-                bp::handle<> out2Py(bp::borrowed(out2PyPtr_));
-                handlePyPtr_(posFrame[0], posFrame[1], out1Py, out2Py);
-            }
-            if (std::isnan(*out1Ptr_))
+            switch (heightmapType_)
             {
-                throw std::runtime_error("Heightmap height output not set.");
+            case heightmapType_t::CONSTANT:
+                height = bp::extract<double>(handlePyPtr_);
+                normal = Eigen::Vector3d::UnitZ();
+                break;
+            case heightmapType_t::STAIRS:
+                handlePyPtr_(posFrame, convertToPython(height, false));
+                normal = Eigen::Vector3d::UnitZ();
+                break;
+            case heightmapType_t::GENERIC:
+            default:
+                handlePyPtr_(
+                    posFrame, convertToPython(height, false), convertToPython(normal, false));
             }
-            if ((out2Ptr_->array() != out2Ptr_->array()).any())
-            {
-                throw std::runtime_error("Heightmap normal output not set.");
-            }
-            return {*out1Ptr_, *out2Ptr_};
         }
 
-    public:
         heightmapType_t heightmapType_;
         bp::object handlePyPtr_;
-
-    private:
-        double * out1Ptr_;
-        Eigen::Vector3d * out2Ptr_;
-        PyObject * out1PyPtr_;
-        PyObject * out2PyPtr_;
     };
 
     // **************************** HeightmapFunctorVisitor *****************************
diff --git a/python/jiminy_pywrap/include/jiminy/python/utilities.h b/python/jiminy_pywrap/include/jiminy/python/utilities.h
index 0f740df92..ffeb30729 100644
--- a/python/jiminy_pywrap/include/jiminy/python/utilities.h
+++ b/python/jiminy_pywrap/include/jiminy/python/utilities.h
@@ -19,24 +19,25 @@
 #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
 
 
-namespace boost::python::converter
-{
-#define EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(type)   \
-    template<>                                   \
-    struct expected_pytype_for_arg<type>         \
-    {                                            \
-        static const PyTypeObject * get_pytype() \
-        {                                        \
-            return &PyArray_Type;                \
-        }                                        \
-    };
+#define EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(type)       \
+    namespace boost::python::converter               \
+    {                                                \
+        template<>                                   \
+        struct expected_pytype_for_arg<type>         \
+        {                                            \
+            static const PyTypeObject * get_pytype() \
+            {                                        \
+                return &PyArray_Type;                \
+            }                                        \
+        };                                           \
+    }
 
-    EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(numpy::ndarray)
-    EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(const numpy::ndarray)
-    EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(numpy::ndarray &)
-    EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(const numpy::ndarray &)
-}
+EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(numpy::ndarray)
+EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(const numpy::ndarray)
+EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(numpy::ndarray &)
+EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY(const numpy::ndarray &)
 
+#undef EXPECTED_PYTYPE_FOR_ARG_IS_ARRAY
 
 namespace jiminy::python
 {
@@ -71,10 +72,25 @@ namespace jiminy::python
         Py##class##Visitor::expose();      \
     }
 
+    template<typename T>
+    struct get_signature_impl;
+
     template<typename R, typename... Args>
-    boost::mpl::vector<R, Args...> functionToMLP(std::function<R(Args...)> /* func */)
+    struct get_signature_impl<std::function<R(Args...)>>
     {
-        return {};
+        static constexpr const boost::mpl::vector<R, Args...> value{};
+    };
+
+    template<typename T>
+    constexpr auto getSignature(T && fun)
+    {
+        return get_signature_impl<decltype(std::function{fun})>::value;
+    }
+
+    template<class F, class CallPolicies, class Keywords>
+    bp::object makeFunction(F f, const CallPolicies & policies, const Keywords & keywords)
+    {
+        return bp::make_function(f, policies, keywords, getSignature(f));
     }
 
     namespace detail
@@ -518,7 +534,7 @@ namespace jiminy::python
 
     /// \brief Generic converter from Eigen Matrix to Numpy array by reference.
     template<typename T>
-    std::enable_if_t<!is_eigen_v<T>, PyObject *> getNumpyReference(T & value)
+    std::enable_if_t<!is_eigen_object_v<T>, PyObject *> getNumpyReference(T & value)
     {
         return getNumpyReferenceFromScalar(value);
     }
@@ -530,15 +546,14 @@ namespace jiminy::python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_v<T> && !is_eigen_vector_v<T>, PyObject *>
+    std::enable_if_t<is_eigen_object_v<T> && !is_eigen_vector_v<T>, PyObject *>
     getNumpyReference(T & value)
     {
         return getNumpyReferenceFromEigenMatrix(value);
     }
 
     template<typename T>
-    std::optional<
-        Eigen::Map<Eigen::Matrix<T, -1, -1>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>>
+    std::optional<Eigen::Map<MatrixX<T>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>>
     getEigenReferenceImpl(PyArrayObject * dataPyArray)
     {
         // Check array dtype
@@ -591,9 +606,8 @@ namespace jiminy::python
 
     /// \brief Generic converter from Numpy array to Eigen Matrix by reference.
     inline std::optional<std::variant<
-        Eigen::Map<Eigen::Matrix<double, -1, -1>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>,
-        Eigen::
-            Map<Eigen::Matrix<int64_t, -1, -1>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>>>
+        Eigen::Map<MatrixX<double>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>,
+        Eigen::Map<MatrixX<int64_t>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>>>>
     getEigenReference(PyObject * dataPy)
     {
         // Check if raw Python object pointer is actually a numpy array
@@ -626,7 +640,7 @@ namespace jiminy::python
     /// Convert most C++ objects into Python objects by value
     template<typename T>
     std::enable_if_t<
-        !is_vector_v<T> && !is_array_v<T> && !is_eigen_v<T> &&
+        !is_vector_v<T> && !is_array_v<T> && !is_eigen_any_v<T> &&
             !std::is_arithmetic_v<std::decay_t<T>> &&
             !std::is_same_v<std::decay_t<T>, GenericConfig> &&
             !std::is_same_v<std::decay_t<T>, std::pair<const std::string, SensorDataTypeMap>> &&
@@ -655,17 +669,25 @@ namespace jiminy::python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_v<T>, bp::object> convertToPython(T && data,
-                                                                const bool & copy = true)
+    std::enable_if_t<is_eigen_any_v<T>, bp::object> convertToPython(T && data,
+                                                                    const bool & copy = true)
     {
-        PyObject * vecPyPtr = getNumpyReference(data);
-        if (copy)
+        if constexpr (!is_eigen_object_v<T>)
+        {
+            assert(copy && "Impossible to convert Eigen expression to python without copy.");
+            return convertToPython(data.eval(), true);
+        }
+        else
         {
-            PyObject * copyVecPyPtr = PyArray_FROM_OF(vecPyPtr, NPY_ARRAY_ENSURECOPY);
-            bp::decref(vecPyPtr);
-            vecPyPtr = copyVecPyPtr;
+            PyObject * matPyPtr = getNumpyReference(data);
+            if (copy)
+            {
+                PyObject * copyMatPyPtr = PyArray_FROM_OF(matPyPtr, NPY_ARRAY_ENSURECOPY);
+                bp::decref(matPyPtr);
+                matPyPtr = copyMatPyPtr;
+            }
+            return bp::object(bp::handle<>(matPyPtr));
         }
-        return bp::object(bp::handle<>(vecPyPtr));
     }
 
     template<typename T>
@@ -804,41 +826,42 @@ namespace jiminy::python
     // ****************************************************************************
 
     /// \brief Convert a 1D python list into an Eigen vector by value.
-    inline Eigen::VectorXd listPyToEigenVector(const bp::list & listPy)
+    template<typename Scalar>
+    inline VectorX<Scalar> listPyToEigenVector(const bp::list & listPy)
     {
-        Eigen::VectorXd x(len(listPy));
+        VectorX<Scalar> x(len(listPy));
         for (bp::ssize_t i = 0; i < len(listPy); ++i)
         {
-            x[i] = bp::extract<double>(listPy[i]);
+            x[i] = bp::extract<Scalar>(listPy[i]);
         }
-
         return x;
     }
 
     /// \brief Convert a 2D python list into an Eigen matrix.
-    inline Eigen::MatrixXd listPyToEigenMatrix(const bp::list & listPy)
+    template<typename Scalar>
+    inline MatrixX<Scalar> listPyToEigenMatrix(const bp::list & listPy)
     {
         const bp::ssize_t nRows = len(listPy);
-        assert(nRows > 0 && "empty list");
-
+        if (nRows == 0)
+        {
+            return {};
+        }
         const bp::ssize_t nCols = len(bp::extract<bp::list>(listPy[0]));
-        assert(nCols > 0 && "empty row");
 
-        Eigen::MatrixXd M(nRows, nCols);
+        MatrixX<Scalar> M(nRows, nCols);
         for (bp::ssize_t i = 0; i < nRows; ++i)
         {
-            bp::list row = bp::extract<bp::list>(listPy[i]);  // Beware elements are not copied.
-            assert(len(row) == nCols && "wrong number of columns");
-            M.row(i) = listPyToEigenVector(row);
+            bp::list row = bp::extract<bp::list>(listPy[i]);
+            assert(len(row) == nCols && "Inconsistent number of columns");
+            M.row(i) = listPyToEigenVector<Scalar>(row);
         }
-
         return M;
     }
 
     // Convert most Python objects in C++ objects by value.
 
     template<typename T>
-    std::enable_if_t<!is_vector_v<T> && !is_array_v<T> && !is_map_v<T> && !is_eigen_v<T> &&
+    std::enable_if_t<!is_vector_v<T> && !is_array_v<T> && !is_map_v<T> && !is_eigen_any_v<T> &&
                          !std::is_same_v<T, SensorsDataMap>,
                      T>
     convertFromPython(const bp::object & dataPy)
@@ -899,7 +922,7 @@ namespace jiminy::python
     }
 
     template<typename T>
-    std::enable_if_t<is_eigen_v<T>, T> convertFromPython(const bp::object & dataPy)
+    std::enable_if_t<is_eigen_any_v<T>, T> convertFromPython(const bp::object & dataPy)
     {
         using Scalar = typename T::Scalar;
 
@@ -911,23 +934,20 @@ namespace jiminy::python
                 throw std::runtime_error(
                     "Scalar type of eigen object does not match dtype of numpy object.");
             }
-            Scalar * dataPtr = reinterpret_cast<Scalar *>(dataNumpy.get_data());
-            const Py_intptr_t * dataShape = dataNumpy.get_shape();
-            if (is_eigen_vector_v<T>)
-            {
-                return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, 1>>(dataPtr, dataShape[0]);
-            }
-            return Eigen::Map<Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic>>(
-                dataPtr, dataShape[0], dataShape[1]);
+            return getEigenReferenceImpl<Scalar>(reinterpret_cast<PyArrayObject *>(dataPy.ptr()))
+                .value();
         }
         catch (const bp::error_already_set &)
         {
             PyErr_Clear();
-            if (is_eigen_vector_v<T>)
+            if constexpr (is_eigen_vector_v<T>)
+            {
+                return listPyToEigenVector<Scalar>(bp::extract<bp::list>(dataPy));
+            }
+            else
             {
-                return listPyToEigenVector(bp::extract<bp::list>(dataPy));
+                return listPyToEigenMatrix<Scalar>(bp::extract<bp::list>(dataPy));
             }
-            return listPyToEigenMatrix(bp::extract<bp::list>(dataPy));
         }
     }
 
@@ -943,20 +963,37 @@ namespace jiminy::python
         return flexData;
     }
 
+
     template<typename T>
     std::enable_if_t<is_vector_v<T>, T> convertFromPython(const bp::object & dataPy)
     {
-        const bp::list listPy = bp::extract<bp::list>(dataPy);
         T vec;
-        vec.reserve(bp::len(listPy));
-        for (bp::ssize_t i = 0; i < bp::len(listPy); ++i)
+
+        auto converter = [&vec](auto seqPy)
+        {
+            vec.reserve(bp::len(seqPy));
+            for (bp::ssize_t i = 0; i < bp::len(seqPy); ++i)
+            {
+                vec.push_back(convertFromPython<typename T::value_type>(seqPy[i]));
+            }
+        };
+
+        bp::extract<bp::list> dataPyListExtract{dataPy};
+        if (dataPyListExtract.check())
         {
-            vec.push_back(convertFromPython<typename T::value_type>(listPy[i]));
+            bp::list listPy = dataPyListExtract();
+            converter(listPy);
         }
+        else
+        {
+            bp::tuple tuplePy = bp::extract<bp::tuple>(dataPy);
+            converter(tuplePy);
+        }
+
         return vec;
     }
 
-    namespace details
+    namespace internal
     {
         template<typename T, size_t... Is, typename F>
         std::array<T, sizeof...(Is)> BuildArrayFromCallable(std::index_sequence<Is...>, F fun)
@@ -974,7 +1011,7 @@ namespace jiminy::python
         {
             throw std::runtime_error("Consistent number of elements");
         }
-        return details::BuildArrayFromCallable<typename T::value_type>(
+        return internal::BuildArrayFromCallable<typename T::value_type>(
             std::make_index_sequence<N>{},
             [&listPy](std::size_t i)
             { return convertFromPython<typename T::value_type>(listPy[i]); });
diff --git a/python/jiminy_pywrap/src/compatibility.cc b/python/jiminy_pywrap/src/compatibility.cc
index 0ee160347..ccb7fe85e 100644
--- a/python/jiminy_pywrap/src/compatibility.cc
+++ b/python/jiminy_pywrap/src/compatibility.cc
@@ -21,13 +21,10 @@ namespace jiminy::python
             {
                 return obj;
             }
-            else
+            np::dtype dt(bp::detail::borrowed_reference(obj->ob_type));
+            if (equivalent(dt, np::dtype::get_builtin<T>()))
             {
-                np::dtype dt(bp::detail::borrowed_reference(obj->ob_type));
-                if (equivalent(dt, np::dtype::get_builtin<T>()))
-                {
-                    return obj;
-                }
+                return obj;
             }
             return 0;
         }
@@ -107,10 +104,12 @@ namespace jiminy::python
     void exposeCompatibility()
     {
         /* Add some automatic C++ to Python converters for numpy array of scalars, which is
-           different from a 0-dimensional numpy array. */
+           different from a 0-dimensional numpy array. See:
+           https://github.com/boostorg/python/blob/master/src/numpy/dtype.cpp */
         arrayScalarFromPython<bool>::declare();
         arrayScalarFromPython<npy_uint8>::declare();
         arrayScalarFromPython<npy_uint32>::declare();
+        arrayScalarFromPython<uint64_t>::declare();
 #ifdef _MSC_VER
         arrayScalarFromPython<boost::uint32_t>::declare();
 #endif
diff --git a/python/jiminy_pywrap/src/controllers.cc b/python/jiminy_pywrap/src/controllers.cc
index 374e9cec0..19656de14 100644
--- a/python/jiminy_pywrap/src/controllers.cc
+++ b/python/jiminy_pywrap/src/controllers.cc
@@ -158,12 +158,11 @@ namespace jiminy::python
             }
         }
 
-        template<typename T>
+        template<typename Scalar>
         static hresult_t registerVariableArrayImpl(
             AbstractController & self,
             const bp::list & fieldnamesPy,
-            Eigen::Map<Eigen::Matrix<T, -1, -1>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
-                data)
+            Eigen::Map<MatrixX<Scalar>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> & data)
         {
             hresult_t returnCode = hresult_t::SUCCESS;
 
diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc
index 98bfa95ea..4038e8306 100644
--- a/python/jiminy_pywrap/src/engine.cc
+++ b/python/jiminy_pywrap/src/engine.cc
@@ -24,20 +24,18 @@ namespace jiminy::python
 
     static bp::object forceProfileWrapper(const ForceProfile & self)
     {
-        bp::object func = bp::make_function(self.forceFct,
-                                            bp::return_value_policy<bp::return_by_value>(),
-                                            (bp::arg("t"), "q", "v"),
-                                            functionToMLP(self.forceFct));
+        bp::object func = makeFunction(self.forceFct,
+                                       bp::return_value_policy<bp::return_by_value>(),
+                                       (bp::arg("t"), "q", "v"));
         setFunctionWrapperModule<ForceProfile>(func);
         return func;
     }
 
     static bp::object forceCouplingWrapper(const ForceCoupling & self)
     {
-        bp::object func = bp::make_function(self.forceFct,
-                                            bp::return_value_policy<bp::return_by_value>(),
-                                            (bp::arg("t"), "q_1", "v_1", "q_2", "v_2"),
-                                            functionToMLP(self.forceFct));
+        bp::object func = makeFunction(self.forceFct,
+                                       bp::return_value_policy<bp::return_by_value>(),
+                                       (bp::arg("t"), "q_1", "v_1", "q_2", "v_2"));
         setFunctionWrapperModule<ForceCoupling>(func);
         return func;
     }
@@ -609,8 +607,8 @@ namespace jiminy::python
             bp::dict variables, constants;
 
             // Temporary contiguous storage for variables
-            Eigen::Matrix<int64_t, Eigen::Dynamic, 1> intVector;
-            Eigen::Matrix<double, Eigen::Dynamic, 1> floatVector;
+            VectorX<int64_t> intVector;
+            VectorX<double> floatVector;
 
             // Get the number of integer and float variables
             const Eigen::Index numInt = logData.integerValues.rows();
diff --git a/python/jiminy_pywrap/src/functors.cc b/python/jiminy_pywrap/src/functors.cc
index 4dfac928d..066b0c7d5 100644
--- a/python/jiminy_pywrap/src/functors.cc
+++ b/python/jiminy_pywrap/src/functors.cc
@@ -46,10 +46,12 @@ namespace jiminy::python
             // clang-format on
         }
 
-        static bp::tuple eval(HeightmapFunctor & self, const Eigen::Vector3d & posFrame)
+        static bp::tuple eval(HeightmapFunctor & self, const Eigen::Vector2d & position)
         {
-            const std::pair<double, Eigen::Vector3d> ground = self(posFrame);
-            return bp::make_tuple(std::get<double>(ground), std::get<Eigen::Vector3d>(ground));
+            double height;
+            Eigen::Vector3d normal;
+            self(position, height, normal);
+            return bp::make_tuple(height, normal);
         }
 
         static bp::object getPyFun(HeightmapFunctor & self)
diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc
index f1e2d1097..c165ef32e 100644
--- a/python/jiminy_pywrap/src/generators.cc
+++ b/python/jiminy_pywrap/src/generators.cc
@@ -1,4 +1,7 @@
+#include "hpp/fcl/BVH/BVH_model.h"  // `hpp::fcl::BVHModel`, `hpp::fcl::OBBRSS`
+
 #include "jiminy/core/utilities/random.h"
+#include "jiminy/core/utilities/geometry.h"
 
 #include "jiminy/python/utilities.h"
 #include "jiminy/python/generators.h"
@@ -8,109 +11,283 @@ namespace jiminy::python
 {
     namespace bp = boost::python;
 
-    HeightmapFunctor sumHeightmap(const bp::list & heightmapsPy)
+    HeightmapFunctor sumHeightmaps(const bp::list & heightmapsPy)
     {
         auto heightmaps = convertFromPython<std::vector<HeightmapFunctor>>(heightmapsPy);
-        return ::jiminy::sumHeightmap(heightmaps);
+        return ::jiminy::sumHeightmaps(heightmaps);
     }
 
-    HeightmapFunctor mergeHeightmap(const bp::list & heightmapsPy)
+    HeightmapFunctor mergeHeightmaps(const bp::list & heightmapsPy)
     {
         auto heightmaps = convertFromPython<std::vector<HeightmapFunctor>>(heightmapsPy);
-        return ::jiminy::mergeHeightmap(heightmaps);
+        return ::jiminy::mergeHeightmaps(heightmaps);
     }
 
-    void resetRandomGenerators(const bp::object & seedPy)
+    std::shared_ptr<PCG32> makePCG32FromSeedSed(bp::list & seedSeqPy)
     {
-        std::optional<uint32_t> seed = std::nullopt;
-        if (!seedPy.is_none())
+        std::vector<int> seedSeq = bp::extract<std::vector<int>>(seedSeqPy);
+        return std::make_shared<PCG32>(std::seed_seq(seedSeq.cbegin(), seedSeq.cend()));
+    }
+
+    void seedPCG32FromSeedSed(PCG32 & generator, bp::list & seedSeqPy)
+    {
+        std::vector<int> seedSeq = bp::extract<std::vector<int>>(seedSeqPy);
+        return generator.seed(std::seed_seq(seedSeq.cbegin(), seedSeq.cend()));
+    }
+
+
+#define GENERIC_DISTRIBUTION_WRAPPER(dist, arg1, arg2)                                            \
+    Eigen::MatrixXd dist##FromStackedArgs(                                                        \
+        const uniform_random_bit_generator_ref<uint32_t> & generator,                             \
+        np::ndarray & arg1,                                                                       \
+        np::ndarray & arg2)                                                                       \
+    {                                                                                             \
+        /* Extract Eigen::MatriXf from generic numpy arrays  */                                   \
+        auto cast = [](np::ndarray & array) -> Eigen::Ref<const MatrixX<float>>                   \
+        {                                                                                         \
+            if (array.get_dtype() == np::dtype::get_builtin<float>())                             \
+            {                                                                                     \
+                return convertFromPython<Eigen::Ref<const MatrixX<float>>>(array);                \
+            }                                                                                     \
+            if (array.get_dtype() == np::dtype::get_builtin<double>())                            \
+            {                                                                                     \
+                return convertFromPython<Eigen::Ref<const MatrixX<double>>>(array).cast<float>(); \
+            }                                                                                     \
+            throw std::invalid_argument(                                                          \
+                "Matrix arguments must have dtype 'np.float32' or 'np.float64'.");                \
+        };                                                                                        \
+        return dist(generator, cast(arg1), cast(arg2)).cast<double>();                            \
+    }                                                                                             \
+                                                                                                  \
+    bp::object dist##FromSize(const uniform_random_bit_generator_ref<uint32_t> & generator,       \
+                              float arg1,                                                         \
+                              float arg2,                                                         \
+                              bp::object & sizePy)                                                \
+    {                                                                                             \
+        /* Extract nrows, ncols from optional tuple with at most 2 elements  */                   \
+        if (sizePy.is_none())                                                                     \
+        {                                                                                         \
+            return convertToPython(dist(generator, arg1, arg2), true);                            \
+        }                                                                                         \
+        auto size = convertFromPython<std::vector<int>>(sizePy);                                  \
+        int nrows = 1;                                                                            \
+        int ncols = 1;                                                                            \
+        switch (size.size())                                                                      \
+        {                                                                                         \
+        case 2:                                                                                   \
+            ncols = size[1];                                                                      \
+            [[fallthrough]];                                                                      \
+        case 1:                                                                                   \
+            nrows = size[0];                                                                      \
+            [[fallthrough]];                                                                      \
+        case 0:                                                                                   \
+            break;                                                                                \
+        default:                                                                                  \
+            throw std::invalid_argument("'size' must have at most 2 dimensions.");                \
+        }                                                                                         \
+        return convertToPython(dist(nrows, ncols, generator, arg1, arg2).cast<double>(), true);   \
+    }
+
+    GENERIC_DISTRIBUTION_WRAPPER(uniform, lo, hi)
+    GENERIC_DISTRIBUTION_WRAPPER(normal, mean, stddev)
+
+#undef GENERIC_DISTRIBUTION_WRAPPER
+
+    template<typename R, typename F, typename... Args>
+    R convertGeneratorToPythonAndInvoke(F callable, bp::object generatorPy, Args &&... args)
+    {
+        // First, check if the provided generator can be implicitly converted
+        bp::extract<uniform_random_bit_generator_ref<uint32_t>> generatorPyGetter(generatorPy);
+        if (generatorPyGetter.check())
         {
-            seed = bp::extract<uint32_t>(seedPy);
+            return callable(generatorPyGetter(), std::forward<Args>(args)...);
         }
-        ::jiminy::resetRandomGenerators(seed);
+
+        // If not, assuming it is a numpy random generator and try to extract the raw function
+        bp::object ctypes_addressof = bp::import("ctypes").attr("addressof");
+        bp::object next_uint32_ctype = generatorPy.attr("ctypes").attr("next_uint32");
+        uintptr_t next_uint32_addr = bp::extract<uintptr_t>(ctypes_addressof(next_uint32_ctype));
+        auto next_uint32 = *reinterpret_cast<uint32_t (**)(void *)>(next_uint32_addr);
+        bp::object state_ctype = generatorPy.attr("ctypes").attr("state_address");
+        void * state_ptr = reinterpret_cast<void *>(bp::extract<uintptr_t>(state_ctype)());
+
+        return callable([state_ptr, next_uint32]() -> uint32_t { return next_uint32(state_ptr); },
+                        std::forward<Args>(args)...);
     }
 
+    template<typename Signature, typename>
+    class ConvertGeneratorToPythonAndInvoke;
+
+    template<typename R, typename Generator, typename... Args>
+    class ConvertGeneratorToPythonAndInvoke<R(Generator, Args...), void>
+    {
+    public:
+        ConvertGeneratorToPythonAndInvoke(R (*fun)(Generator, Args...)) :
+        fun_{fun}
+        {
+        }
+
+        R operator()(bp::object generatorPy, Args... argsPy)
+        {
+            return convertGeneratorToPythonAndInvoke<R, R (*)(Generator, Args...), Args...>(
+                fun_, generatorPy, std::forward<Args>(argsPy)...);
+        }
+
+    private:
+        R (*fun_)(Generator, Args...);
+    };
+
+    template<typename R, typename... Args>
+    ConvertGeneratorToPythonAndInvoke(R (*)(Args...))
+        -> ConvertGeneratorToPythonAndInvoke<R(Args...), void>;
+
+    template<typename T, typename R, typename Generator, typename... Args>
+    class ConvertGeneratorToPythonAndInvoke<R(Generator, Args...), T>
+    {
+    public:
+        ConvertGeneratorToPythonAndInvoke(R (T::*memFun)(Generator, Args...)) :
+        memFun_{memFun}
+        {
+        }
+
+        R operator()(T & obj, bp::object generatorPy, Args... argsPy)
+        {
+            auto callable = [&obj, memFun = memFun_](Generator generator, Args... args) -> R
+            {
+                return (obj.*memFun)(generator, args...);
+            };
+            return convertGeneratorToPythonAndInvoke<R, decltype(callable), Args...>(
+                callable, generatorPy, std::forward<Args>(argsPy)...);
+        }
+
+    private:
+        R (T::*memFun_)(Generator, Args...);
+    };
+
+    template<typename T, typename R, typename... Args>
+    ConvertGeneratorToPythonAndInvoke(R (T::*)(Args...))
+        -> ConvertGeneratorToPythonAndInvoke<R(Args...), T>;
+
     void exposeGenerators()
     {
         // clang-format off
-        bp::def("reset_random_generator", &resetRandomGenerators, (bp::arg("seed") = bp::object()));
+        bp::class_<PCG32,
+                   std::shared_ptr<PCG32>,
+                   boost::noncopyable>("PCG32",
+                   bp::init<uint64_t>((bp::arg("self"), "state")))
+            .def(bp::init<>((bp::arg("self"))))
+            .def("__init__", bp::make_constructor(&makePCG32FromSeedSed,
+                             bp::default_call_policies(),
+                             (bp::arg("seed_seq"))))
+            .def("__call__", &PCG32::operator(), bp::args("self"))
+            .def("seed", &seedPCG32FromSeedSed, (bp::arg("self"), "seed_seq"))
+            .add_static_property(
+                "min", &PCG32::min, getPropertySignaturesWithDoc(nullptr, &PCG32::min).c_str())
+            .add_static_property(
+                "max", &PCG32::max, getPropertySignaturesWithDoc(nullptr, &PCG32::max).c_str());
 
-        bp::class_<AbstractPerlinProcess,
-                   std::shared_ptr<AbstractPerlinProcess>,
-                   boost::noncopyable>("AbstractPerlinProcess", bp::no_init)
-            .def("__call__", &AbstractPerlinProcess::operator(),
-                             (bp::arg("self"), bp::arg("time")))
-            .def("reset", &AbstractPerlinProcess::reset)
-            .ADD_PROPERTY_GET_WITH_POLICY("wavelength",
-                                          &AbstractPerlinProcess::getWavelength,
-                                          bp::return_value_policy<bp::return_by_value>())
-            .ADD_PROPERTY_GET_WITH_POLICY("num_octaves",
-                                          &AbstractPerlinProcess::getNumOctaves,
-                                          bp::return_value_policy<bp::return_by_value>())
-            .ADD_PROPERTY_GET_WITH_POLICY("scale",
-                                          &AbstractPerlinProcess::getScale,
-                                          bp::return_value_policy<bp::return_by_value>());
+        bp::implicitly_convertible<PCG32, uniform_random_bit_generator_ref<uint32_t>>();
 
-        bp::class_<RandomPerlinProcess, bp::bases<AbstractPerlinProcess>,
-                   std::shared_ptr<RandomPerlinProcess>,
-                   boost::noncopyable>("RandomPerlinProcess",
-                   bp::init<double, double, uint32_t>(
-                   (bp::arg("self"), "wavelength", bp::arg("scale") = 1.0, bp::arg("num_octaves") = 6U)));
+#define BIND_GENERIC_DISTRIBUTION(dist, arg1, arg2)                                          \
+        bp::def(#dist, makeFunction(                                                         \
+            ConvertGeneratorToPythonAndInvoke(&dist##FromStackedArgs),                       \
+            bp::default_call_policies(),                                                     \
+            (bp::arg("generator"), #arg1, #arg2)));                                          \
+        bp::def(#dist, makeFunction(                                                         \
+            ConvertGeneratorToPythonAndInvoke(&dist##FromSize),                              \
+            bp::default_call_policies(),                                                     \
+            (bp::arg("generator"), bp::arg(#arg1) = 0.0F, bp::arg(#arg2) = 1.0F,             \
+             bp::arg("size") = bp::object())));
 
-        bp::class_<PeriodicPerlinProcess, bp::bases<AbstractPerlinProcess>,
-                   std::shared_ptr<PeriodicPerlinProcess>,
-                   boost::noncopyable>("PeriodicPerlinProcess",
-                   bp::init<double, double, double, uint32_t>(
-                   (bp::arg("self"), "wavelength", "period", bp::arg("scale") = 1.0, bp::arg("num_octaves") = 6U)))
-            .ADD_PROPERTY_GET_WITH_POLICY("period",
-                                          &PeriodicPerlinProcess::getPeriod,
-                                          bp::return_value_policy<bp::return_by_value>());
+    BIND_GENERIC_DISTRIBUTION(uniform, lo, hi)
+    BIND_GENERIC_DISTRIBUTION(normal, mean, stddev)
+
+#undef BIND_GENERIC_DISTRIBUTION
+
+        // Must be declared last to take precedence over generic declaration with default values
+        bp::def("uniform", makeFunction(ConvertGeneratorToPythonAndInvoke(
+            static_cast<
+                float (*)(const uniform_random_bit_generator_ref<uint32_t> &)
+            >(&uniform)),
+            bp::default_call_policies(),
+            (bp::arg("generator"))));
 
         bp::class_<PeriodicGaussianProcess,
                    std::shared_ptr<PeriodicGaussianProcess>,
                    boost::noncopyable>("PeriodicGaussianProcess",
-                   bp::init<double, double, double>(
-                   (bp::arg("self"), "wavelength", "period", bp::arg("scale") = 1.0)))
+                   bp::init<double, double>(
+                   (bp::arg("self"), "wavelength", "period")))
             .def("__call__", &PeriodicGaussianProcess::operator(),
                              (bp::arg("self"), bp::arg("time")))
-            .def("reset", &PeriodicGaussianProcess::reset)
+            .def("reset", makeFunction(
+                          ConvertGeneratorToPythonAndInvoke(&PeriodicGaussianProcess::reset),
+                          bp::default_call_policies(),
+                          (bp::arg("self"), "generator")))
             .ADD_PROPERTY_GET_WITH_POLICY("wavelength",
-                                          &PeriodicGaussianProcess::getWavelength,
+                                          &PeriodicGaussianProcess::wavelength,
                                           bp::return_value_policy<bp::return_by_value>())
             .ADD_PROPERTY_GET_WITH_POLICY("period",
-                                          &PeriodicGaussianProcess::getPeriod,
-                                          bp::return_value_policy<bp::return_by_value>())
-            .ADD_PROPERTY_GET_WITH_POLICY("dt",
-                                          &PeriodicGaussianProcess::getDt,
+                                          &PeriodicGaussianProcess::period,
                                           bp::return_value_policy<bp::return_by_value>());
 
         bp::class_<PeriodicFourierProcess,
                    std::shared_ptr<PeriodicFourierProcess>,
                    boost::noncopyable>("PeriodicFourierProcess",
-                   bp::init<double, double, double>(
-                   (bp::arg("self"), "wavelength", "period", bp::arg("scale") = 1.0)))
+                   bp::init<double, double>(
+                   (bp::arg("self"), "wavelength", "period")))
             .def("__call__", &PeriodicFourierProcess::operator(),
                              (bp::arg("self"), bp::arg("time")))
-            .def("reset", &PeriodicFourierProcess::reset)
+            .def("reset", makeFunction(
+                          ConvertGeneratorToPythonAndInvoke(&PeriodicFourierProcess::reset),
+                          bp::default_call_policies(),
+                          (bp::arg("self"), "generator")))
             .ADD_PROPERTY_GET_WITH_POLICY("wavelength",
-                                          &PeriodicFourierProcess::getWavelength,
+                                          &PeriodicFourierProcess::wavelength,
                                           bp::return_value_policy<bp::return_by_value>())
             .ADD_PROPERTY_GET_WITH_POLICY("period",
-                                          &PeriodicFourierProcess::getPeriod,
-                                          bp::return_value_policy<bp::return_by_value>())
-            .ADD_PROPERTY_GET_WITH_POLICY("dt",
-                                          &PeriodicFourierProcess::getDt,
+                                          &PeriodicFourierProcess::period,
+                                          bp::return_value_policy<bp::return_by_value>());
+
+        bp::class_<AbstractPerlinProcess,
+                   std::shared_ptr<AbstractPerlinProcess>,
+                   boost::noncopyable>("AbstractPerlinProcess", bp::no_init)
+            .def("__call__", &AbstractPerlinProcess::operator(),
+                             (bp::arg("self"), "time"))
+            .def("reset", makeFunction(
+                          ConvertGeneratorToPythonAndInvoke(&AbstractPerlinProcess::reset),
+                          bp::default_call_policies(),
+                          (bp::arg("self"), "generator")))
+            .ADD_PROPERTY_GET_WITH_POLICY("wavelength",
+                                          &AbstractPerlinProcess::wavelength,
                                           bp::return_value_policy<bp::return_by_value>())
-            .ADD_PROPERTY_GET_WITH_POLICY("num_harmonics",
-                                          &PeriodicFourierProcess::getNumHarmonics,
+            .ADD_PROPERTY_GET_WITH_POLICY("num_octaves",
+                                          &AbstractPerlinProcess::num_octaves,
+                                          bp::return_value_policy<bp::return_by_value>());
+
+        bp::class_<RandomPerlinProcess, bp::bases<AbstractPerlinProcess>,
+                   std::shared_ptr<RandomPerlinProcess>,
+                   boost::noncopyable>("RandomPerlinProcess",
+                   bp::init<double, uint32_t>(
+                   (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U)));
+
+        bp::class_<PeriodicPerlinProcess, bp::bases<AbstractPerlinProcess>,
+                   std::shared_ptr<PeriodicPerlinProcess>,
+                   boost::noncopyable>("PeriodicPerlinProcess",
+                   bp::init<double, double, uint32_t>(
+                   (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U)))
+            .ADD_PROPERTY_GET_WITH_POLICY("period",
+                                          &PeriodicPerlinProcess::period,
                                           bp::return_value_policy<bp::return_by_value>());
 
-        bp::def("random_tile_ground", &randomTileGround,
-                                      (bp::arg("size"), "height_max", "interp_delta", "sparsity", "orientation", "seed"));
-        bp::def("sum_heightmap", &sumHeightmap, (bp::arg("heightmaps")));
-        bp::def("merge_heightmap", &mergeHeightmap, (bp::arg("heightmaps")));
+        bp::def("random_tile_ground", &tiles,
+                                      (bp::arg("size"), "height_max", "interp_delta",
+                                       "sparsity", "orientation", "seed"));
+        bp::def("sum_heightmaps", &sumHeightmaps, (bp::arg("heightmaps")));
+        bp::def("merge_heightmaps", &mergeHeightmaps, (bp::arg("heightmaps")));
 
-        bp::def("discretize_heightmap", &discretizeHeightmap, (bp::arg("heightmap"), "grid_size", "grid_unit"));
+        bp::def("discretize_heightmap", &discretizeHeightmap,
+                                        (bp::arg("heightmap"), "x_min", "x_max", "x_unit", "y_min",
+                                         "y_max", "y_unit", bp::arg("must_simplify") = false));
         // clang-format on
     }
 }
diff --git a/python/jiminy_pywrap/src/helpers.cc b/python/jiminy_pywrap/src/helpers.cc
index 23152aa80..0234ad5c2 100644
--- a/python/jiminy_pywrap/src/helpers.cc
+++ b/python/jiminy_pywrap/src/helpers.cc
@@ -19,14 +19,6 @@ namespace jiminy::python
 {
     namespace bp = boost::python;
 
-    uint32_t getRandomSeed()
-    {
-        uint32_t seed;
-        // Cannot fail since random number generators are initialized at shared lib import
-        ::jiminy::getRandomSeed(seed);
-        return seed;
-    }
-
     JointModelType getJointTypeFromIdx(const pinocchio::Model & model, std::size_t jointIdx)
     {
         JointModelType jointType = JointModelType::UNSUPPORTED;
@@ -111,6 +103,7 @@ namespace jiminy::python
     np::ndarray solveJMinvJtv(
         pinocchio::Data & data, const np::ndarray & vPy, bool updateDecomposition)
     {
+        bp::object objPy;
         const int32_t nDims = vPy.get_nd();
         assert(nDims < 3 && "The number of dimensions of 'v' cannot exceed 2.");
         if (nDims == 1)
@@ -118,15 +111,16 @@ namespace jiminy::python
             const Eigen::VectorXd v = convertFromPython<Eigen::VectorXd>(vPy);
             const Eigen::VectorXd x =
                 pinocchio_overload::solveJMinvJtv<Eigen::VectorXd>(data, v, updateDecomposition);
-            return bp::extract<np::ndarray>(convertToPython(x, true));
+            objPy = convertToPython(x, true);
         }
         else
         {
             const Eigen::MatrixXd v = convertFromPython<Eigen::MatrixXd>(vPy);
             const Eigen::MatrixXd x =
                 pinocchio_overload::solveJMinvJtv<Eigen::MatrixXd>(data, v, updateDecomposition);
-            return bp::extract<np::ndarray>(convertToPython(x, true));
+            objPy = convertToPython(x, true);
         }
+        return bp::extract<np::ndarray>(objPy);
     }
 
     void arrayCopyTo(PyObject * dstPy, PyObject * srcPy)
@@ -214,8 +208,8 @@ namespace jiminy::python
                 // Copy scalar bytes to destination if available
                 if (isSuccess)
                 {
-                    Eigen::Map<Eigen::Matrix<double, Eigen::Dynamic, 1>> dst(
-                        reinterpret_cast<double *>(dstPyData), PyArray_SIZE(dstPyArray));
+                    Eigen::Map<VectorX<double>> dst(reinterpret_cast<double *>(dstPyData),
+                                                    PyArray_SIZE(dstPyArray));
                     dst.setConstant(srcPyScalar);
                     return;
                 }
@@ -317,8 +311,6 @@ namespace jiminy::python
     void exposeHelpers()
     {
         // clang-format off
-        bp::def("get_random_seed", bp::make_function(&getRandomSeed,
-                                   bp::return_value_policy<bp::return_by_value>()));
         bp::def("build_geom_from_urdf", &buildGeomFromUrdf,
                                         (bp::arg("pinocchio_model"), "urdf_filename", "geom_type",
                                          bp::arg("mesh_package_dirs") = bp::list(),
diff --git a/python/jiminy_pywrap/src/module.cc b/python/jiminy_pywrap/src/module.cc
index 340e84d22..032a1c2a4 100755
--- a/python/jiminy_pywrap/src/module.cc
+++ b/python/jiminy_pywrap/src/module.cc
@@ -52,9 +52,6 @@ namespace jiminy::python
 
     BOOST_PYTHON_MODULE(PYTHON_LIBRARY_NAME)
     {
-        // Initialize Jiminy random number generator
-        resetRandomGenerators(0U);
-
         /* Initialized boost::python::numpy.
            It is required to handle boost::python::numpy::ndarray object directly.
 


Test patch:
diff --git a/core/unit/model_test.cc b/core/unit/model_test.cc
index 2d550d635..0da460fa2 100644
--- a/core/unit/model_test.cc
+++ b/core/unit/model_test.cc
@@ -60,7 +60,6 @@ TEST_P(ModelTestFixture, CreateFlexible)
     GenericConfig & dynamicsOptions = boost::get<GenericConfig>(options.at("dynamics"));
     boost::get<FlexibilityConfig>(dynamicsOptions.at("flexibilityConfig")) = flexConfig;
     model->setOptions(options);
-    model->reset();
 
     ASSERT_TRUE(model->getFlexibleConfigurationFromRigid(q, qflex) == hresult_t::SUCCESS);
     ASSERT_EQ(qflex.size(),
diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py
index 197a9f629..4af4a01be 100644
--- a/python/jiminy_py/unit_py/test_simple_pendulum.py
+++ b/python/jiminy_py/unit_py/test_simple_pendulum.py
@@ -201,9 +201,7 @@ def dynamics(t: float, x: np.ndarray) -> np.ndarray:
         x_rk_python = integrate_dynamics(time, x0, dynamics)
 
         # Compute sensor acceleration, i.e. acceleration in polar coordinates
-        theta = x_rk_python[:, 0]
-        dtheta = x_rk_python[:, 1]
-        dtheta = x_rk_python[:, 1]
+        theta, dtheta = x_rk_python[:, 0], x_rk_python[:, 1]
 
         # Acceleration: to resolve algebraic loop (current acceleration is
         # function of input which itself is function of sensor signal, sensor
@@ -222,9 +220,9 @@ def dynamics(t: float, x: np.ndarray) -> np.ndarray:
         # Compare sensor signal, ignoring first iterations that correspond to
         # system initialization
         self.assertTrue(np.allclose(
-            expected_gyro[2:, :], gyro_jiminy[2:, :], atol=TOLERANCE))
+            expected_gyro, gyro_jiminy, atol=TOLERANCE))
         self.assertTrue(np.allclose(
-            expected_accel[2:, :], accel_jiminy[2:, :], atol=TOLERANCE))
+            expected_accel, accel_jiminy, atol=TOLERANCE))
 
     def test_sensor_delay(self):
         """Test sensor delay for an IMU sensor on a simple pendulum.
@@ -255,11 +253,11 @@ def test_sensor_delay(self):
         imu_jiminy_shifted_0 = interp1d(
             time, imu_jiminy, kind='zero',
             bounds_error=False, fill_value=imu_jiminy[0], axis=0
-        )(time - 1.0e-2)
+        )(time + 1e-10 - 1.0e-2)
         imu_jiminy_shifted_1 = interp1d(
             time, imu_jiminy,
             kind='linear', bounds_error=False, fill_value=imu_jiminy[0], axis=0
-        )(time - 1.0e-2)
+        )(time - 5.0e-3)
 
         # Configure the IMU
         imu_options = self.imu_sensor.get_options()
@@ -275,7 +273,7 @@ def test_sensor_delay(self):
         # Configure the IMU
         imu_options = self.imu_sensor.get_options()
         imu_options['delayInterpolationOrder'] = 1
-        imu_options['delay'] = 1.0e-2
+        imu_options['delay'] = 5.0e-3
         self.imu_sensor.set_options(imu_options)
 
         # Run simulation
@@ -284,10 +282,8 @@ def test_sensor_delay(self):
                 engine, tf, x0, split=False)
 
         # Compare sensor signals
-        self.assertLessEqual(
-            np.mean(imu_jiminy_delayed_0 - imu_jiminy_shifted_0), 1.0e-5)
-        self.assertTrue(np.allclose(
-            imu_jiminy_delayed_1, imu_jiminy_shifted_1, atol=TOLERANCE))
+        np.testing.assert_allclose(imu_jiminy_delayed_0, imu_jiminy_shifted_0)
+        np.testing.assert_allclose(imu_jiminy_delayed_1, imu_jiminy_shifted_1)
 
     def test_sensor_noise_bias(self):
         """Test sensor noise and bias for an IMU sensor on a simple pendulum