From 3ba9d9d7225d9f4d47329d5584c825e54d8cb1aa Mon Sep 17 00:00:00 2001 From: Shobuj Paul <72087882+Shobuj-Paul@users.noreply.github.com> Date: Thu, 19 Oct 2023 19:53:59 +0530 Subject: [PATCH] Using std types instead of boost for Gaussian sampling (#2351) * *Changed boost namespace to std *Need to compare function implementations *Find an equivalent implementation for variate_generator * calling Distribution(Engine) directly * cleanup * Seed mersenne twister variable with random device * Updated with rsl random library --- moveit_planners/stomp/CMakeLists.txt | 2 ++ .../stomp_moveit/math/multivariate_gaussian.hpp | 14 +++----------- moveit_planners/stomp/package.xml | 1 + 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/moveit_planners/stomp/CMakeLists.txt b/moveit_planners/stomp/CMakeLists.txt index e91a6607e8..ef600cf98d 100644 --- a/moveit_planners/stomp/CMakeLists.txt +++ b/moveit_planners/stomp/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(std_msgs REQUIRED) find_package(stomp REQUIRED) find_package(visualization_msgs REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(rsl REQUIRED) generate_parameter_library(stomp_moveit_parameters res/stomp_moveit.yaml) @@ -29,6 +30,7 @@ ament_target_dependencies(stomp_moveit_plugin std_msgs tf2_eigen visualization_msgs + rsl ) target_link_libraries(stomp_moveit_plugin stomp::stomp stomp_moveit_parameters) diff --git a/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.hpp b/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.hpp index 4011f3c647..0eee0348ea 100644 --- a/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.hpp @@ -42,11 +42,7 @@ #include #include -// TODO(#2166): Replace with std types -#include -#include -#include -#include +#include #include namespace stomp_moveit @@ -79,9 +75,7 @@ class MultivariateGaussian Eigen::MatrixXd covariance_cholesky_; /**< Cholesky decomposition (LL^T) of the covariance */ int size_; - boost::mt19937 rng_; - boost::normal_distribution<> normal_dist_; - std::shared_ptr > > gaussian_; + std::normal_distribution normal_dist_; }; //////////////////////// template function definitions follow ////////////////////////////// @@ -91,16 +85,14 @@ MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase& me const Eigen::MatrixBase& covariance) : mean_(mean), covariance_(covariance), covariance_cholesky_(covariance_.llt().matrixL()), normal_dist_(0.0, 1.0) { - rng_.seed(rand()); size_ = mean.rows(); - gaussian_.reset(new boost::variate_generator >(rng_, normal_dist_)); } template void MultivariateGaussian::sample(Eigen::MatrixBase& output, bool use_covariance) { for (int i = 0; i < size_; ++i) - output(i) = (*gaussian_)(); + output(i) = normal_dist_(rsl::rng()); if (use_covariance) { diff --git a/moveit_planners/stomp/package.xml b/moveit_planners/stomp/package.xml index b2dbde9e13..9d9082971b 100644 --- a/moveit_planners/stomp/package.xml +++ b/moveit_planners/stomp/package.xml @@ -21,6 +21,7 @@ std_msgs tf2_eigen visualization_msgs + rsl ament_lint_auto ament_lint_common