moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
noise_generators.hpp
Go to the documentation of this file.
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2023, PickNik Inc.
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of PickNik Inc. nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34
40#pragma once
41
42#include <stomp_moveit/stomp_moveit_task.hpp> // Function definitions
44#include <Eigen/Geometry>
45
46namespace stomp_moveit
47{
48namespace noise
49{
58NoiseGeneratorFn getNormalDistributionGenerator(size_t num_timesteps, const std::vector<double>& stddev)
59{
60 // Five-point stencil constants
61 static const std::vector<double> ACC_MATRIX_DIAGONAL_VALUES = { -1.0 / 12.0, 16.0 / 12.0, -30.0 / 12.0, 16.0 / 12.0,
62 -1.0 / 12.0 };
63 static const std::vector<int> ACC_MATRIX_DIAGONAL_INDICES = { -2, -1, 0, 1, 2 };
64
65 auto fill_diagonal = [](Eigen::MatrixXd& m, double coeff, int diag_index) {
66 std::size_t size = m.rows() - std::abs(diag_index);
67 m.diagonal(diag_index) = Eigen::VectorXd::Constant(size, coeff);
68 };
69
70 // creating finite difference acceleration matrix
71 Eigen::MatrixXd acceleration = Eigen::MatrixXd::Zero(num_timesteps, num_timesteps);
72 for (auto i = 0u; i < ACC_MATRIX_DIAGONAL_INDICES.size(); i++)
73 {
74 fill_diagonal(acceleration, ACC_MATRIX_DIAGONAL_VALUES[i], ACC_MATRIX_DIAGONAL_INDICES[i]);
75 }
76
77 // create and scale covariance matrix
78 Eigen::MatrixXd covariance = Eigen::MatrixXd::Identity(num_timesteps, num_timesteps);
79 covariance = acceleration.transpose() * acceleration;
80 covariance = covariance.fullPivLu().inverse();
81 covariance /= covariance.array().abs().matrix().maxCoeff();
82
83 // create random generators
84 std::vector<math::MultivariateGaussianPtr> rand_generators(stddev.size());
85 for (auto& r : rand_generators)
86 {
87 r = std::make_shared<math::MultivariateGaussian>(Eigen::VectorXd::Zero(num_timesteps), covariance);
88 }
89
90 auto raw_noise = std::make_shared<Eigen::VectorXd>(num_timesteps);
91 NoiseGeneratorFn noise_generator_fn = [=](const Eigen::MatrixXd& values, Eigen::MatrixXd& noisy_values,
92 Eigen::MatrixXd& noise) {
93 for (int i = 0; i < values.rows(); ++i)
94 {
95 rand_generators[i]->sample(*raw_noise);
96 raw_noise->head(1).setZero();
97 raw_noise->tail(1).setZero(); // zeroing out the start and end noise values
98 noise.row(i).transpose() = stddev.at(i) * (*raw_noise);
99 noisy_values.row(i) = values.row(i) + noise.row(i);
100 }
101 return true;
102 };
103 return noise_generator_fn;
104}
105} // namespace noise
106} // namespace stomp_moveit
Implementation of a multi-variate Gaussian used for randomizing path waypoints.
NoiseGeneratorFn getNormalDistributionGenerator(size_t num_timesteps, const std::vector< double > &stddev)
std::function< bool(const Eigen::MatrixXd &values, Eigen::MatrixXd &noisy_values, Eigen::MatrixXd &noise)> NoiseGeneratorFn
A STOMP task definition that allows injecting custom functions for planning.