From 6f70c7d52d41a15dc2adf3db915076c76a91ae96 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 8 Jul 2024 15:57:41 +0200 Subject: [PATCH 1/8] [core] Minor improvement periodic perlin process and periodic stair ground. --- core/include/jiminy/core/utilities/geometry.h | 2 +- core/include/jiminy/core/utilities/random.h | 15 +- core/src/utilities/geometry.cc | 20 ++- core/src/utilities/random.cc | 41 ++--- python/jiminy_pywrap/src/generators.cc | 148 ++++++++++-------- 5 files changed, 117 insertions(+), 109 deletions(-) diff --git a/core/include/jiminy/core/utilities/geometry.h b/core/include/jiminy/core/utilities/geometry.h index 75ef89ca5..310a34528 100644 --- a/core/include/jiminy/core/utilities/geometry.h +++ b/core/include/jiminy/core/utilities/geometry.h @@ -46,7 +46,7 @@ namespace jiminy /// \param[in] stepHeight Heigh of the steps. /// \param[in] stepNumber Number of steps in the ascending or descending direction. /// \param[in] orientation Orientation of the staircases in the XY plane. - HeightmapFunction JIMINY_DLLAPI stairs( + HeightmapFunction JIMINY_DLLAPI periodicStairs( double stepWidth, double stepHeight, uint32_t stepNumber, double orientation); } diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index d2f2642a6..6beca29a0 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -220,9 +220,9 @@ namespace jiminy template std::enable_if_t< - (is_eigen_any_v || - is_eigen_any_v)&&(!std::is_arithmetic_v> || - !std::is_arithmetic_v>), + (is_eigen_any_v || is_eigen_any_v) && + (!std::is_arithmetic_v> || + !std::is_arithmetic_v>), Eigen::CwiseNullaryOp< scalar_random_op &, float, float), Generator &, @@ -271,9 +271,9 @@ namespace jiminy /// optimizations enabled (level 01 is enough), probably due to inlining. template std::enable_if_t< - (is_eigen_any_v || - is_eigen_any_v)&&(!std::is_arithmetic_v> || - !std::is_arithmetic_v>), + (is_eigen_any_v || is_eigen_any_v) && + (!std::is_arithmetic_v> || + !std::is_arithmetic_v>), Eigen::CwiseNullaryOp< scalar_random_op &, float, float), Generator &, @@ -461,7 +461,8 @@ namespace jiminy private: const double period_; - std::array perm_{}; + std::vector hashes_ = + std::vector(static_cast(period_ / wavelength_)); }; /// \brief Sum of Perlin noise octaves. diff --git a/core/src/utilities/geometry.cc b/core/src/utilities/geometry.cc index 3cf456151..fee7bd87c 100644 --- a/core/src/utilities/geometry.cc +++ b/core/src/utilities/geometry.cc @@ -752,20 +752,23 @@ namespace jiminy }; } - HeightmapFunction stairs( + HeightmapFunction periodicStairs( double stepWidth, double stepHeight, uint32_t stepNumber, double orientation) { const double interpDelta = 0.01; - const Eigen::Rotation2D rot_mat(orientation); - return [stepWidth, stepHeight, stepNumber, rot_mat, interpDelta]( + // Define the projection axis + const Eigen::Vector2d axis{std::cos(orientation), std::sin(orientation)}; + + return [stepWidth, stepHeight, stepNumber, axis, interpDelta]( const Eigen::Vector2d & pos, double & height, Eigen::Ref normal) -> void { // Compute position in stairs reference frame - Eigen::Vector2d posRel = rot_mat.inverse() * pos; - const double modPos = std::fmod(std::abs(posRel[0]), stepWidth * stepNumber * 2); + // Eigen::Vector2d posRel = rotMat.inverse() * pos; + const double posRel = axis.dot(pos); + const double modPos = std::fmod(std::abs(posRel), stepWidth * stepNumber * 2); // Compute the default height and normal uint32_t stairIndex = static_cast(modPos / stepWidth); @@ -779,7 +782,8 @@ namespace jiminy normal = Eigen::Vector3d::UnitZ(); // Avoid unsupported vertical edge - const double posRelOnStep = std::fmod(modPos, stepWidth) / stepWidth; + const double posRelOnStep = + std::fmod(modPos + std::numeric_limits::epsilon(), stepWidth) / stepWidth; if (1.0 - posRelOnStep < interpDelta) { const double slope = staircaseSlopeSign * stepHeight / interpDelta; @@ -793,9 +797,9 @@ namespace jiminy // step 1. compute normal in stairs reference frame: // normal << -slope * normInv, 0.0, normInv; // step 2. Rotate normal vector in world plane reference frame: - // normal.head<2>() = rot_mat * normal.head<2>(); + // normal.head<2>() = rotMat * normal.head<2>(); // Or simply in a single operation: - normal << -slope * normInv * rot_mat.toRotationMatrix().col(0), normInv; + normal << -slope * normInv * axis, normInv; } }; } diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index 9f434aa60..bb4dcd05e 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -432,23 +432,16 @@ namespace jiminy return 2.0 * grad * delta; } - namespace internal - { - template - 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 period is larger than the wavelength + if (period_ < wavelength_) + { + JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); + } + // Make sure the wavelength is multiple of the period if (std::abs(std::round(period / wavelength) * wavelength - period) > std::numeric_limits::epsilon()) @@ -456,8 +449,8 @@ namespace jiminy JIMINY_THROW(std::invalid_argument, "'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_); + // Initialize the pre-computed hash table + std::generate(hashes_.begin(), hashes_.end(), std::random_device{}); } void PeriodicPerlinNoiseOctave::reset( @@ -466,8 +459,8 @@ namespace jiminy // Call base implementation AbstractPerlinNoiseOctave::reset(g); - // Re-Initialize the permutation vector with values from 0 to 255 - internal::randomizePermutationVector(g, perm_); + // Re-initialize the pre-computed hash table + std::generate(hashes_.begin(), hashes_.end(), g); } double PeriodicPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept @@ -476,7 +469,8 @@ namespace jiminy knot %= static_cast(period_ / wavelength_); // Convert to double in [0.0, 1.0) - const double s = perm_[knot] / 256.0; + const double s = static_cast(hashes_[knot]) / + static_cast(std::numeric_limits::max()); // Compute rescaled gradient between [-1.0, 1.0) const double grad = 2.0 * s - 1.0; @@ -574,11 +568,6 @@ namespace jiminy { return std::make_unique(wavelengthIn, period); })), period_{period} { - // Make sure the period is larger than the wavelength - if (period_ < wavelength) - { - JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); - } } double PeriodicPerlinProcess::getPeriod() const noexcept @@ -675,15 +664,15 @@ namespace jiminy uniformSparseFromState(Vector1::Constant(i), 1, seed); }); - const Eigen::Rotation2D rot_mat(orientation); + const Eigen::Rotation2D rotMat(orientation); - return [size, heightMax, interpDelta, rot_mat, sparsity, interpThr, offset, seed]( + return [size, heightMax, interpDelta, rotMat, sparsity, interpThr, offset, seed]( const Eigen::Vector2d & pos, double & height, Eigen::Ref normal) -> void { // Compute the tile index and relative coordinate - Eigen::Vector2d posRel = (rot_mat * (pos + offset)).array() / size.array(); + Eigen::Vector2d posRel = (rotMat * (pos + offset)).array() / size.array(); Vector2 posIndices = posRel.array().floor().cast(); posRel -= posIndices.cast(); diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 725846a55..6d8bcd7c4 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -95,15 +95,12 @@ namespace jiminy::python void exposeGenerators() { - // clang-format off - bp::class_, - boost::noncopyable>("PCG32", - bp::init((bp::arg("self"), "state"))) + bp::class_, boost::noncopyable>( + "PCG32", bp::init((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("__init__", + bp::make_constructor( + &makePCG32FromSeedSed, bp::default_call_policies(), (bp::arg("seed_seq")))) .def("__call__", &PCG32::operator(), (bp::arg("self"))) .def("seed", &seedPCG32FromSeedSed, (bp::arg("self"), "seed_seq")) .add_static_property( @@ -113,93 +110,110 @@ namespace jiminy::python bp::implicitly_convertible>(); -#define BIND_GENERIC_DISTRIBUTION(dist, arg1, arg2) \ - bp::def(#dist, makeFunction( \ - ConvertGeneratorFromPythonAndInvoke(&dist##FromStackedArgs), \ - bp::default_call_policies(), \ - (bp::arg("generator"), #arg1, #arg2))); \ - bp::def(#dist, makeFunction( \ - ConvertGeneratorFromPythonAndInvoke(&dist##FromSize), \ - bp::default_call_policies(), \ - (bp::arg("generator"), bp::arg(#arg1) = 0.0F, bp::arg(#arg2) = 1.0F, \ - bp::arg("size") = bp::object()))); - - BIND_GENERIC_DISTRIBUTION(uniform, lo, hi) - BIND_GENERIC_DISTRIBUTION(normal, mean, stddev) +#define BIND_GENERIC_DISTRIBUTION(dist, arg1, arg2) \ + bp::def(#dist, \ + makeFunction(ConvertGeneratorFromPythonAndInvoke(&dist##FromStackedArgs), \ + bp::default_call_policies(), \ + (bp::arg("generator"), #arg1, #arg2))); \ + bp::def(#dist, \ + makeFunction(ConvertGeneratorFromPythonAndInvoke(&dist##FromSize), \ + bp::default_call_policies(), \ + (bp::arg("generator"), \ + bp::arg(#arg1) = 0.0F, \ + bp::arg(#arg2) = 1.0F, \ + bp::arg("size") = bp::object()))); + + 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(ConvertGeneratorFromPythonAndInvoke( - static_cast< - float (*)(const uniform_random_bit_generator_ref &) - >(&uniform)), - bp::default_call_policies(), - (bp::arg("generator")))); + bp::def("uniform", + makeFunction( + ConvertGeneratorFromPythonAndInvoke( + static_cast &)>( + &uniform)), + bp::default_call_policies(), + (bp::arg("generator")))); bp::class_, - boost::noncopyable>("PeriodicGaussianProcess", - bp::init( - (bp::arg("self"), "wavelength", "period"))) - .def("__call__", &PeriodicGaussianProcess::operator(), - (bp::arg("self"), bp::arg("time"))) - .def("reset", makeFunction( - ConvertGeneratorFromPythonAndInvoke(&PeriodicGaussianProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) + boost::noncopyable>( + "PeriodicGaussianProcess", + bp::init((bp::arg("self"), "wavelength", "period"))) + .def("__call__", + &PeriodicGaussianProcess::operator(), + (bp::arg("self"), bp::arg("time"))) + .def("reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicGaussianProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) .ADD_PROPERTY_GET("wavelength", &PeriodicGaussianProcess::getWavelength) .ADD_PROPERTY_GET("period", &PeriodicGaussianProcess::getPeriod); bp::class_, - boost::noncopyable>("PeriodicFourierProcess", - bp::init( - (bp::arg("self"), "wavelength", "period"))) - .def("__call__", &PeriodicFourierProcess::operator(), - (bp::arg("self"), bp::arg("time"))) - .def("reset", makeFunction( - ConvertGeneratorFromPythonAndInvoke(&PeriodicFourierProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) + boost::noncopyable>( + "PeriodicFourierProcess", + bp::init((bp::arg("self"), "wavelength", "period"))) + .def("__call__", + &PeriodicFourierProcess::operator(), + (bp::arg("self"), bp::arg("time"))) + .def("reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicFourierProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) .ADD_PROPERTY_GET("wavelength", &PeriodicFourierProcess::getWavelength) .ADD_PROPERTY_GET("period", &PeriodicFourierProcess::getPeriod); bp::class_, boost::noncopyable>("AbstractPerlinProcess", bp::no_init) - .def("__call__", &AbstractPerlinProcess::operator(), - (bp::arg("self"), "time")) - .def("reset", makeFunction( - ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) + .def("__call__", &AbstractPerlinProcess::operator(), (bp::arg("self"), "time")) + .def("reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) .ADD_PROPERTY_GET("wavelength", &AbstractPerlinProcess::getWavelength) .ADD_PROPERTY_GET("num_octaves", &AbstractPerlinProcess::getNumOctaves); - bp::class_, + bp::class_, std::shared_ptr, - boost::noncopyable>("RandomPerlinProcess", - bp::init( - (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))); + boost::noncopyable>( + "RandomPerlinProcess", + bp::init( + (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))); - bp::class_, + bp::class_, std::shared_ptr, - boost::noncopyable>("PeriodicPerlinProcess", - bp::init( - (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) + boost::noncopyable>( + "PeriodicPerlinProcess", + bp::init( + (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) .ADD_PROPERTY_GET("period", &PeriodicPerlinProcess::getPeriod); - bp::def("random_tile_ground", &tiles, - (bp::arg("size"), "height_max", "interp_delta", - "sparsity", "orientation", "seed")); - bp::def("stairs_ground", &stairs, (bp::arg("step_width"), "step_height", "step_number", "orientation")); + bp::def( + "random_tile_ground", + &tiles, + (bp::arg("size"), "height_max", "interp_delta", "sparsity", "orientation", "seed")); + bp::def("periodic_stairs_ground", + &periodicStairs, + (bp::arg("step_width"), "step_height", "step_number", "orientation")); bp::def("sum_heightmaps", &sumHeightmaps, (bp::arg("heightmaps"))); bp::def("merge_heightmaps", &mergeHeightmaps, (bp::arg("heightmaps"))); - 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 + 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)); } } From 4eecbb819649372f7fb9c6cde476cf4b7cdb9d38 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Wed, 10 Jul 2024 10:23:18 +0200 Subject: [PATCH 2/8] [core] 'PeriodicGaussianProcess' and 'PeriodicFourierProcess' are now differentiable. Fix negative time support. --- core/include/jiminy/core/utilities/random.h | 54 +++-- core/include/jiminy/core/utilities/random.hxx | 15 +- core/src/utilities/random.cc | 207 +++++++++++++----- python/jiminy_pywrap/src/generators.cc | 10 +- 4 files changed, 200 insertions(+), 86 deletions(-) diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 6beca29a0..cde5b2be9 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -314,7 +314,7 @@ namespace jiminy /// \param[in] coeffs First row of the matrix to decompose. template MatrixX - standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs); + standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs, double reg = 0.0); } class JIMINY_DLLAPI PeriodicGaussianProcess @@ -327,7 +327,8 @@ namespace jiminy void reset(const uniform_random_bit_generator_ref & g) noexcept; - double operator()(float t); + double operator()(double t); + double gradient(double t); double getWavelength() const noexcept; double getPeriod() const noexcept; @@ -336,7 +337,7 @@ namespace jiminy const double wavelength_; const double period_; - const double dt_{0.02 * wavelength_}; + const double dt_{0.1 * wavelength_}; const Eigen::Index numTimes_{static_cast(std::ceil(period_ / dt_))}; /// \brief Cholesky decomposition (LLT) of the covariance matrix. @@ -347,20 +348,42 @@ namespace jiminy /// 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 covSqrtRoot_{ - internal::standardToeplitzCholeskyLower(Eigen::VectorXd::NullaryExpr( + /// Ultimately, the algorithmic complexity can be reduced from O(n^3) to O(n^2), + /// which is lower than the matrix multiplication itself. + /// + Eigen::MatrixXd covSqrtRoot_{internal::standardToeplitzCholeskyLower( + Eigen::VectorXd::NullaryExpr( numTimes_, [numTimes = static_cast(numTimes_), wavelength = wavelength_](double i) { return std::exp(-2.0 * std::pow(std::sin(M_PI / numTimes * i) / wavelength, 2)); - }))}; + }), + 1e-9)}; + Eigen::MatrixXd covJacobian_{Eigen::MatrixXd::NullaryExpr( + numTimes_, + numTimes_, + [numTimes = static_cast(numTimes_), + wavelength = wavelength_, + period = period_](double i, double j) + { + return -2 * M_PI / period / std::pow(wavelength, 2) * + std::sin(2 * M_PI / numTimes * (i - j)) * + std::exp(-2.0 * + std::pow(std::sin(M_PI / numTimes * (i - j)) / wavelength, 2)); + })}; + Eigen::VectorXd values_{numTimes_}; + Eigen::VectorXd grads_{numTimes_}; }; // **************************** 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 */ + /// https://hal.inria.fr/hal-01944992/file/random_revision2.pdf + /// + /// \see For references about the derivatives of a Gaussian Process: + /// http://herbsusmann.com/2020/07/06/gaussian-process-derivatives + /// https://arxiv.org/abs/1810.12283 class JIMINY_DLLAPI PeriodicFourierProcess { public: @@ -371,7 +394,8 @@ namespace jiminy void reset(const uniform_random_bit_generator_ref & g) noexcept; - double operator()(float t); + double operator()(double t); + double gradient(double t); double getWavelength() const noexcept; double getPeriod() const noexcept; @@ -380,7 +404,7 @@ namespace jiminy const double wavelength_; const double period_; - const double dt_{0.02 * wavelength_}; + const double dt_{0.1 * wavelength_}; const Eigen::Index numTimes_{static_cast(std::ceil(period_ / dt_))}; const Eigen::Index numHarmonics_{ static_cast(std::ceil(period_ / wavelength_))}; @@ -389,13 +413,15 @@ namespace jiminy numTimes_, numHarmonics_, [numTimes = static_cast(numTimes_)](double i, double j) - { return std::cos(2 * M_PI / numTimes * i * j); })}; + { return std::cos(2 * M_PI / numTimes * i * (j + 1)); })}; const Eigen::MatrixXd sinMat_{Eigen::MatrixXd::NullaryExpr( numTimes_, numHarmonics_, [numTimes = static_cast(numTimes_)](double i, double j) - { return std::sin(2 * M_PI / numTimes * i * j); })}; + { return std::sin(2 * M_PI / numTimes * i * (j + 1)); })}; + Eigen::VectorXd values_{numTimes_}; + Eigen::VectorXd grads_{numTimes_}; }; // ***************************** Continuous 1D Perlin processes **************************** // @@ -461,8 +487,8 @@ namespace jiminy private: const double period_; - std::vector hashes_ = - std::vector(static_cast(period_ / wavelength_)); + std::vector grads_ = + std::vector(static_cast(period_ / wavelength_)); }; /// \brief Sum of Perlin noise octaves. @@ -496,7 +522,7 @@ namespace jiminy public: void reset(const uniform_random_bit_generator_ref & g) noexcept; - double operator()(float t); + double operator()(double t); double getWavelength() const noexcept; std::size_t getNumOctaves() const noexcept; diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index 8813f32e7..d6dcc8e6a 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -57,9 +57,9 @@ namespace jiminy template std::enable_if_t< - (is_eigen_any_v || - is_eigen_any_v)&&(!std::is_arithmetic_v> || - !std::is_arithmetic_v>), + (is_eigen_any_v || is_eigen_any_v) && + (!std::is_arithmetic_v> || + !std::is_arithmetic_v>), Eigen::CwiseNullaryOp< scalar_random_op &, float, float), Generator &, @@ -103,9 +103,9 @@ namespace jiminy template std::enable_if_t< - (is_eigen_any_v || - is_eigen_any_v)&&(!std::is_arithmetic_v> || - !std::is_arithmetic_v>), + (is_eigen_any_v || is_eigen_any_v) && + (!std::is_arithmetic_v> || + !std::is_arithmetic_v>), Eigen::CwiseNullaryOp< scalar_random_op &, float, float), Generator &, @@ -153,7 +153,7 @@ namespace jiminy { template MatrixX - standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs) + standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs, double reg) { using Scalar = typename Derived::Scalar; @@ -165,6 +165,7 @@ namespace jiminy It coincides with the Schur generator for Toepliz matrices. */ Eigen::Matrix g{2, n}; g.rowwise() = coeffs.transpose(); + g(0, 0) += reg; // Run progressive Schur algorithm, adapted to Toepliz matrices l.col(0) = g.row(0); diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index bb4dcd05e..b99d97571 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -247,6 +247,64 @@ namespace jiminy // **************************** Continuous 1D Gaussian processes *************************** // + static std::tuple getClosestKnots(double value, + double delta) + { + // Compute closest left and right indices + const double quot = value / delta; + const Eigen::Index indexLeft = static_cast(std::floor(quot)); + Eigen::Index indexRight = indexLeft + 1; + + // Compute the time ratio + const double ratio = quot - indexLeft; + + return {indexLeft, indexRight, ratio}; + } + + static std::tuple getClosestKnots( + double value, double delta, double period) + { + // Compute discretized period + const Eigen::Index numTimes = static_cast(std::ceil(period / delta)); + + // Wrap value in period interval + double periodKnots = numTimes * delta; + value = std::fmod(value, periodKnots); + if (value < 0.0) + { + value += periodKnots; + } + + // Compute closest left and right indices, wrapping around if needed + auto [indexLeft, indexRight, ratio] = getClosestKnots(value, delta); + if (indexRight == numTimes) + { + indexRight = 0; + } + return {indexLeft, indexRight, ratio}; + } + + template + static std::decay_t cubicInterp( + double ratio, double delta, T && valueLeft, T && valueRight, T && gradLeft, T && gradRight) + { + const auto dy = valueRight - valueLeft; + const auto a = gradLeft * delta - dy; + const auto b = -gradRight * delta + dy; + return valueLeft + ratio * ((1.0 - ratio) * ((1.0 - ratio) * a + ratio * b) + dy); + } + + template + static std::decay_t derivativeCubicInterp( + double ratio, double delta, T && valueLeft, T && valueRight, T && gradLeft, T && gradRight) + { + const auto dy = valueRight - valueLeft; + const auto a = gradLeft * delta - dy; + const auto b = -gradRight * delta + dy; + return ((1.0 - ratio) * (1.0 - 3.0 * ratio) * a + ratio * (2.0 - 3.0 * ratio) * b + dy) / + delta; + } + PeriodicGaussianProcess::PeriodicGaussianProcess(double wavelength, double period) noexcept : wavelength_{wavelength}, period_{period} @@ -258,28 +316,56 @@ namespace jiminy const uniform_random_bit_generator_ref & g) noexcept { // Sample normal vector - auto normalVec = normal(numTimes_, 1, g); + const Eigen::VectorXd normalVec = normal(numTimes_, 1, g).cast(); + + /* Compute discrete periodic gaussian process values. + + A gaussian process can be derived from a normally distributed random vector. + More precisely, a Gaussian Process y is uniquely defined by its kernel K and + a normally distributed random vector z ~ N(0, I). Let us consider a timestamp t. + The value of the Gaussian process y at time t is given by: + y(t) = K(t*, t) @ (L^-T @ z), + where: + t* are evenly spaced sampling timestamps associated with z + Cov = K(t*, t*) = L @ L^T is the Cholesky decomposition of the covariance matrix. + + Its analytical derivative can be deduced easily: + dy/dt(t) = dK/dt(t*, t) @ (L^-T @ z). - // Compute discrete periodic gaussian process values - values_.noalias() = covSqrtRoot_.triangularView() * normalVec.cast(); + When the query timestamps corresponds to the sampling timestamps, it yields: + y^* = K(t*, t*) @ (L^-T @ z) = L @ z + dy/dt^* = dK/dt(t*, t*) @ (L^-T @ z). */ + values_.noalias() = covSqrtRoot_.triangularView() * normalVec; + grads_.noalias() = + covJacobian_ * + covSqrtRoot_.transpose().triangularView().solve(normalVec); } - double PeriodicGaussianProcess::operator()(float t) + double PeriodicGaussianProcess::operator()(double t) { - // Wrap requested time in gaussian process period - double tWrap = std::fmod(t, period_); - if (tWrap < 0) - { - tWrap += period_; - } + // Compute closest left index within time period + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); - // Compute closest left and right indices - const Eigen::Index tLeftIndex = static_cast(std::floor(tWrap / dt_)); - const Eigen::Index tRightIndex = (tLeftIndex + 1) % numTimes_; + /* Perform cubic spline interpolation to ensure continuity of the derivative: + https://en.wikipedia.org/wiki/Spline_interpolation#Algorithm_to_find_the_interpolating_cubic_spline + */ + return cubicInterp(ratio, + dt_, + values_[indexLeft], + values_[indexRight], + grads_[indexLeft], + grads_[indexRight]); + } - // Perform First order interpolation - const double ratio = tWrap / dt_ - static_cast(tLeftIndex); - return values_[tLeftIndex] + ratio * (values_[tRightIndex] - values_[tLeftIndex]); + double PeriodicGaussianProcess::gradient(double t) + { + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); + return derivativeCubicInterp(ratio, + dt_, + values_[indexLeft], + values_[indexRight], + grads_[indexLeft], + grads_[indexRight]); } double PeriodicGaussianProcess::getWavelength() const noexcept @@ -305,31 +391,40 @@ namespace jiminy const uniform_random_bit_generator_ref & g) noexcept { // Sample normal vectors - auto normalVec1 = normal(numHarmonics_, 1, g); - auto normalVec2 = normal(numHarmonics_, 1, g); + const Eigen::VectorXd normalVec1 = normal(numHarmonics_, 1, g).cast(); + const Eigen::VectorXd normalVec2 = normal(numHarmonics_, 1, g).cast(); - // Compute discrete periodic gaussian process values + // Compute discrete periodic fourrier process values and derivatives const double scale = M_SQRT2 / std::sqrt(2 * numHarmonics_ + 1); - values_ = scale * cosMat_ * normalVec1.cast(); - values_.noalias() += scale * sinMat_ * normalVec2.cast(); + values_ = scale * sinMat_ * normalVec1; + values_.noalias() += scale * cosMat_ * normalVec2; + + const auto diff = + 2 * M_PI / period_ * Eigen::VectorXd::LinSpaced(numHarmonics_, 1, numHarmonics_); + grads_ = scale * cosMat_ * normalVec1.cwiseProduct(diff); + grads_.noalias() -= scale * sinMat_ * normalVec2.cwiseProduct(diff); } - double PeriodicFourierProcess::operator()(float t) + double PeriodicFourierProcess::operator()(double t) { - // Wrap requested time in guassian process period - double tWrap = std::fmod(t, period_); - if (tWrap < 0) - { - tWrap += period_; - } - - // Compute closest left and right indices - const Eigen::Index tLeftIndex = static_cast(std::floor(tWrap / dt_)); - const Eigen::Index tRightIndex = (tLeftIndex + 1) % numTimes_; + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); + return cubicInterp(ratio, + dt_, + values_[indexLeft], + values_[indexRight], + grads_[indexLeft], + grads_[indexRight]); + } - // Perform First order interpolation - const double ratio = tWrap / dt_ - static_cast(tLeftIndex); - return values_[tLeftIndex] + ratio * (values_[tRightIndex] - values_[tLeftIndex]); + double PeriodicFourierProcess::gradient(double t) + { + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); + return derivativeCubicInterp(ratio, + dt_, + values_[indexLeft], + values_[indexRight], + grads_[indexLeft], + grads_[indexRight]); } double PeriodicFourierProcess::getWavelength() const noexcept @@ -367,7 +462,7 @@ namespace jiminy const double phase = t / wavelength_ + shift_; // Compute closest right and left knots - const int32_t phaseIndexLeft = static_cast(phase); + const int32_t phaseIndexLeft = static_cast(std::lroundl(std::floor(phase))); const int32_t phaseIndexRight = phaseIndexLeft + 1; // Compute smoothed ratio of current phase wrt to the closest knots @@ -429,28 +524,24 @@ namespace jiminy const double grad = 2.0 * s - 1.0; // Return scalar product between distance and gradient - return 2.0 * grad * delta; + return grad * delta; } PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(double wavelength, double period) : - AbstractPerlinNoiseOctave(wavelength), + AbstractPerlinNoiseOctave(period / std::max(std::round(period / wavelength), 1.0)), period_{period} { // Make sure the period is larger than the wavelength - if (period_ < wavelength_) + if (period < wavelength) { JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); } - // Make sure the wavelength is multiple of the period - if (std::abs(std::round(period / wavelength) * wavelength - period) > - std::numeric_limits::epsilon()) - { - JIMINY_THROW(std::invalid_argument, "'wavelength' must be multiple of 'period'."); - } - // Initialize the pre-computed hash table - std::generate(hashes_.begin(), hashes_.end(), std::random_device{}); + std::generate(grads_.begin(), + grads_.end(), + [g = uniform_random_bit_generator_ref{std::random_device{}}]() + { return uniform(g, -1.0F, 1.0F); }); } void PeriodicPerlinNoiseOctave::reset( @@ -460,23 +551,21 @@ namespace jiminy AbstractPerlinNoiseOctave::reset(g); // Re-initialize the pre-computed hash table - std::generate(hashes_.begin(), hashes_.end(), g); + std::generate(grads_.begin(), grads_.end(), [&g]() { return uniform(g, -1.0F, 1.0F); }); } double PeriodicPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept { // Wrap knot is period interval - knot %= static_cast(period_ / wavelength_); - - // Convert to double in [0.0, 1.0) - const double s = static_cast(hashes_[knot]) / - static_cast(std::numeric_limits::max()); - - // Compute rescaled gradient between [-1.0, 1.0) - const double grad = 2.0 * s - 1.0; + const int32_t numTimes = static_cast(grads_.size()); + knot %= numTimes; + if (knot < 0) + { + knot += numTimes; + } - // Return scalar product between distance and gradient - return 2.0 * grad * delta; + // Return scalar product between time delta and gradient + return grads_[knot] * delta; } AbstractPerlinProcess::AbstractPerlinProcess( @@ -504,7 +593,7 @@ namespace jiminy } } - double AbstractPerlinProcess::operator()(float t) + double AbstractPerlinProcess::operator()(double t) { // Compute sum of octaves' values double value = 0.0; diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 6d8bcd7c4..7b7b3f513 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -142,9 +142,8 @@ namespace jiminy::python boost::noncopyable>( "PeriodicGaussianProcess", bp::init((bp::arg("self"), "wavelength", "period"))) - .def("__call__", - &PeriodicGaussianProcess::operator(), - (bp::arg("self"), bp::arg("time"))) + .def("__call__", &PeriodicGaussianProcess::operator(), (bp::arg("self"), "time")) + .def("gradient", &PeriodicGaussianProcess::gradient, (bp::arg("self"), "time")) .def("reset", makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicGaussianProcess::reset), bp::default_call_policies(), @@ -157,9 +156,8 @@ namespace jiminy::python boost::noncopyable>( "PeriodicFourierProcess", bp::init((bp::arg("self"), "wavelength", "period"))) - .def("__call__", - &PeriodicFourierProcess::operator(), - (bp::arg("self"), bp::arg("time"))) + .def("__call__", &PeriodicFourierProcess::operator(), (bp::arg("self"), "time")) + .def("gradient", &PeriodicFourierProcess::gradient, (bp::arg("self"), "time")) .def("reset", makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicFourierProcess::reset), bp::default_call_policies(), From da963791f493220e879963c80c075b58ba931e3a Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Thu, 11 Jul 2024 17:03:16 +0200 Subject: [PATCH 3/8] [core] Add N-dimension Perlin processes. --- core/include/jiminy/core/fwd.h | 5 + core/include/jiminy/core/robot/model.h | 1 - core/include/jiminy/core/utilities/random.h | 123 +++--- core/include/jiminy/core/utilities/random.hxx | 356 +++++++++++++++++ core/src/solver/constraint_solvers.cc | 1 - core/src/utilities/random.cc | 364 +++--------------- python/jiminy_pywrap/src/generators.cc | 118 +++--- 7 files changed, 550 insertions(+), 418 deletions(-) diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index f56254514..d7fd282fd 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -189,6 +189,11 @@ namespace jiminy using std::logic_error::logic_error::what; }; + template::min(), + ResultType max_ = std::numeric_limits::max()> + class uniform_random_bit_generator_ref; + // Ground profile functors using HeightmapFunction = std::function // `std::array` #include // `std::unique_ptr` #include // `std::optional` #include // `std::pair`, `std::declval` @@ -85,9 +84,7 @@ namespace jiminy /// /// \sa For technical reference about type-erasure for random generators: /// https://stackoverflow.com/a/77809228/4820605 - template::min(), - ResultType max_ = std::numeric_limits::max()> + template class uniform_random_bit_generator_ref : private function_ref { public: @@ -317,29 +314,40 @@ namespace jiminy standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs, double reg = 0.0); } - class JIMINY_DLLAPI PeriodicGaussianProcess + class JIMINY_DLLAPI PeriodicTabularProcess { public: - JIMINY_DISABLE_COPY(PeriodicGaussianProcess) + JIMINY_DISABLE_COPY(PeriodicTabularProcess) public: - explicit PeriodicGaussianProcess(double wavelength, double period) noexcept; + explicit PeriodicTabularProcess(double wavelength, double period); - void reset(const uniform_random_bit_generator_ref & g) noexcept; + virtual void reset(const uniform_random_bit_generator_ref & g) = 0; - double operator()(double t); - double gradient(double t); + double operator()(double t) const noexcept; + double grad(double t) const noexcept; double getWavelength() const noexcept; double getPeriod() const noexcept; - private: + protected: const double wavelength_; const double period_; + const Eigen::Index numTimes_{static_cast(std::ceil(period_ / (0.1 * wavelength_)))}; + const double dt_{period_ / numTimes_}; - const double dt_{0.1 * wavelength_}; - const Eigen::Index numTimes_{static_cast(std::ceil(period_ / dt_))}; + Eigen::VectorXd values_{numTimes_}; + Eigen::VectorXd grads_{numTimes_}; + }; + + class JIMINY_DLLAPI PeriodicGaussianProcess final : public PeriodicTabularProcess + { + public: + explicit PeriodicGaussianProcess(double wavelength, double period); + + void reset(const uniform_random_bit_generator_ref & g) noexcept override; + private: /// \brief Cholesky decomposition (LLT) of the covariance matrix. /// /// \details All decompositions are equivalent as the covariance matrix is symmetric, @@ -350,7 +358,6 @@ namespace jiminy /// decomposition algorithm. See: https://math.stackexchange.com/q/22825/375496 /// Ultimately, the algorithmic complexity can be reduced from O(n^3) to O(n^2), /// which is lower than the matrix multiplication itself. - /// Eigen::MatrixXd covSqrtRoot_{internal::standardToeplitzCholeskyLower( Eigen::VectorXd::NullaryExpr( numTimes_, @@ -371,9 +378,6 @@ namespace jiminy std::exp(-2.0 * std::pow(std::sin(M_PI / numTimes * (i - j)) / wavelength, 2)); })}; - - Eigen::VectorXd values_{numTimes_}; - Eigen::VectorXd grads_{numTimes_}; }; // **************************** Continuous 1D Fourier processes **************************** // @@ -384,28 +388,14 @@ namespace jiminy /// \see For references about the derivatives of a Gaussian Process: /// http://herbsusmann.com/2020/07/06/gaussian-process-derivatives /// https://arxiv.org/abs/1810.12283 - class JIMINY_DLLAPI PeriodicFourierProcess + class JIMINY_DLLAPI PeriodicFourierProcess final : public PeriodicTabularProcess { public: - JIMINY_DISABLE_COPY(PeriodicFourierProcess) - - public: - explicit PeriodicFourierProcess(double wavelength, double period) noexcept; - - void reset(const uniform_random_bit_generator_ref & g) noexcept; - - double operator()(double t); - double gradient(double t); + explicit PeriodicFourierProcess(double wavelength, double period); - double getWavelength() const noexcept; - double getPeriod() const noexcept; + void reset(const uniform_random_bit_generator_ref & g) noexcept override; private: - const double wavelength_; - const double period_; - - const double dt_{0.1 * wavelength_}; - const Eigen::Index numTimes_{static_cast(std::ceil(period_ / dt_))}; const Eigen::Index numHarmonics_{ static_cast(std::ceil(period_ / wavelength_))}; @@ -419,45 +409,46 @@ namespace jiminy numHarmonics_, [numTimes = static_cast(numTimes_)](double i, double j) { return std::sin(2 * M_PI / numTimes * i * (j + 1)); })}; - - Eigen::VectorXd values_{numTimes_}; - Eigen::VectorXd grads_{numTimes_}; }; - // ***************************** Continuous 1D Perlin processes **************************** // + // ****************************** Continuous Perlin processes ****************************** // + + uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept; + template class JIMINY_DLLAPI AbstractPerlinNoiseOctave { + public: + template + using VectorN = Eigen::Matrix; + public: explicit AbstractPerlinNoiseOctave(double wavelength); virtual ~AbstractPerlinNoiseOctave() = default; virtual void reset(const uniform_random_bit_generator_ref & g) noexcept; - double operator()(double t) const; + double operator()(const VectorN & x) const; + // VectorN grad(VectorN const & x) const; double getWavelength() const noexcept; protected: - virtual double grad(int32_t knot, double delta) const noexcept = 0; - - /// \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 yLeft, double yRight) noexcept; + virtual VectorN grad(const VectorN & knot) const noexcept = 0; protected: const double wavelength_; - double shift_{0.0}; + VectorN shift_{}; }; - class JIMINY_DLLAPI RandomPerlinNoiseOctave : public AbstractPerlinNoiseOctave + template + class JIMINY_DLLAPI RandomPerlinNoiseOctave : public AbstractPerlinNoiseOctave { + public: + template + using VectorN = Eigen::Matrix; + public: explicit RandomPerlinNoiseOctave(double wavelength); ~RandomPerlinNoiseOctave() override = default; @@ -465,14 +456,19 @@ namespace jiminy void reset(const uniform_random_bit_generator_ref & g) noexcept override; protected: - double grad(int32_t knot, double delta) const noexcept override; + VectorN grad(const VectorN & knot) const noexcept override; private: uint32_t seed_{0U}; }; - class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : public AbstractPerlinNoiseOctave + template + class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : public AbstractPerlinNoiseOctave { + public: + template + using VectorN = Eigen::Matrix; + public: explicit PeriodicPerlinNoiseOctave(double wavelength, double period); ~PeriodicPerlinNoiseOctave() override = default; @@ -482,13 +478,14 @@ namespace jiminy double getPeriod() const noexcept; protected: - double grad(int32_t knot, double delta) const noexcept override; + VectorN grad(const VectorN & knot) const noexcept override; private: const double period_; - std::vector grads_ = - std::vector(static_cast(period_ / wavelength_)); + const int32_t size_{static_cast(period_ / this->wavelength_)}; + std::vector> grads_ = + std::vector>(static_cast(std::pow(size_, N))); }; /// \brief Sum of Perlin noise octaves. @@ -511,18 +508,22 @@ namespace jiminy /// https://github.com/bradykieffer/SimplexNoise/blob/master/simplexnoise/noise.py /// https://github.com/sol-prog/Perlin_Noise/blob/master/PerlinNoise.cpp /// https://github.com/ashima/webgl-noise/blob/master/src/classicnoise2D.glsl + template class JIMINY_DLLAPI AbstractPerlinProcess { public: JIMINY_DISABLE_COPY(AbstractPerlinProcess) + template + using VectorN = Eigen::Matrix; + using OctaveScalePair = - std::pair, const double>; + std::pair>, const double>; public: void reset(const uniform_random_bit_generator_ref & g) noexcept; - double operator()(double t); + double operator()(const VectorN & x) const; double getWavelength() const noexcept; std::size_t getNumOctaves() const noexcept; @@ -537,13 +538,15 @@ namespace jiminy double amplitude_{0.0}; }; - class JIMINY_DLLAPI RandomPerlinProcess : public AbstractPerlinProcess + template + class JIMINY_DLLAPI RandomPerlinProcess : public AbstractPerlinProcess { public: explicit RandomPerlinProcess(double wavelength, std::size_t numOctaves = 6U); }; - class PeriodicPerlinProcess : public AbstractPerlinProcess + template + class PeriodicPerlinProcess : public AbstractPerlinProcess { public: explicit PeriodicPerlinProcess( diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index d6dcc8e6a..d448e2bab 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -8,6 +8,9 @@ namespace jiminy { + static inline constexpr double PERLIN_NOISE_PERSISTENCE{1.50}; + static inline constexpr double PERLIN_NOISE_LACUNARITY{1.15}; + // ***************************** Uniform random bit generators ***************************** // namespace internal @@ -185,6 +188,359 @@ namespace jiminy return l; } } + + // ****************************** Continuous Perlin processes ****************************** // + + /// \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 inline double 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: + https://en.wikipedia.org/wiki/Smoothstep#Variations */ + return std::pow(delta, 3) * (delta * (delta * 6.0 - 15.0) + 10.0); + } + + static inline double lerp(double ratio, double yLeft, double yRight) noexcept + { + return yLeft + ratio * (yRight - yLeft); + } + + template + AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) : + wavelength_{wavelength} + { + if (wavelength_ <= 0.0) + { + JIMINY_THROW(std::invalid_argument, "'wavelength' must be strictly larger than 0.0."); + } + auto gen = uniform_random_bit_generator_ref{std::random_device{}}; + shift_ = uniform(N, 1, gen).cast(); + } + + template + void AbstractPerlinNoiseOctave::reset( + const uniform_random_bit_generator_ref & g) noexcept + { + // Sample random phase shift + shift_ = uniform(N, 1, g).cast(); + } + + template + double AbstractPerlinNoiseOctave::getWavelength() const noexcept + { + return wavelength_; + } + + template + double AbstractPerlinNoiseOctave::operator()(const VectorN & x) const + { + // Get current phase + const VectorN phase = x / wavelength_ + shift_; + + // Compute closest right and left knots + const VectorN phaseIndexLeft = phase.array().floor().template cast(); + const VectorN phaseIndexRight = phaseIndexLeft.array() + 1; + + // Compute smoothed ratio of current phase wrt to the closest knots + const VectorN dtLeft = phase - phaseIndexLeft.template cast(); + const VectorN dtRight = dtLeft.array() - 1.0; + const VectorN ratio = dtLeft.array().unaryExpr(std::ref(fade)); + + // Compute gradients at knots, ie on a meshgrid, then dedicate offsets at query point + VectorN knot; + VectorN delta; + std::array offsets; + for (uint32_t k = 0; k < (1U << N); k++) + { + // Mapping from index to knot + for (uint32_t i = 0; i < N; i++) + { + if (k & (1 << i)) + { + knot[i] = phaseIndexRight[i]; + delta[i] = dtRight[i]; + } + else + { + knot[i] = phaseIndexLeft[i]; + delta[i] = dtLeft[i]; + } + } + + // Evaluate the gradient at knot + const VectorN grad_ = grad(knot); + + // Compute the offset at query point + offsets[k] = grad_.dot(delta); + } + + // Perform linear interpolation on each dimension recursively until to get a single scalar + for (int32_t i = N - 1; i >= 0; --i) + { + for (uint32_t k = 0; k < (1U << i); k++) + { + double & offsetLeft = offsets[k]; + const double offsetRight = offsets[k | (1U << i)]; + offsetLeft = lerp(ratio[i], offsetLeft, offsetRight); + } + } + return offsets[0]; + } + + template + RandomPerlinNoiseOctave::RandomPerlinNoiseOctave(double wavelength) : + AbstractPerlinNoiseOctave(wavelength) + { + seed_ = std::random_device{}(); + } + + template + void RandomPerlinNoiseOctave::reset( + const uniform_random_bit_generator_ref & g) noexcept + { + // Call base implementation + AbstractPerlinNoiseOctave::reset(g); + + // Sample new random seed for MurmurHash + seed_ = g(); + } + + template + typename RandomPerlinNoiseOctave::template VectorN + RandomPerlinNoiseOctave::grad(const VectorN & knot) const noexcept + { + constexpr float fHashMax = static_cast(std::numeric_limits::max()); + + // Compute knot hash + uint32_t hash = MurmurHash3(knot.data(), static_cast(sizeof(int32_t) * N), seed_); + + /* Generate random gradient uniformly distributed on n-ball. + For technical reference, see: + https://extremelearning.com.au/how-to-generate-uniformly-random-points-on-n-spheres-and-n-balls/ + */ + if constexpr (N == 1) + { + // Sample random scalar in [0.0, 1.0) + const float s = static_cast(hash) / fHashMax; + + // Compute rescaled gradient between [-1.0, 1.0) + return VectorN{2.0 * s - 1.0}; + } + else if constexpr (N == 2) + { + // Sample random vector on a 2-ball (disk) using + const double theta = 2 * M_PI * static_cast(hash) / fHashMax; + hash = MurmurHash3(&hash, sizeof(uint32_t), seed_); + const float radius = std::sqrt(static_cast(hash) / fHashMax); + return VectorN{radius * std::cos(theta), radius * std::sin(theta)}; + } + else + { + // Generate a uniformly distributed random vector on n-sphere + VectorN dir; + for (uint_fast8_t i = 0; i < N; i += 2) + { + // Generate 2 uniformly distributed random variables + const float u1 = static_cast(hash) / fHashMax; + hash = MurmurHash3(&hash, sizeof(uint32_t), seed_); + const float u2 = static_cast(hash) / fHashMax; + hash = MurmurHash3(&hash, sizeof(uint32_t), seed_); + + // Apply Box-Mueller algorithm to deduce 2 normally distributed random variables + const double theta = 2 * M_PI * u2; + const float radius = std::sqrt(-2 * std::log(u1)); + dir[i] = radius * std::cos(theta); + if (i + 1 < N) + { + dir[i + 1] = radius * std::sin(theta); + } + } + dir.normalize(); + + // Sample radius + const double radius = std::pow(static_cast(hash) / fHashMax, 1.0 / N); + + // Return the resulting random vector on n-ball using Muller method + return radius * dir; + } + } + + template + PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(double wavelength, double period) : + AbstractPerlinNoiseOctave(period / std::max(std::round(period / wavelength), 1.0)), + period_{period} + { + // Make sure the period is larger than the wavelength + if (period < wavelength) + { + JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); + } + + // Initialize the pre-computed hash table + reset(std::random_device{}); + } + + template + void PeriodicPerlinNoiseOctave::reset( + const uniform_random_bit_generator_ref & g) noexcept + { + // Call base implementation + AbstractPerlinNoiseOctave::reset(g); + + // Re-initialize the pre-computed hash table + std::generate( + grads_.begin(), + grads_.end(), + [&g]() -> VectorN + { + if constexpr (N == 1) + { + return VectorN{uniform(g, -1.0F, 1.0F)}; + } + else if constexpr (N == 2) + { + const double theta = 2 * M_PI * uniform(g); + const float radius = std::sqrt(uniform(g)); + return VectorN{radius * std::cos(theta), radius * std::sin(theta)}; + } + else + { + const VectorN dir = normal(N, 1, g).cast().normalized(); + const double radius = std::pow(uniform(g), 1.0 / N); + return radius * dir; + } + }); + } + + template + typename PeriodicPerlinNoiseOctave::template VectorN + PeriodicPerlinNoiseOctave::grad(const VectorN & knot) const noexcept + { + // Wrap knot is period interval + int32_t index = 0; + for (uint_fast8_t i = 0; i < N; ++i) + { + int32_t coord = knot[i] % size_; + if (coord < 0) + { + coord += size_; + } + index += coord * static_cast(std::pow(size_, i)); + } + + // Return the gradient + return grads_[index]; + } + + template + AbstractPerlinProcess::AbstractPerlinProcess( + std::vector && octaveScalePairs) noexcept : + octaveScalePairs_(std::move(octaveScalePairs)) + { + // Compute the scaling factor to keep values within range [-1.0, 1.0] + double amplitudeSquared = 0.0; + for (const OctaveScalePair & octaveScale : octaveScalePairs_) + { + // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) + amplitudeSquared += std::pow(std::get<1>(octaveScale), 2); + } + amplitude_ = std::sqrt(amplitudeSquared); + } + + template + void + AbstractPerlinProcess::reset(const uniform_random_bit_generator_ref & g) noexcept + { + // Reset octaves + for (OctaveScalePair & octaveScale : octaveScalePairs_) + { + // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) + std::get<0>(octaveScale)->reset(g); + } + } + + template + double AbstractPerlinProcess::operator()(const VectorN & x) const + { + // Compute sum of octaves' values + double value = 0.0; + for (const auto & [octave, scale] : octaveScalePairs_) + { + value += scale * (*octave)(x); + } + + // Scale sum by maximum amplitude + return value / amplitude_; + } + + template + double AbstractPerlinProcess::getWavelength() const noexcept + { + double wavelength = INF; + for (const OctaveScalePair & octaveScale : octaveScalePairs_) + { + // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) + wavelength = std::min(wavelength, std::get<0>(octaveScale)->getWavelength()); + } + return wavelength; + } + + template + std::size_t AbstractPerlinProcess::getNumOctaves() const noexcept + { + return octaveScalePairs_.size(); + } + + template + static std::vector::OctaveScalePair> buildPerlinNoiseOctaves( + double wavelength, + std::size_t numOctaves, + std::function>(double)> factory) + { + std::vector::OctaveScalePair> octaveScalePairs; + octaveScalePairs.reserve(numOctaves); + double scale = 1.0; + for (std::size_t i = 0; i < numOctaves; ++i) + { + octaveScalePairs.emplace_back(factory(wavelength), scale); + wavelength *= PERLIN_NOISE_LACUNARITY; + scale *= PERLIN_NOISE_PERSISTENCE; + } + return octaveScalePairs; + } + + template + RandomPerlinProcess::RandomPerlinProcess(double wavelength, std::size_t numOctaves) : + AbstractPerlinProcess(buildPerlinNoiseOctaves( + wavelength, + numOctaves, + [](double wavelengthIn) -> std::unique_ptr> + { return std::make_unique>(wavelengthIn); })) + { + } + + template + PeriodicPerlinProcess::PeriodicPerlinProcess( + double wavelength, double period, std::size_t numOctaves) : + AbstractPerlinProcess(buildPerlinNoiseOctaves( + wavelength, + numOctaves, + [period](double wavelengthIn) -> std::unique_ptr> + { return std::make_unique>(wavelengthIn, period); })), + period_{period} + { + } + + template + double PeriodicPerlinProcess::getPeriod() const noexcept + { + return period_; + } } #endif // JIMINY_RANDOM_HXX diff --git a/core/src/solver/constraint_solvers.cc b/core/src/solver/constraint_solvers.cc index 8bbbf4f9e..d56cde3c1 100644 --- a/core/src/solver/constraint_solvers.cc +++ b/core/src/solver/constraint_solvers.cc @@ -2,7 +2,6 @@ #include "pinocchio/multibody/data.hpp" // `pinocchio::Data` #include "pinocchio/algorithm/cholesky.hpp" // `pinocchio::cholesky::` -#include "jiminy/core/utilities/random.h" #include "jiminy/core/utilities/helpers.h" #include "jiminy/core/constraints/abstract_constraint.h" #include "jiminy/core/robot/pinocchio_overload_algorithms.h" diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index b99d97571..d44dd3bc6 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -5,9 +5,6 @@ namespace jiminy { - static inline constexpr double PERLIN_NOISE_PERSISTENCE{1.50}; - static inline constexpr double PERLIN_NOISE_LACUNARITY{1.15}; - // ***************************** Uniform random bit generators ***************************** // PCG32::PCG32(uint64_t state) noexcept : @@ -182,7 +179,7 @@ namespace jiminy /// \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 + uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept { // Define some internal constants constexpr uint32_t c1 = 0xcc9e2d51; @@ -262,17 +259,14 @@ namespace jiminy } static std::tuple getClosestKnots( - double value, double delta, double period) + double value, double delta, Eigen::Index numTimes) { - // Compute discretized period - const Eigen::Index numTimes = static_cast(std::ceil(period / delta)); - // Wrap value in period interval - double periodKnots = numTimes * delta; - value = std::fmod(value, periodKnots); + const double period = numTimes * delta; + value = std::fmod(value, period); if (value < 0.0) { - value += periodKnots; + value += period; } // Compute closest left and right indices, wrapping around if needed @@ -305,46 +299,21 @@ namespace jiminy delta; } - PeriodicGaussianProcess::PeriodicGaussianProcess(double wavelength, double period) noexcept : + PeriodicTabularProcess::PeriodicTabularProcess(double wavelength, double period) : wavelength_{wavelength}, period_{period} { - reset(std::random_device{}); - } - - void PeriodicGaussianProcess::reset( - const uniform_random_bit_generator_ref & g) noexcept - { - // Sample normal vector - const Eigen::VectorXd normalVec = normal(numTimes_, 1, g).cast(); - - /* Compute discrete periodic gaussian process values. - - A gaussian process can be derived from a normally distributed random vector. - More precisely, a Gaussian Process y is uniquely defined by its kernel K and - a normally distributed random vector z ~ N(0, I). Let us consider a timestamp t. - The value of the Gaussian process y at time t is given by: - y(t) = K(t*, t) @ (L^-T @ z), - where: - t* are evenly spaced sampling timestamps associated with z - Cov = K(t*, t*) = L @ L^T is the Cholesky decomposition of the covariance matrix. - - Its analytical derivative can be deduced easily: - dy/dt(t) = dK/dt(t*, t) @ (L^-T @ z). - - When the query timestamps corresponds to the sampling timestamps, it yields: - y^* = K(t*, t*) @ (L^-T @ z) = L @ z - dy/dt^* = dK/dt(t*, t*) @ (L^-T @ z). */ - values_.noalias() = covSqrtRoot_.triangularView() * normalVec; - grads_.noalias() = - covJacobian_ * - covSqrtRoot_.transpose().triangularView().solve(normalVec); + // Make sure the period is positive + if (period < 0.0) + { + JIMINY_THROW(std::invalid_argument, "'period' must be positive."); + } } - double PeriodicGaussianProcess::operator()(double t) + double PeriodicTabularProcess::operator()(double t) const noexcept { // Compute closest left index within time period - const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, numTimes_); /* Perform cubic spline interpolation to ensure continuity of the derivative: https://en.wikipedia.org/wiki/Spline_interpolation#Algorithm_to_find_the_interpolating_cubic_spline @@ -357,9 +326,9 @@ namespace jiminy grads_[indexRight]); } - double PeriodicGaussianProcess::gradient(double t) + double PeriodicTabularProcess::grad(double t) const noexcept { - const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); + const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, numTimes_); return derivativeCubicInterp(ratio, dt_, values_[indexLeft], @@ -368,21 +337,55 @@ namespace jiminy grads_[indexRight]); } - double PeriodicGaussianProcess::getWavelength() const noexcept + double PeriodicTabularProcess::getWavelength() const noexcept { return wavelength_; } - double PeriodicGaussianProcess::getPeriod() const noexcept + double PeriodicTabularProcess::getPeriod() const noexcept { return period_; } + PeriodicGaussianProcess::PeriodicGaussianProcess(double wavelength, double period) : + PeriodicTabularProcess(wavelength, period) + { + reset(std::random_device{}); + } + + void PeriodicGaussianProcess::reset( + const uniform_random_bit_generator_ref & g) noexcept + { + // Sample normal vector + const Eigen::VectorXd normalVec = normal(numTimes_, 1, g).cast(); + + /* Compute discrete periodic gaussian process values. + + A gaussian process can be derived from a normally distributed random vector. + More precisely, a Gaussian Process y is uniquely defined by its kernel K and + a normally distributed random vector z ~ N(0, I). Let us consider a timestamp t. + The value of the Gaussian process y at time t is given by: + y(t) = K(t*, t) @ (L^-T @ z), + where: + t* are evenly spaced sampling timestamps associated with z + Cov = K(t*, t*) = L @ L^T is the Cholesky decomposition of the covariance matrix. + + Its analytical derivative can be deduced easily: + dy/dt(t) = dK/dt(t*, t) @ (L^-T @ z). + + When the query timestamps corresponds to the sampling timestamps, it yields: + y^* = K(t*, t*) @ (L^-T @ z) = L @ z + dy/dt^* = dK/dt(t*, t*) @ (L^-T @ z). */ + values_.noalias() = covSqrtRoot_.triangularView() * normalVec; + grads_.noalias() = + covJacobian_ * + covSqrtRoot_.transpose().triangularView().solve(normalVec); + } + // **************************** Continuous 1D Fourier processes **************************** // - PeriodicFourierProcess::PeriodicFourierProcess(double wavelength, double period) noexcept : - wavelength_{wavelength}, - period_{period} + PeriodicFourierProcess::PeriodicFourierProcess(double wavelength, double period) : + PeriodicTabularProcess(wavelength, period) { reset(std::random_device{}); } @@ -405,265 +408,6 @@ namespace jiminy grads_.noalias() -= scale * sinMat_ * normalVec2.cwiseProduct(diff); } - double PeriodicFourierProcess::operator()(double t) - { - const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); - return cubicInterp(ratio, - dt_, - values_[indexLeft], - values_[indexRight], - grads_[indexLeft], - grads_[indexRight]); - } - - double PeriodicFourierProcess::gradient(double t) - { - const auto [indexLeft, indexRight, ratio] = getClosestKnots(t, dt_, period_); - return derivativeCubicInterp(ratio, - dt_, - values_[indexLeft], - values_[indexRight], - grads_[indexLeft], - grads_[indexRight]); - } - - double PeriodicFourierProcess::getWavelength() const noexcept - { - return wavelength_; - } - - double PeriodicFourierProcess::getPeriod() const noexcept - { - return period_; - } - - // ***************************** Continuous 1D Perlin processes **************************** // - - AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) : - wavelength_{wavelength} - { - if (wavelength_ <= 0.0) - { - JIMINY_THROW(std::invalid_argument, "'wavelength' must be strictly larger than 0.0."); - } - shift_ = uniform(std::random_device{}); - } - - void AbstractPerlinNoiseOctave::reset( - const uniform_random_bit_generator_ref & g) noexcept - { - // Sample random phase shift - shift_ = uniform(g); - } - - double AbstractPerlinNoiseOctave::operator()(double t) const - { - // Get current phase - const double phase = t / wavelength_ + shift_; - - // Compute closest right and left knots - const int32_t phaseIndexLeft = static_cast(std::lroundl(std::floor(phase))); - const int32_t phaseIndexRight = phaseIndexLeft + 1; - - // Compute smoothed ratio of current phase wrt to the closest knots - const double dtLeft = phase - phaseIndexLeft; - const double dtRight = dtLeft - 1.0; - const double ratio = fade(dtLeft); - - /* Compute gradients at knots, and perform linear interpolation between them to get value - at current phase.*/ - const double yLeft = grad(phaseIndexLeft, dtLeft); - const double yRight = grad(phaseIndexRight, dtRight); - return lerp(ratio, yLeft, yRight); - } - - double AbstractPerlinNoiseOctave::getWavelength() const noexcept - { - return wavelength_; - } - - 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: - https://en.wikipedia.org/wiki/Smoothstep#Variations */ - return std::pow(delta, 3) * (delta * (delta * 6.0 - 15.0) + 10.0); - } - - double AbstractPerlinNoiseOctave::lerp(double ratio, double yLeft, double yRight) noexcept - { - return yLeft + ratio * (yRight - yLeft); - } - - RandomPerlinNoiseOctave::RandomPerlinNoiseOctave(double wavelength) : - AbstractPerlinNoiseOctave(wavelength) - { - seed_ = std::random_device{}(); - } - - void RandomPerlinNoiseOctave::reset( - const uniform_random_bit_generator_ref & g) noexcept - { - // Call base implementation - AbstractPerlinNoiseOctave::reset(g); - - // Sample new random seed for MurmurHash - seed_ = g(); - } - - double RandomPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept - { - // Get hash of knot - const uint32_t hash = MurmurHash3(&knot, sizeof(int32_t), seed_); - - // Convert to double in [0.0, 1.0) - const double s = - static_cast(hash) / static_cast(std::numeric_limits::max()); - - // Compute rescaled gradient between [-1.0, 1.0) - const double grad = 2.0 * s - 1.0; - - // Return scalar product between distance and gradient - return grad * delta; - } - - PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(double wavelength, double period) : - AbstractPerlinNoiseOctave(period / std::max(std::round(period / wavelength), 1.0)), - period_{period} - { - // Make sure the period is larger than the wavelength - if (period < wavelength) - { - JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); - } - - // Initialize the pre-computed hash table - std::generate(grads_.begin(), - grads_.end(), - [g = uniform_random_bit_generator_ref{std::random_device{}}]() - { return uniform(g, -1.0F, 1.0F); }); - } - - void PeriodicPerlinNoiseOctave::reset( - const uniform_random_bit_generator_ref & g) noexcept - { - // Call base implementation - AbstractPerlinNoiseOctave::reset(g); - - // Re-initialize the pre-computed hash table - std::generate(grads_.begin(), grads_.end(), [&g]() { return uniform(g, -1.0F, 1.0F); }); - } - - double PeriodicPerlinNoiseOctave::grad(int32_t knot, double delta) const noexcept - { - // Wrap knot is period interval - const int32_t numTimes = static_cast(grads_.size()); - knot %= numTimes; - if (knot < 0) - { - knot += numTimes; - } - - // Return scalar product between time delta and gradient - return grads_[knot] * delta; - } - - AbstractPerlinProcess::AbstractPerlinProcess( - std::vector && octaveScalePairs) noexcept : - octaveScalePairs_(std::move(octaveScalePairs)) - { - // Compute the scaling factor to keep values within range [-1.0, 1.0] - double amplitudeSquared = 0.0; - for (const OctaveScalePair & octaveScale : octaveScalePairs_) - { - // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - amplitudeSquared += std::pow(std::get<1>(octaveScale), 2); - } - amplitude_ = std::sqrt(amplitudeSquared); - } - - void AbstractPerlinProcess::reset( - const uniform_random_bit_generator_ref & g) noexcept - { - // Reset octaves - for (OctaveScalePair & octaveScale : octaveScalePairs_) - { - // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - std::get<0>(octaveScale)->reset(g); - } - } - - double AbstractPerlinProcess::operator()(double t) - { - // Compute sum of octaves' values - double value = 0.0; - for (const auto & [octave, scale] : octaveScalePairs_) - { - value += scale * (*octave)(t); - } - - // Scale sum by maximum amplitude - return value / amplitude_; - } - - double AbstractPerlinProcess::getWavelength() const noexcept - { - double wavelength = INF; - for (const OctaveScalePair & octaveScale : octaveScalePairs_) - { - // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - wavelength = std::min(wavelength, std::get<0>(octaveScale)->getWavelength()); - } - return wavelength; - } - - std::size_t AbstractPerlinProcess::getNumOctaves() const noexcept - { - return octaveScalePairs_.size(); - } - - std::vector buildPerlinNoiseOctaves( - double wavelength, - std::size_t numOctaves, - std::function(double)> factory) - { - std::vector octaveScalePairs; - octaveScalePairs.reserve(numOctaves); - double scale = 1.0; - for (std::size_t i = 0; i < numOctaves; ++i) - { - octaveScalePairs.emplace_back(factory(wavelength), scale); - wavelength *= PERLIN_NOISE_LACUNARITY; - scale *= PERLIN_NOISE_PERSISTENCE; - } - return octaveScalePairs; - } - - RandomPerlinProcess::RandomPerlinProcess(double wavelength, std::size_t numOctaves) : - AbstractPerlinProcess(buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [](double wavelengthIn) -> std::unique_ptr - { return std::make_unique(wavelengthIn); })) - { - } - - PeriodicPerlinProcess::PeriodicPerlinProcess( - double wavelength, double period, std::size_t numOctaves) : - AbstractPerlinProcess(buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [period](double wavelengthIn) -> std::unique_ptr - { return std::make_unique(wavelengthIn, period); })), - period_{period} - { - } - - double PeriodicPerlinProcess::getPeriod() const noexcept - { - return period_; - } - // ******************************* Random terrain generators ******************************* // template diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 7b7b3f513..2ce6f70f8 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -35,7 +35,6 @@ namespace jiminy::python 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 & generator, \ @@ -93,6 +92,57 @@ namespace jiminy::python #undef GENERIC_DISTRIBUTION_WRAPPER + template + std::enable_if_t>...>, double> + evaluateRandomProcessUnpacked(AbstractPerlinProcess & process, Args... args) + { + return process(Eigen::Matrix{args...}); + } + + template + using type_t = T; + + template + auto evaluateRandomProcessUnpackedSignature(std::index_sequence) + -> double (*)(AbstractPerlinProcess &, type_t...); + + template + constexpr void exposeRandomProcess() + { + bp::class_, + std::shared_ptr>, + boost::noncopyable>(toString("AbstractPerlinProcess", N, "D").c_str(), + bp::no_init) + .def("__call__", + static_cast{}))>(evaluateRandomProcessUnpacked)) + .def("__call__", &AbstractPerlinProcess::operator(), (bp::arg("self"), "vec")) + .def( + "reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) + .ADD_PROPERTY_GET("wavelength", &AbstractPerlinProcess::getWavelength) + .ADD_PROPERTY_GET("num_octaves", &AbstractPerlinProcess::getNumOctaves); + + bp::class_, + bp::bases>, + std::shared_ptr>, + boost::noncopyable>( + toString("RandomPerlinProcess", N, "D").c_str(), + bp::init( + (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))); + + bp::class_, + bp::bases>, + std::shared_ptr>, + boost::noncopyable>( + toString("PeriodicPerlinProcess", N, "D").c_str(), + bp::init( + (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) + .ADD_PROPERTY_GET("period", &PeriodicPerlinProcess::getPeriod); + } + void exposeGenerators() { bp::class_, boost::noncopyable>( @@ -137,61 +187,37 @@ namespace jiminy::python bp::default_call_policies(), (bp::arg("generator")))); + bp::class_, + boost::noncopyable>("PeriodicTabularProcess", bp::no_init) + .def("__call__", &PeriodicTabularProcess::operator(), (bp::arg("self"), "time")) + .def("grad", &PeriodicTabularProcess::grad, (bp::arg("self"), "time")) + .def("reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicTabularProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) + .ADD_PROPERTY_GET("wavelength", &PeriodicTabularProcess::getWavelength) + .ADD_PROPERTY_GET("period", &PeriodicTabularProcess::getPeriod); + bp::class_, std::shared_ptr, boost::noncopyable>( "PeriodicGaussianProcess", - bp::init((bp::arg("self"), "wavelength", "period"))) - .def("__call__", &PeriodicGaussianProcess::operator(), (bp::arg("self"), "time")) - .def("gradient", &PeriodicGaussianProcess::gradient, (bp::arg("self"), "time")) - .def("reset", - makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicGaussianProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) - .ADD_PROPERTY_GET("wavelength", &PeriodicGaussianProcess::getWavelength) - .ADD_PROPERTY_GET("period", &PeriodicGaussianProcess::getPeriod); + bp::init((bp::arg("self"), "wavelength", "period"))); bp::class_, std::shared_ptr, boost::noncopyable>( "PeriodicFourierProcess", - bp::init((bp::arg("self"), "wavelength", "period"))) - .def("__call__", &PeriodicFourierProcess::operator(), (bp::arg("self"), "time")) - .def("gradient", &PeriodicFourierProcess::gradient, (bp::arg("self"), "time")) - .def("reset", - makeFunction(ConvertGeneratorFromPythonAndInvoke(&PeriodicFourierProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) - .ADD_PROPERTY_GET("wavelength", &PeriodicFourierProcess::getWavelength) - .ADD_PROPERTY_GET("period", &PeriodicFourierProcess::getPeriod); - - bp::class_, - boost::noncopyable>("AbstractPerlinProcess", bp::no_init) - .def("__call__", &AbstractPerlinProcess::operator(), (bp::arg("self"), "time")) - .def("reset", - makeFunction(ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) - .ADD_PROPERTY_GET("wavelength", &AbstractPerlinProcess::getWavelength) - .ADD_PROPERTY_GET("num_octaves", &AbstractPerlinProcess::getNumOctaves); + bp::init((bp::arg("self"), "wavelength", "period"))); - bp::class_, - std::shared_ptr, - boost::noncopyable>( - "RandomPerlinProcess", - bp::init( - (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))); - - bp::class_, - std::shared_ptr, - boost::noncopyable>( - "PeriodicPerlinProcess", - bp::init( - (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) - .ADD_PROPERTY_GET("period", &PeriodicPerlinProcess::getPeriod); + /* FIXME: Use template lambda and compile-time for-loop when moving to c++20. + For reference: https://stackoverflow.com/a/76272348/4820605 */ + exposeRandomProcess<1>(); + exposeRandomProcess<2>(); + exposeRandomProcess<3>(); bp::def( "random_tile_ground", From 49617c699b9542df47359db905a41ce355135223 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Jul 2024 11:51:06 +0200 Subject: [PATCH 4/8] [core] Add perlin process gradient computation. --- core/include/jiminy/core/utilities/random.h | 8 +- core/include/jiminy/core/utilities/random.hxx | 120 +++++++++++++++--- python/jiminy_pywrap/src/generators.cc | 21 ++- 3 files changed, 129 insertions(+), 20 deletions(-) diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 8c0dcd937..1497c6825 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -429,13 +429,18 @@ namespace jiminy virtual void reset(const uniform_random_bit_generator_ref & g) noexcept; double operator()(const VectorN & x) const; - // VectorN grad(VectorN const & x) const; + VectorN grad(const VectorN & x) const; double getWavelength() const noexcept; protected: virtual VectorN grad(const VectorN & knot) const noexcept = 0; + private: + template + std::conditional_t, double> + evaluate(const VectorN & x) const; + protected: const double wavelength_; @@ -524,6 +529,7 @@ namespace jiminy void reset(const uniform_random_bit_generator_ref & g) noexcept; double operator()(const VectorN & x) const; + VectorN grad(const VectorN & x) const; double getWavelength() const noexcept; std::size_t getNumOctaves() const noexcept; diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index d448e2bab..59e4867a9 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -205,11 +205,25 @@ namespace jiminy return std::pow(delta, 3) * (delta * (delta * 6.0 - 15.0) + 10.0); } - static inline double lerp(double ratio, double yLeft, double yRight) noexcept + static inline double derivativeFade(double delta) noexcept + { + return 30 * std::pow(delta, 2) * (delta * (delta - 2.0) + 1.0); + } + + template + static std::common_type_t, std::decay_t> + lerp(double ratio, T1 && yLeft, T2 && yRight) noexcept { return yLeft + ratio * (yRight - yLeft); } + template + static std::common_type_t, std::decay_t> + derivativeLerp(double dratio, T1 && yLeft, T2 && yRight) noexcept + { + return dratio * (yRight - yLeft); + } + template AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) : wavelength_{wavelength} @@ -237,7 +251,11 @@ namespace jiminy } template - double AbstractPerlinNoiseOctave::operator()(const VectorN & x) const + template + std::conditional_t::template VectorN, + double> + AbstractPerlinNoiseOctave::evaluate(const VectorN & x) const { // Get current phase const VectorN phase = x / wavelength_ + shift_; @@ -247,49 +265,102 @@ namespace jiminy const VectorN phaseIndexRight = phaseIndexLeft.array() + 1; // Compute smoothed ratio of current phase wrt to the closest knots - const VectorN dtLeft = phase - phaseIndexLeft.template cast(); - const VectorN dtRight = dtLeft.array() - 1.0; - const VectorN ratio = dtLeft.array().unaryExpr(std::ref(fade)); + const VectorN deltaLeft = phase - phaseIndexLeft.template cast(); + const VectorN deltaRight = deltaLeft.array() - 1.0; // Compute gradients at knots, ie on a meshgrid, then dedicate offsets at query point VectorN knot; VectorN delta; std::array offsets; + std::array, (1U << N)> grads; for (uint32_t k = 0; k < (1U << N); k++) { // Mapping from index to knot for (uint32_t i = 0; i < N; i++) { - if (k & (1 << i)) + if (k & (1U << i)) { knot[i] = phaseIndexRight[i]; - delta[i] = dtRight[i]; + delta[i] = deltaRight[i]; } else { knot[i] = phaseIndexLeft[i]; - delta[i] = dtLeft[i]; + delta[i] = deltaLeft[i]; } } // Evaluate the gradient at knot - const VectorN grad_ = grad(knot); + grads[k] = grad(knot); // Compute the offset at query point - offsets[k] = grad_.dot(delta); + offsets[k] = grads[k].dot(delta); } - // Perform linear interpolation on each dimension recursively until to get a single scalar - for (int32_t i = N - 1; i >= 0; --i) + // Compute the derivative along each axis + const VectorN ratio = deltaLeft.array().unaryExpr(std::ref(fade)); + if constexpr (isGradient) + { + const VectorN dratio = deltaLeft.array().unaryExpr(std::ref(derivativeFade)); + for (int32_t i = N - 1; i >= 0; --i) + { + for (uint32_t k = 0; k < (1U << i); k++) + { + VectorN & gradLeft = grads[k]; + const VectorN gradRight = grads[k | (1U << i)]; + gradLeft = lerp(ratio[i], gradLeft, gradRight); + } + } + for (int32_t j = 0; j < static_cast(N); ++j) + { + std::array interpOffsets_ = offsets; + for (int32_t i = N - 1; i >= 0; --i) + { + for (uint32_t k = 0; k < (1U << i); k++) + { + double & offsetLeft = interpOffsets_[k]; + const double offsetRight = interpOffsets_[k | (1U << i)]; + if (i == j) + { + offsetLeft = derivativeLerp(dratio[i], offsetLeft, offsetRight); + } + else + { + offsetLeft = lerp(ratio[i], offsetLeft, offsetRight); + } + } + } + grads[0][j] += interpOffsets_[0]; + } + return grads[0] / wavelength_; + } + else { - for (uint32_t k = 0; k < (1U << i); k++) + // Perform linear interpolation on each dimension recursively until to get a scalar + for (int32_t i = N - 1; i >= 0; --i) { - double & offsetLeft = offsets[k]; - const double offsetRight = offsets[k | (1U << i)]; - offsetLeft = lerp(ratio[i], offsetLeft, offsetRight); + for (uint32_t k = 0; k < (1U << i); k++) + { + double & offsetLeft = offsets[k]; + const double offsetRight = offsets[k | (1U << i)]; + offsetLeft = lerp(ratio[i], offsetLeft, offsetRight); + } } + return offsets[0]; } - return offsets[0]; + } + + template + double AbstractPerlinNoiseOctave::operator()(const VectorN & x) const + { + return evaluate(x); + } + + template + typename AbstractPerlinNoiseOctave::template VectorN + AbstractPerlinNoiseOctave::grad(const VectorN & x) const + { + return evaluate(x); } template @@ -478,6 +549,21 @@ namespace jiminy return value / amplitude_; } + template + typename AbstractPerlinProcess::template VectorN + AbstractPerlinProcess::grad(const VectorN & x) const + { + // Compute sum of octaves' values + VectorN value = VectorN::Zero(); + for (const auto & [octave, scale] : octaveScalePairs_) + { + value += scale * octave->grad(x); + } + + // Scale sum by maximum amplitude + return value / amplitude_; + } + template double AbstractPerlinProcess::getWavelength() const noexcept { diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 2ce6f70f8..80a132522 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -94,9 +94,17 @@ namespace jiminy::python template std::enable_if_t>...>, double> - evaluateRandomProcessUnpacked(AbstractPerlinProcess & process, Args... args) + evaluateRandomProcessUnpacked(AbstractPerlinProcess & fun, Args... args) { - return process(Eigen::Matrix{args...}); + return fun(Eigen::Matrix{args...}); + } + + template + std::enable_if_t>...>, + typename AbstractPerlinProcess::template VectorN> + gradRandomProcessUnpacked(AbstractPerlinProcess & fun, Args... args) + { + return fun.grad(Eigen::Matrix{args...}); } template @@ -106,6 +114,11 @@ namespace jiminy::python auto evaluateRandomProcessUnpackedSignature(std::index_sequence) -> double (*)(AbstractPerlinProcess &, type_t...); + template + auto gradRandomProcessUnpackedSignature(std::index_sequence) -> + typename AbstractPerlinProcess::template VectorN (*)( + AbstractPerlinProcess &, type_t...); + template constexpr void exposeRandomProcess() { @@ -117,6 +130,10 @@ namespace jiminy::python static_cast{}))>(evaluateRandomProcessUnpacked)) .def("__call__", &AbstractPerlinProcess::operator(), (bp::arg("self"), "vec")) + .def("grad", + static_cast{}))>(gradRandomProcessUnpacked)) + .def("grad", &AbstractPerlinProcess::grad, (bp::arg("self"), "vec")) .def( "reset", makeFunction(ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), From d53085d4714aa3f0168c69c441392e2ab884f9a9 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Jul 2024 15:48:30 +0200 Subject: [PATCH 5/8] [core] Make perlin processes faster and copy-able. --- core/include/jiminy/core/fwd.h | 3 +- core/include/jiminy/core/stepper/lie_group.h | 10 +- core/include/jiminy/core/utilities/random.h | 54 ++++---- core/include/jiminy/core/utilities/random.hxx | 118 ++++++++++-------- core/src/utilities/random.cc | 2 +- .../include/jiminy/python/utilities.h | 3 +- python/jiminy_pywrap/src/generators.cc | 117 +++++++++-------- 7 files changed, 165 insertions(+), 142 deletions(-) diff --git a/core/include/jiminy/core/fwd.h b/core/include/jiminy/core/fwd.h index d7fd282fd..f60675f2c 100644 --- a/core/include/jiminy/core/fwd.h +++ b/core/include/jiminy/core/fwd.h @@ -194,7 +194,8 @@ namespace jiminy ResultType max_ = std::numeric_limits::max()> class uniform_random_bit_generator_ref; - // Ground profile functors + // Ground profile functors. + // FIXME: use `std::move_only_function` instead of `std::function` when moving to C++23 using HeightmapFunction = std::function /* normal */)>; diff --git a/core/include/jiminy/core/stepper/lie_group.h b/core/include/jiminy/core/stepper/lie_group.h index 4342f3d3f..f129a15b6 100644 --- a/core/include/jiminy/core/stepper/lie_group.h +++ b/core/include/jiminy/core/stepper/lie_group.h @@ -1238,7 +1238,7 @@ namespace Eigen #define StateDerivative_SHARED_ADDON \ template::ValueType>::value, \ void>> \ @@ -1268,7 +1268,7 @@ namespace Eigen template< \ typename Derived, \ typename OtherDerived, \ - typename = typename std::enable_if_t< \ + typename = std::enable_if_t< \ is_base_of_template_v::ValueType>::value && \ is_base_of_template_v::ValueType>::value, \ void>> \ @@ -1301,7 +1301,7 @@ namespace Eigen } \ \ template::ValueType>::value, \ void>> \ @@ -1320,7 +1320,7 @@ namespace Eigen template< \ typename Derived, \ typename OtherDerived, \ - typename = typename std::enable_if_t< \ + typename = std::enable_if_t< \ is_base_of_template_v::ValueType>::value && \ is_base_of_template_v::min() == min_ && std::decay_t::max() == max_>::value> > constexpr uniform_random_bit_generator_ref(F && f) noexcept : @@ -316,9 +316,6 @@ namespace jiminy class JIMINY_DLLAPI PeriodicTabularProcess { - public: - JIMINY_DISABLE_COPY(PeriodicTabularProcess) - public: explicit PeriodicTabularProcess(double wavelength, double period); @@ -415,7 +412,7 @@ namespace jiminy uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept; - template + template class DerivedPerlinNoiseOctave, unsigned int N> class JIMINY_DLLAPI AbstractPerlinNoiseOctave { public: @@ -424,18 +421,14 @@ namespace jiminy public: explicit AbstractPerlinNoiseOctave(double wavelength); - virtual ~AbstractPerlinNoiseOctave() = default; - virtual void reset(const uniform_random_bit_generator_ref & g) noexcept; + void reset(const uniform_random_bit_generator_ref & g) noexcept; double operator()(const VectorN & x) const; VectorN grad(const VectorN & x) const; double getWavelength() const noexcept; - protected: - virtual VectorN grad(const VectorN & knot) const noexcept = 0; - private: template std::conditional_t, double> @@ -448,42 +441,46 @@ namespace jiminy }; template - class JIMINY_DLLAPI RandomPerlinNoiseOctave : public AbstractPerlinNoiseOctave + class JIMINY_DLLAPI RandomPerlinNoiseOctave : + public AbstractPerlinNoiseOctave { public: template using VectorN = Eigen::Matrix; + friend class AbstractPerlinNoiseOctave; + public: explicit RandomPerlinNoiseOctave(double wavelength); - ~RandomPerlinNoiseOctave() override = default; - void reset(const uniform_random_bit_generator_ref & g) noexcept override; + void reset(const uniform_random_bit_generator_ref & g) noexcept; protected: - VectorN grad(const VectorN & knot) const noexcept override; + VectorN gradKnot(const VectorN & knot) const noexcept; private: uint32_t seed_{0U}; }; template - class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : public AbstractPerlinNoiseOctave + class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : + public AbstractPerlinNoiseOctave { public: template using VectorN = Eigen::Matrix; + friend class AbstractPerlinNoiseOctave; + public: explicit PeriodicPerlinNoiseOctave(double wavelength, double period); - ~PeriodicPerlinNoiseOctave() override = default; - void reset(const uniform_random_bit_generator_ref & g) noexcept override; + void reset(const uniform_random_bit_generator_ref & g) noexcept; double getPeriod() const noexcept; protected: - VectorN grad(const VectorN & knot) const noexcept override; + VectorN gradKnot(const VectorN & knot) const noexcept; private: const double period_; @@ -513,17 +510,21 @@ namespace jiminy /// https://github.com/bradykieffer/SimplexNoise/blob/master/simplexnoise/noise.py /// https://github.com/sol-prog/Perlin_Noise/blob/master/PerlinNoise.cpp /// https://github.com/ashima/webgl-noise/blob/master/src/classicnoise2D.glsl - template - class JIMINY_DLLAPI AbstractPerlinProcess + template class DerivedPerlinNoiseOctave, + unsigned int N, + typename = std::enable_if_t< + std::is_base_of_v, + DerivedPerlinNoiseOctave>>> + class AbstractPerlinProcess; + + template class DerivedPerlinNoiseOctave, unsigned int N> + class JIMINY_DLLAPI AbstractPerlinProcess { public: - JIMINY_DISABLE_COPY(AbstractPerlinProcess) - template using VectorN = Eigen::Matrix; - using OctaveScalePair = - std::pair>, const double>; + using OctaveScalePair = std::pair, const double>; public: void reset(const uniform_random_bit_generator_ref & g) noexcept; @@ -545,14 +546,15 @@ namespace jiminy }; template - class JIMINY_DLLAPI RandomPerlinProcess : public AbstractPerlinProcess + class JIMINY_DLLAPI RandomPerlinProcess : + public AbstractPerlinProcess { public: explicit RandomPerlinProcess(double wavelength, std::size_t numOctaves = 6U); }; template - class PeriodicPerlinProcess : public AbstractPerlinProcess + class PeriodicPerlinProcess : public AbstractPerlinProcess { public: explicit PeriodicPerlinProcess( diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index 59e4867a9..0f5093b8d 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -224,8 +224,9 @@ namespace jiminy return dratio * (yRight - yLeft); } - template - AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) : + template class DerivedPerlinNoiseOctave, unsigned int N> + AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave( + double wavelength) : wavelength_{wavelength} { if (wavelength_ <= 0.0) @@ -236,26 +237,28 @@ namespace jiminy shift_ = uniform(N, 1, gen).cast(); } - template - void AbstractPerlinNoiseOctave::reset( + template class DerivedPerlinNoiseOctave, unsigned int N> + void AbstractPerlinNoiseOctave::reset( const uniform_random_bit_generator_ref & g) noexcept { // Sample random phase shift shift_ = uniform(N, 1, g).cast(); } - template - double AbstractPerlinNoiseOctave::getWavelength() const noexcept + template class DerivedPerlinNoiseOctave, unsigned int N> + double AbstractPerlinNoiseOctave::getWavelength() const noexcept { return wavelength_; } - template + template class DerivedPerlinNoiseOctave, unsigned int N> template - std::conditional_t::template VectorN, - double> - AbstractPerlinNoiseOctave::evaluate(const VectorN & x) const + std::conditional_t< + isGradient, + typename AbstractPerlinNoiseOctave::template VectorN, + double> + AbstractPerlinNoiseOctave::evaluate( + const VectorN & x) const { // Get current phase const VectorN phase = x / wavelength_ + shift_; @@ -291,7 +294,7 @@ namespace jiminy } // Evaluate the gradient at knot - grads[k] = grad(knot); + grads[k] = static_cast &>(*this).gradKnot(knot); // Compute the offset at query point offsets[k] = grads[k].dot(delta); @@ -350,22 +353,23 @@ namespace jiminy } } - template - double AbstractPerlinNoiseOctave::operator()(const VectorN & x) const + template class DerivedPerlinNoiseOctave, unsigned int N> + double AbstractPerlinNoiseOctave::operator()( + const VectorN & x) const { return evaluate(x); } - template - typename AbstractPerlinNoiseOctave::template VectorN - AbstractPerlinNoiseOctave::grad(const VectorN & x) const + template class DerivedPerlinNoiseOctave, unsigned int N> + typename AbstractPerlinNoiseOctave::template VectorN + AbstractPerlinNoiseOctave::grad(const VectorN & x) const { return evaluate(x); } template RandomPerlinNoiseOctave::RandomPerlinNoiseOctave(double wavelength) : - AbstractPerlinNoiseOctave(wavelength) + AbstractPerlinNoiseOctave(wavelength) { seed_ = std::random_device{}(); } @@ -375,7 +379,7 @@ namespace jiminy const uniform_random_bit_generator_ref & g) noexcept { // Call base implementation - AbstractPerlinNoiseOctave::reset(g); + AbstractPerlinNoiseOctave::reset(g); // Sample new random seed for MurmurHash seed_ = g(); @@ -383,7 +387,7 @@ namespace jiminy template typename RandomPerlinNoiseOctave::template VectorN - RandomPerlinNoiseOctave::grad(const VectorN & knot) const noexcept + RandomPerlinNoiseOctave::gradKnot(const VectorN & knot) const noexcept { constexpr float fHashMax = static_cast(std::numeric_limits::max()); @@ -443,7 +447,8 @@ namespace jiminy template PeriodicPerlinNoiseOctave::PeriodicPerlinNoiseOctave(double wavelength, double period) : - AbstractPerlinNoiseOctave(period / std::max(std::round(period / wavelength), 1.0)), + AbstractPerlinNoiseOctave( + period / std::max(std::round(period / wavelength), 1.0)), period_{period} { // Make sure the period is larger than the wavelength @@ -461,7 +466,7 @@ namespace jiminy const uniform_random_bit_generator_ref & g) noexcept { // Call base implementation - AbstractPerlinNoiseOctave::reset(g); + AbstractPerlinNoiseOctave::reset(g); // Re-initialize the pre-computed hash table std::generate( @@ -490,7 +495,7 @@ namespace jiminy template typename PeriodicPerlinNoiseOctave::template VectorN - PeriodicPerlinNoiseOctave::grad(const VectorN & knot) const noexcept + PeriodicPerlinNoiseOctave::gradKnot(const VectorN & knot) const noexcept { // Wrap knot is period interval int32_t index = 0; @@ -508,8 +513,8 @@ namespace jiminy return grads_[index]; } - template - AbstractPerlinProcess::AbstractPerlinProcess( + template class DerivedPerlinNoiseOctave, unsigned int N> + AbstractPerlinProcess::AbstractPerlinProcess( std::vector && octaveScalePairs) noexcept : octaveScalePairs_(std::move(octaveScalePairs)) { @@ -523,72 +528,74 @@ namespace jiminy amplitude_ = std::sqrt(amplitudeSquared); } - template - void - AbstractPerlinProcess::reset(const uniform_random_bit_generator_ref & g) noexcept + template class DerivedPerlinNoiseOctave, unsigned int N> + void AbstractPerlinProcess::reset( + const uniform_random_bit_generator_ref & g) noexcept { // Reset octaves for (OctaveScalePair & octaveScale : octaveScalePairs_) { // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - std::get<0>(octaveScale)->reset(g); + std::get<0>(octaveScale).reset(g); } } - template - double AbstractPerlinProcess::operator()(const VectorN & x) const + template class DerivedPerlinNoiseOctave, unsigned int N> + double + AbstractPerlinProcess::operator()(const VectorN & x) const { // Compute sum of octaves' values double value = 0.0; for (const auto & [octave, scale] : octaveScalePairs_) { - value += scale * (*octave)(x); + value += scale * octave(x); } // Scale sum by maximum amplitude return value / amplitude_; } - template - typename AbstractPerlinProcess::template VectorN - AbstractPerlinProcess::grad(const VectorN & x) const + template class DerivedPerlinNoiseOctave, unsigned int N> + typename AbstractPerlinProcess::template VectorN + AbstractPerlinProcess::grad(const VectorN & x) const { // Compute sum of octaves' values VectorN value = VectorN::Zero(); for (const auto & [octave, scale] : octaveScalePairs_) { - value += scale * octave->grad(x); + value += scale * octave.grad(x); } // Scale sum by maximum amplitude return value / amplitude_; } - template - double AbstractPerlinProcess::getWavelength() const noexcept + template class DerivedPerlinNoiseOctave, unsigned int N> + double AbstractPerlinProcess::getWavelength() const noexcept { double wavelength = INF; for (const OctaveScalePair & octaveScale : octaveScalePairs_) { // FIXME: replaced `std::get` by placeholder `_` when moving to C++26 (P2169R4) - wavelength = std::min(wavelength, std::get<0>(octaveScale)->getWavelength()); + wavelength = std::min(wavelength, std::get<0>(octaveScale).getWavelength()); } return wavelength; } - template - std::size_t AbstractPerlinProcess::getNumOctaves() const noexcept + template class DerivedPerlinNoiseOctave, unsigned int N> + std::size_t AbstractPerlinProcess::getNumOctaves() const noexcept { return octaveScalePairs_.size(); } - template - static std::vector::OctaveScalePair> buildPerlinNoiseOctaves( + template class DerivedPerlinNoiseOctave, unsigned int N> + static std::vector, const double>> + buildPerlinNoiseOctaves( double wavelength, std::size_t numOctaves, - std::function>(double)> factory) + std::function(double /* wavelength */)> factory) { - std::vector::OctaveScalePair> octaveScalePairs; + std::vector, const double>> octaveScalePairs; octaveScalePairs.reserve(numOctaves); double scale = 1.0; for (std::size_t i = 0; i < numOctaves; ++i) @@ -602,22 +609,23 @@ namespace jiminy template RandomPerlinProcess::RandomPerlinProcess(double wavelength, std::size_t numOctaves) : - AbstractPerlinProcess(buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [](double wavelengthIn) -> std::unique_ptr> - { return std::make_unique>(wavelengthIn); })) + AbstractPerlinProcess( + buildPerlinNoiseOctaves( + wavelength, + numOctaves, + [](double wavelengthIn) { return RandomPerlinNoiseOctave(wavelengthIn); })) { } template PeriodicPerlinProcess::PeriodicPerlinProcess( double wavelength, double period, std::size_t numOctaves) : - AbstractPerlinProcess(buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [period](double wavelengthIn) -> std::unique_ptr> - { return std::make_unique>(wavelengthIn, period); })), + AbstractPerlinProcess( + buildPerlinNoiseOctaves( + wavelength, + numOctaves, + [period](double wavelengthIn) + { return PeriodicPerlinNoiseOctave(wavelengthIn, period); })), period_{period} { } diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index d44dd3bc6..81d4e578e 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -480,7 +480,7 @@ namespace jiminy double orientation, uint32_t seed) { - if ((0.01 < interpDelta.array()).any() || (interpDelta.array() > size.array() / 2.0).any()) + if ((0.01 > interpDelta.array()).any() || (interpDelta.array() > size.array() / 2.0).any()) { JIMINY_WARNING( "All components of 'interpDelta' must be in range [0.01, 'size'/2.0]. Value: ", diff --git a/python/jiminy_pywrap/include/jiminy/python/utilities.h b/python/jiminy_pywrap/include/jiminy/python/utilities.h index de6b4f812..e669a9c2f 100644 --- a/python/jiminy_pywrap/include/jiminy/python/utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/utilities.h @@ -812,8 +812,7 @@ namespace jiminy::python struct result_converter { template || - is_eigen_ref_v>> + typename = std::enable_if_t || is_eigen_ref_v>> struct apply { struct type diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 80a132522..22e6b23a7 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -92,17 +92,17 @@ namespace jiminy::python #undef GENERIC_DISTRIBUTION_WRAPPER - template + template std::enable_if_t>...>, double> - evaluateRandomProcessUnpacked(AbstractPerlinProcess & fun, Args... args) + evaluatePerlinProcessUnpacked(DerivedPerlinProcess & fun, Args... args) { return fun(Eigen::Matrix{args...}); } - template + template std::enable_if_t>...>, - typename AbstractPerlinProcess::template VectorN> - gradRandomProcessUnpacked(AbstractPerlinProcess & fun, Args... args) + typename DerivedPerlinProcess::template VectorN> + gradPerlinProcessUnpacked(DerivedPerlinProcess & fun, Args... args) { return fun.grad(Eigen::Matrix{args...}); } @@ -110,55 +110,68 @@ namespace jiminy::python template using type_t = T; - template - auto evaluateRandomProcessUnpackedSignature(std::index_sequence) - -> double (*)(AbstractPerlinProcess &, type_t...); + template + auto evaluatePerlinProcessUnpackedSignature( + std::index_sequence) -> double (*)(DerivedPerlinProcess &, type_t...); - template - auto gradRandomProcessUnpackedSignature(std::index_sequence) -> - typename AbstractPerlinProcess::template VectorN (*)( - AbstractPerlinProcess &, type_t...); + template + auto gradPerlinProcessUnpackedSignature(std::index_sequence) -> + typename DerivedPerlinProcess::template VectorN (*)(DerivedPerlinProcess &, + type_t...); template - constexpr void exposeRandomProcess() + struct PyPerlinProcessVisitor : public bp::def_visitor> { - bp::class_, - std::shared_ptr>, - boost::noncopyable>(toString("AbstractPerlinProcess", N, "D").c_str(), - bp::no_init) - .def("__call__", - static_cast{}))>(evaluateRandomProcessUnpacked)) - .def("__call__", &AbstractPerlinProcess::operator(), (bp::arg("self"), "vec")) - .def("grad", - static_cast{}))>(gradRandomProcessUnpacked)) - .def("grad", &AbstractPerlinProcess::grad, (bp::arg("self"), "vec")) - .def( - "reset", - makeFunction(ConvertGeneratorFromPythonAndInvoke(&AbstractPerlinProcess::reset), - bp::default_call_policies(), - (bp::arg("self"), "generator"))) - .ADD_PROPERTY_GET("wavelength", &AbstractPerlinProcess::getWavelength) - .ADD_PROPERTY_GET("num_octaves", &AbstractPerlinProcess::getNumOctaves); - - bp::class_, - bp::bases>, - std::shared_ptr>, - boost::noncopyable>( - toString("RandomPerlinProcess", N, "D").c_str(), - bp::init( - (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))); + public: + template + static void visit(PyClass & cl) + { + using DerivedPerlinProcess = typename PyClass::wrapped_type; - bp::class_, - bp::bases>, - std::shared_ptr>, - boost::noncopyable>( - toString("PeriodicPerlinProcess", N, "D").c_str(), - bp::init( - (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) - .ADD_PROPERTY_GET("period", &PeriodicPerlinProcess::getPeriod); - } + // clang-format off + cl + .def("__call__", + static_cast( + std::make_index_sequence{}))>(evaluatePerlinProcessUnpacked)) + .def("__call__", &DerivedPerlinProcess::operator(), (bp::arg("self"), "vec")) + .def("grad", + static_cast( + std::make_index_sequence{}))>(gradPerlinProcessUnpacked)) + .def("grad", &DerivedPerlinProcess::grad, (bp::arg("self"), "vec")) + .def( + "reset", + makeFunction(ConvertGeneratorFromPythonAndInvoke< + void(const uniform_random_bit_generator_ref &), DerivedPerlinProcess + >(&DerivedPerlinProcess::reset), + bp::default_call_policies(), + (bp::arg("self"), "generator"))) + .ADD_PROPERTY_GET("wavelength", &DerivedPerlinProcess::getWavelength) + .ADD_PROPERTY_GET("num_octaves", &DerivedPerlinProcess::getNumOctaves); + // clang-format on + } + + static void expose() + { + bp::class_, + // bp::bases>, + std::shared_ptr>, + boost::noncopyable>( + toString("RandomPerlinProcess", N, "D").c_str(), + bp::init( + (bp::arg("self"), "wavelength", bp::arg("num_octaves") = 6U))) + .def(PyPerlinProcessVisitor()); + + bp::class_, + // bp::bases>, + std::shared_ptr>, + boost::noncopyable>( + toString("PeriodicPerlinProcess", N, "D").c_str(), + bp::init( + (bp::arg("self"), "wavelength", "period", bp::arg("num_octaves") = 6U))) + .ADD_PROPERTY_GET("period", &PeriodicPerlinProcess::getPeriod) + .def(PyPerlinProcessVisitor()); + } + }; void exposeGenerators() { @@ -232,9 +245,9 @@ namespace jiminy::python /* FIXME: Use template lambda and compile-time for-loop when moving to c++20. For reference: https://stackoverflow.com/a/76272348/4820605 */ - exposeRandomProcess<1>(); - exposeRandomProcess<2>(); - exposeRandomProcess<3>(); + PyPerlinProcessVisitor<1>::expose(); + PyPerlinProcessVisitor<2>::expose(); + PyPerlinProcessVisitor<3>::expose(); bp::def( "random_tile_ground", From d1dab34a9a3baf312a64ff71232ea066b2ee3670 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Jul 2024 16:09:51 +0200 Subject: [PATCH 6/8] [core] Add Perlin ground generators. --- core/include/jiminy/core/robot/model.h | 2 + core/include/jiminy/core/utilities/geometry.h | 15 ++++ core/include/jiminy/core/utilities/random.h | 2 +- core/include/jiminy/core/utilities/random.hxx | 2 +- core/src/utilities/geometry.cc | 86 +++++++++++++++++++ core/src/utilities/random.cc | 7 +- python/jiminy_pywrap/src/generators.cc | 12 +++ 7 files changed, 121 insertions(+), 5 deletions(-) diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index ddb9e1348..97cea4cfe 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -1,6 +1,8 @@ #ifndef JIMINY_MODEL_H #define JIMINY_MODEL_H +#include + #include "pinocchio/spatial/fwd.hpp" // `pinocchio::SE3` #include "pinocchio/multibody/model.hpp" // `pinocchio::Model` #include "pinocchio/multibody/data.hpp" // `pinocchio::Data` diff --git a/core/include/jiminy/core/utilities/geometry.h b/core/include/jiminy/core/utilities/geometry.h index 310a34528..d40576fab 100644 --- a/core/include/jiminy/core/utilities/geometry.h +++ b/core/include/jiminy/core/utilities/geometry.h @@ -48,6 +48,21 @@ namespace jiminy /// \param[in] orientation Orientation of the staircases in the XY plane. HeightmapFunction JIMINY_DLLAPI periodicStairs( double stepWidth, double stepHeight, uint32_t stepNumber, double orientation); + + HeightmapFunction JIMINY_DLLAPI unidirectionalRandomPerlinGround( + double wavelength, std::size_t numOctaves, double orientation, uint32_t seed); + + HeightmapFunction JIMINY_DLLAPI unidirectionalPeriodicPerlinGround(double wavelength, + double period, + std::size_t numOctaves, + double orientation, + uint32_t seed); + + HeightmapFunction JIMINY_DLLAPI randomPerlinGround( + double wavelength, std::size_t numOctaves, uint32_t seed); + + HeightmapFunction JIMINY_DLLAPI periodicPerlinGround( + double wavelength, double period, std::size_t numOctaves, uint32_t seed); } #endif // JIMINY_GEOMETRY_H \ No newline at end of file diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 9e55bd004..05c5fd616 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -331,7 +331,7 @@ namespace jiminy const double wavelength_; const double period_; const Eigen::Index numTimes_{static_cast(std::ceil(period_ / (0.1 * wavelength_)))}; - const double dt_{period_ / numTimes_}; + const double dt_{period_ / static_cast(numTimes_)}; Eigen::VectorXd values_{numTimes_}; Eigen::VectorXd grads_{numTimes_}; diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index 0f5093b8d..b18780672 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -418,7 +418,7 @@ namespace jiminy { // Generate a uniformly distributed random vector on n-sphere VectorN dir; - for (uint_fast8_t i = 0; i < N; i += 2) + for (uint32_t i = 0; i < N; i += 2) { // Generate 2 uniformly distributed random variables const float u1 = static_cast(hash) / fHashMax; diff --git a/core/src/utilities/geometry.cc b/core/src/utilities/geometry.cc index fee7bd87c..4ff39f270 100644 --- a/core/src/utilities/geometry.cc +++ b/core/src/utilities/geometry.cc @@ -1,7 +1,9 @@ #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" +#include "jiminy/core/utilities/random.h" namespace jiminy @@ -803,4 +805,88 @@ namespace jiminy } }; } + + template class AnyPerlinProcess> + static HeightmapFunction generateHeightmapFromPerlinProcess1D(AnyPerlinProcess<1> && fun, + double orientation) + { + // Define the projection axis + const Eigen::Vector2d axis{std::cos(orientation), std::sin(orientation)}; + + auto tmp = [fun = std::move(fun), axis](const Eigen::Vector2d & pos, + double & height, + Eigen::Ref normal) mutable + { + // Compute the position along axis + const Vector1 posAxis = Vector1{axis.dot(pos)}; + + // Compute the height + height = fun(posAxis); + + // Compute the gradient of the Perlin Proces + const double slope = fun.grad(posAxis)[0]; + + // Compute the inverse of the normal's Euclidean norm + const double normInv = 1.0 / std::sqrt(1.0 + std::pow(slope, 2)); + + // Update normal vector + normal << -slope * normInv * axis, normInv; + }; + return tmp; + } + + template class AnyPerlinProcess> + static HeightmapFunction generateHeightmapFromPerlinProcess2D(AnyPerlinProcess<2> && fun) + { + return [fun = std::move(fun)](const Eigen::Vector2d & pos, + double & height, + Eigen::Ref normal) mutable + { + // Compute the height + height = fun(pos); + + // Compute the gradient of the Perlin Proces + const auto grad = fun.grad(pos); + + // Compute the inverse of the normal's Euclidean norm + const double normInv = 1.0 / grad.norm(); + + // Update normal vector + normal << -normInv * grad.template head<2>(), normInv; + }; + } + + HeightmapFunction unidirectionalRandomPerlinGround( + double wavelength, std::size_t numOctaves, double orientation, uint32_t seed) + { + auto fun = RandomPerlinProcess<1>(wavelength, numOctaves); + fun.reset(PCG32(seed)); + return generateHeightmapFromPerlinProcess1D(std::move(fun), orientation); + } + + HeightmapFunction randomPerlinGround(double wavelength, std::size_t numOctaves, uint32_t seed) + { + auto fun = RandomPerlinProcess<2>(wavelength, numOctaves); + fun.reset(PCG32(seed)); + return generateHeightmapFromPerlinProcess2D(std::move(fun)); + } + + HeightmapFunction periodicPerlinGround( + double wavelength, double period, std::size_t numOctaves, uint32_t seed) + { + auto fun = PeriodicPerlinProcess<2>(wavelength, period, numOctaves); + fun.reset(PCG32(seed)); + return generateHeightmapFromPerlinProcess2D(std::move(fun)); + } + + HeightmapFunction unidirectionalPeriodicPerlinGround(double wavelength, + double period, + std::size_t numOctaves, + double orientation, + uint32_t seed) + { + auto fun = PeriodicPerlinProcess<1>(wavelength, period, numOctaves); + fun.reset(PCG32(seed)); + return generateHeightmapFromPerlinProcess1D(std::move(fun), orientation); + } } \ No newline at end of file diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index 81d4e578e..9891489b8 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -253,7 +253,7 @@ namespace jiminy Eigen::Index indexRight = indexLeft + 1; // Compute the time ratio - const double ratio = quot - indexLeft; + const double ratio = quot - static_cast(indexLeft); return {indexLeft, indexRight, ratio}; } @@ -262,7 +262,7 @@ namespace jiminy double value, double delta, Eigen::Index numTimes) { // Wrap value in period interval - const double period = numTimes * delta; + const double period = static_cast(numTimes) * delta; value = std::fmod(value, period); if (value < 0.0) { @@ -403,7 +403,8 @@ namespace jiminy values_.noalias() += scale * cosMat_ * normalVec2; const auto diff = - 2 * M_PI / period_ * Eigen::VectorXd::LinSpaced(numHarmonics_, 1, numHarmonics_); + 2 * M_PI / period_ * Eigen::VectorXd::LinSpaced( + numHarmonics_, 1, static_cast(numHarmonics_)); grads_ = scale * cosMat_ * normalVec1.cwiseProduct(diff); grads_.noalias() -= scale * sinMat_ * normalVec2.cwiseProduct(diff); } diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index 22e6b23a7..eeac7c103 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -256,6 +256,18 @@ namespace jiminy::python bp::def("periodic_stairs_ground", &periodicStairs, (bp::arg("step_width"), "step_height", "step_number", "orientation")); + bp::def("unidirectional_random_perlin_ground", + &unidirectionalRandomPerlinGround, + (bp::arg("wavelength"), "num_octaves", "orientation", "seed")); + bp::def("unidirectional_periodic_perlin_ground", + &unidirectionalPeriodicPerlinGround, + (bp::arg("wavelength"), "period", "num_octaves", "orientation", "seed")); + bp::def("random_perlin_ground", + &randomPerlinGround, + (bp::arg("wavelength"), "num_octaves", "seed")); + bp::def("periodic_perlin_ground", + &periodicPerlinGround, + (bp::arg("wavelength"), "period", "num_octaves", "seed")); bp::def("sum_heightmaps", &sumHeightmaps, (bp::arg("heightmaps"))); bp::def("merge_heightmaps", &mergeHeightmaps, (bp::arg("heightmaps"))); From 8e5c07bcdec1e22ed23d54e909166c33bcb4e7c2 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Jul 2024 18:17:51 +0200 Subject: [PATCH 7/8] [core] Add Perlin processes unit tests. --- core/include/jiminy/core/utilities/random.h | 16 ++-- core/include/jiminy/core/utilities/random.hxx | 6 ++ core/unit/CMakeLists.txt | 1 + core/unit/random_test.cc | 95 +++++++++++++++++++ python/jiminy_pywrap/src/generators.cc | 4 +- 5 files changed, 112 insertions(+), 10 deletions(-) create mode 100644 core/unit/random_test.cc diff --git a/core/include/jiminy/core/utilities/random.h b/core/include/jiminy/core/utilities/random.h index 05c5fd616..af01b44f4 100644 --- a/core/include/jiminy/core/utilities/random.h +++ b/core/include/jiminy/core/utilities/random.h @@ -314,7 +314,7 @@ namespace jiminy standardToeplitzCholeskyLower(const Eigen::MatrixBase & coeffs, double reg = 0.0); } - class JIMINY_DLLAPI PeriodicTabularProcess + class JIMINY_TEMPLATE_DLLAPI PeriodicTabularProcess { public: explicit PeriodicTabularProcess(double wavelength, double period); @@ -337,7 +337,7 @@ namespace jiminy Eigen::VectorXd grads_{numTimes_}; }; - class JIMINY_DLLAPI PeriodicGaussianProcess final : public PeriodicTabularProcess + class JIMINY_TEMPLATE_DLLAPI PeriodicGaussianProcess final : public PeriodicTabularProcess { public: explicit PeriodicGaussianProcess(double wavelength, double period); @@ -385,7 +385,7 @@ namespace jiminy /// \see For references about the derivatives of a Gaussian Process: /// http://herbsusmann.com/2020/07/06/gaussian-process-derivatives /// https://arxiv.org/abs/1810.12283 - class JIMINY_DLLAPI PeriodicFourierProcess final : public PeriodicTabularProcess + class JIMINY_TEMPLATE_DLLAPI PeriodicFourierProcess final : public PeriodicTabularProcess { public: explicit PeriodicFourierProcess(double wavelength, double period); @@ -413,7 +413,7 @@ namespace jiminy uint32_t MurmurHash3(const void * key, int32_t len, uint32_t seed) noexcept; template class DerivedPerlinNoiseOctave, unsigned int N> - class JIMINY_DLLAPI AbstractPerlinNoiseOctave + class JIMINY_TEMPLATE_DLLAPI AbstractPerlinNoiseOctave { public: template @@ -441,7 +441,7 @@ namespace jiminy }; template - class JIMINY_DLLAPI RandomPerlinNoiseOctave : + class JIMINY_TEMPLATE_DLLAPI RandomPerlinNoiseOctave : public AbstractPerlinNoiseOctave { public: @@ -463,7 +463,7 @@ namespace jiminy }; template - class JIMINY_DLLAPI PeriodicPerlinNoiseOctave : + class JIMINY_TEMPLATE_DLLAPI PeriodicPerlinNoiseOctave : public AbstractPerlinNoiseOctave { public: @@ -518,7 +518,7 @@ namespace jiminy class AbstractPerlinProcess; template class DerivedPerlinNoiseOctave, unsigned int N> - class JIMINY_DLLAPI AbstractPerlinProcess + class JIMINY_TEMPLATE_DLLAPI AbstractPerlinProcess { public: template @@ -546,7 +546,7 @@ namespace jiminy }; template - class JIMINY_DLLAPI RandomPerlinProcess : + class JIMINY_TEMPLATE_DLLAPI RandomPerlinProcess : public AbstractPerlinProcess { public: diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index b18780672..bef9eabad 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -461,6 +461,12 @@ namespace jiminy reset(std::random_device{}); } + template + double PeriodicPerlinNoiseOctave::getPeriod() const noexcept + { + return period_; + } + template void PeriodicPerlinNoiseOctave::reset( const uniform_random_bit_generator_ref & g) noexcept diff --git a/core/unit/CMakeLists.txt b/core/unit/CMakeLists.txt index d68c0c0c0..9f33e99f3 100755 --- a/core/unit/CMakeLists.txt +++ b/core/unit/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(Threads) set(UNIT_TEST_FILES "${CMAKE_CURRENT_SOURCE_DIR}/engine_sanity_check.cc" "${CMAKE_CURRENT_SOURCE_DIR}/model_test.cc" + "${CMAKE_CURRENT_SOURCE_DIR}/random_test.cc" "${CMAKE_CURRENT_SOURCE_DIR}/miscellaneous.cc" ) diff --git a/core/unit/random_test.cc b/core/unit/random_test.cc new file mode 100644 index 000000000..8d5d244b9 --- /dev/null +++ b/core/unit/random_test.cc @@ -0,0 +1,95 @@ +#include + +#include "jiminy/core/utilities/random.h" + + +using namespace jiminy; + +static inline constexpr double DELTA{1e-6}; +static inline constexpr double TOL{1e-3}; + + +TEST(PerlinNoiseTest, RandomPerlinNoiseOctaveInitialization) +{ + double wavelength = 10.0; + RandomPerlinNoiseOctave<1> randomOctave(wavelength); + + EXPECT_DOUBLE_EQ(randomOctave.getWavelength(), wavelength); +} + +TEST(PerlinNoiseTest, PeriodicPerlinNoiseOctaveInitialization) +{ + double wavelength = 10.0; + double period = 20.0; + PeriodicPerlinNoiseOctave<1> periodicOctave(wavelength, period); + + EXPECT_DOUBLE_EQ(periodicOctave.getWavelength(), wavelength); + EXPECT_DOUBLE_EQ(periodicOctave.getPeriod(), period); +} + +TEST(PerlinNoiseTest, RandomGradientCalculation1D) +{ + { + double wavelength = 10.0; + RandomPerlinNoiseOctave<1> octave(wavelength); + + Eigen::Array t{5.43}; + Eigen::Matrix finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) / + (2 * DELTA)}; + ASSERT_TRUE(finiteDiffGrad.isApprox(octave.grad(t), TOL)); + } + { + double wavelength = 3.41; + RandomPerlinNoiseOctave<1> octave(wavelength); + + Eigen::Array t{17.0}; + Eigen::Matrix finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) / + (2 * DELTA)}; + ASSERT_TRUE(finiteDiffGrad.isApprox(octave.grad(t), TOL)); + } +} + +TEST(PerlinNoiseTest, PeriodicGradientCalculation1D) +{ + double wavelength = 10.0; + double period = 30.0; + PeriodicPerlinNoiseOctave<1> octave(wavelength, period); + + Eigen::Array t{5.43}; + Eigen::Matrix finiteDiffGrad{(octave(t + DELTA) - octave(t - DELTA)) / + (2 * DELTA)}; + ASSERT_TRUE(finiteDiffGrad.isApprox(octave.grad(t), TOL)); +} + + +TEST(PerlinNoiseTest, RandomGradientCalculation2D) +{ + double wavelength = 10.0; + RandomPerlinNoiseOctave<2> octave(wavelength); + + Eigen::Vector2d pos{5.43, 7.12}; + Eigen::Vector2d finiteDiffGrad{(octave(pos + DELTA * Eigen::Vector2d::UnitX()) - + octave(pos - DELTA * Eigen::Vector2d::UnitX())) / + (2 * DELTA), + (octave(pos + DELTA * Eigen::Vector2d::UnitY()) - + octave(pos - DELTA * Eigen::Vector2d::UnitY())) / + (2 * DELTA)}; + ASSERT_TRUE(finiteDiffGrad.isApprox(octave.grad(pos), TOL)); +} + + +TEST(PerlinNoiseTest, PeriodicGradientCalculation2D) +{ + double wavelength = 10.0; + double period = 30.0; + PeriodicPerlinNoiseOctave<2> octave(wavelength, period); + + Eigen::Vector2d pos{5.43, 7.12}; + Eigen::Vector2d finiteDiffGrad{(octave(pos + DELTA * Eigen::Vector2d::UnitX()) - + octave(pos - DELTA * Eigen::Vector2d::UnitX())) / + (2 * DELTA), + (octave(pos + DELTA * Eigen::Vector2d::UnitY()) - + octave(pos - DELTA * Eigen::Vector2d::UnitY())) / + (2 * DELTA)}; + ASSERT_TRUE(finiteDiffGrad.isApprox(octave.grad(pos), TOL)); +} diff --git a/python/jiminy_pywrap/src/generators.cc b/python/jiminy_pywrap/src/generators.cc index eeac7c103..9f7f4a39d 100644 --- a/python/jiminy_pywrap/src/generators.cc +++ b/python/jiminy_pywrap/src/generators.cc @@ -96,7 +96,7 @@ namespace jiminy::python std::enable_if_t>...>, double> evaluatePerlinProcessUnpacked(DerivedPerlinProcess & fun, Args... args) { - return fun(Eigen::Matrix{args...}); + return fun(Eigen::Matrix(sizeof...(Args)), 1>{args...}); } template @@ -104,7 +104,7 @@ namespace jiminy::python typename DerivedPerlinProcess::template VectorN> gradPerlinProcessUnpacked(DerivedPerlinProcess & fun, Args... args) { - return fun.grad(Eigen::Matrix{args...}); + return fun.grad(Eigen::Matrix(sizeof...(Args)), 1>{args...}); } template From 048f768297d362f5d55047e59ebf8f2afc30a3ff Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Jul 2024 23:11:31 +0200 Subject: [PATCH 8/8] [core] Clarify error message about wavelength of perlin noise being too small. --- core/include/jiminy/core/utilities/random.hxx | 38 +++++++++++-------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/core/include/jiminy/core/utilities/random.hxx b/core/include/jiminy/core/utilities/random.hxx index bef9eabad..e4a27c0f5 100644 --- a/core/include/jiminy/core/utilities/random.hxx +++ b/core/include/jiminy/core/utilities/random.hxx @@ -594,19 +594,34 @@ namespace jiminy return octaveScalePairs_.size(); } - template class DerivedPerlinNoiseOctave, unsigned int N> + template class DerivedPerlinNoiseOctave, + unsigned int N, + typename... ExtraArgs> static std::vector, const double>> - buildPerlinNoiseOctaves( - double wavelength, - std::size_t numOctaves, - std::function(double /* wavelength */)> factory) + buildPerlinNoiseOctaves(double wavelength, std::size_t numOctaves, ExtraArgs &&... args) { + if constexpr (std::is_base_of_v, DerivedPerlinNoiseOctave>) + { + const double period = std::get<0>(std::tuple{std::forward(args)...}); + const double wavelengthMax = + std::pow(PERLIN_NOISE_LACUNARITY, numOctaves) * wavelength; + if (period < wavelengthMax) + { + JIMINY_THROW(std::invalid_argument, + "'period' must be larger than (", + PERLIN_NOISE_LACUNARITY, + "^'numOctaves') * 'wavelength' (ie. ", + wavelengthMax, + ")."); + } + } + std::vector, const double>> octaveScalePairs; octaveScalePairs.reserve(numOctaves); double scale = 1.0; for (std::size_t i = 0; i < numOctaves; ++i) { - octaveScalePairs.emplace_back(factory(wavelength), scale); + octaveScalePairs.emplace_back(DerivedPerlinNoiseOctave(wavelength, args...), scale); wavelength *= PERLIN_NOISE_LACUNARITY; scale *= PERLIN_NOISE_PERSISTENCE; } @@ -616,10 +631,7 @@ namespace jiminy template RandomPerlinProcess::RandomPerlinProcess(double wavelength, std::size_t numOctaves) : AbstractPerlinProcess( - buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [](double wavelengthIn) { return RandomPerlinNoiseOctave(wavelengthIn); })) + buildPerlinNoiseOctaves(wavelength, numOctaves)) { } @@ -627,11 +639,7 @@ namespace jiminy PeriodicPerlinProcess::PeriodicPerlinProcess( double wavelength, double period, std::size_t numOctaves) : AbstractPerlinProcess( - buildPerlinNoiseOctaves( - wavelength, - numOctaves, - [period](double wavelengthIn) - { return PeriodicPerlinNoiseOctave(wavelengthIn, period); })), + buildPerlinNoiseOctaves(wavelength, numOctaves, period)), period_{period} { }