moveit2
The MoveIt Motion Planning Framework for ROS 2.
Loading...
Searching...
No Matches
test_noise_generator.cpp
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
39#include <gtest/gtest.h>
41
42constexpr size_t TIMESTEPS = 100;
43constexpr size_t VARIABLES = 6;
44static const std::vector<double> STDDEV(VARIABLES, 0.2);
45static const Eigen::MatrixXd VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 1.0);
46static const Eigen::MatrixXd NOISY_VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
47static const Eigen::MatrixXd NOISE = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
48
49TEST(NoiseGeneratorTest, testStartEndUnchanged)
50{
52
53 auto noise = NOISE;
54 auto noisy_values = NOISY_VALUES;
55
56 noise_gen(VALUES, noisy_values, noise);
57
58 // Test that noise creates output unlike the input
59 EXPECT_NE(noise, NOISE);
60 EXPECT_NE(noisy_values, NOISY_VALUES);
61 EXPECT_NE(VALUES, noisy_values);
62
63 // Test that the dimensions of the output matches the input
64 EXPECT_EQ(noise.size(), NOISE.size());
65 EXPECT_EQ(noisy_values.size(), NOISY_VALUES.size());
66
67 // Test that start and end columns (=states) are not modified
68 EXPECT_EQ(VALUES.col(0), noisy_values.col(0));
69 EXPECT_EQ(VALUES.col(TIMESTEPS - 1), noisy_values.col(TIMESTEPS - 1));
70}
71
72int main(int argc, char** argv)
73{
74 testing::InitGoogleTest(&argc, argv);
75 return RUN_ALL_TESTS();
76}
NoiseGeneratorFn getNormalDistributionGenerator(size_t num_timesteps, const std::vector< double > &stddev)
Noise generator functions for randomizing trajectories in STOMP via a ComposableTask.
TEST(NoiseGeneratorTest, testStartEndUnchanged)
int main(int argc, char **argv)
constexpr size_t VARIABLES
constexpr size_t TIMESTEPS