Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Using std types instead of boost for Gaussian sampling #2351

Merged
merged 5 commits into from
Oct 19, 2023
+6 −11
Merged

Using std types instead of boost for Gaussian sampling #2351

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
52b96a8
*Changed boost namespace to std
Shobuj-Paul Sep 10, 2023
de59c47
calling Distribution(Engine) directly
Shobuj-Paul Sep 17, 2023
6b2ac7c
cleanup
Shobuj-Paul Sep 17, 2023
1cbb59e
Seed mersenne twister variable with random device
Shobuj-Paul Oct 11, 2023
48fa1ef
Updated with rsl random library
Shobuj-Paul Oct 19, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions moveit_planners/stomp/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand All @@ -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)

Expand Down
14 changes: 3 additions & 11 deletions moveit_planners/stomp/include/stomp_moveit/math/multivariate_gaussian.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,11 +42,7 @@
#include <Eigen/Core>
#include <Eigen/Cholesky>

// TODO(#2166): Replace with std types
#include <boost/random/variate_generator.hpp>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/shared_ptr.hpp>
#include <rsl/random.hpp>
#include <cstdlib>

namespace stomp_moveit
Expand Down Expand Up @@ -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<boost::variate_generator<boost::mt19937, boost::normal_distribution<> > > gaussian_;
std::normal_distribution<double> normal_dist_;
};

//////////////////////// template function definitions follow //////////////////////////////
Expand All @@ -91,16 +85,14 @@ MultivariateGaussian::MultivariateGaussian(const Eigen::MatrixBase<Derived1>& me
const Eigen::MatrixBase<Derived2>& 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<boost::mt19937, boost::normal_distribution<> >(rng_, normal_dist_));
}

template <typename Derived>
void MultivariateGaussian::sample(Eigen::MatrixBase<Derived>& output, bool use_covariance)
{
for (int i = 0; i < size_; ++i)
output(i) = (*gaussian_)();
output(i) = normal_dist_(rsl::rng());

if (use_covariance)
{
Expand Down
1 change: 1 addition & 0 deletions moveit_planners/stomp/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
<depend>std_msgs</depend>
<depend>tf2_eigen</depend>
<depend>visualization_msgs</depend>
<depend>rsl</depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
Expand Down
Loading