Fix penalty-based cost function in STOMP

This adds several test cases for STOMP's noise generation and cost
functions, and provides the following fixes:

* out-of-bounds vector access when tail states of trajectory are invalid
* smoothed costs overriding values of previous invalid groups
* missing validity check of last state in trajectory
* inability to disable cost function interpolation steps
* total cost of trajectory not summing up to sum of state penalties

moveit_planners/stomp/CMakeLists.txt

@@ -57,6 +57,8 @@ if(BUILD_TESTING)
  set(ament_cmake_lint_cmake_FOUND TRUE)
 
  ament_lint_auto_find_test_dependencies()
+
+ add_subdirectory(test)
 endif()
 
 ament_package()

moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp

@@ -68,7 +68,7 @@ constexpr double CONSTRAINT_CHECK_DISTANCE = 0.05;
  * optimized as well.
  *
  * @param state_validator_fn The validator function that tests for binary conditions
- * @param interpolation_step_size The L2 norm distance step used for interpolation
+ * @param interpolation_step_size The L2 norm distance step used for interpolation (disabled when set to 0.0)
  *
  * @return Cost function that computes smooth costs for binary validity conditions
  */
@@ -86,46 +86,60 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator
  // Subsequent invalid states are assumed to have the same cause, so we are keeping track
  // of "invalid windows" which are used for smoothing out the costs per violation cause
  // with a gaussian, penalizing neighboring valid states as well.
- for (int timestep = 0; timestep < values.cols() - 1; ++timestep)
+ for (int timestep = 0; timestep < values.cols(); ++timestep)
  {
+ // Get state at current timestep and check for validity
+ // The penalty of the validation function is added to the cost of the current timestep
+ // A state is rendered invalid if a cost results from the current validity check or if a penalty is carried over
+ // from the previous iteration
  Eigen::VectorXd current = values.col(timestep);
- Eigen::VectorXd next = values.col(timestep + 1);
- const double segment_distance = (next - current).norm();
- double interpolation_fraction = 0.0;
- const double interpolation_step = std::min(0.5, interpolation_step_size / segment_distance);
- bool found_invalid_state = false;
- double penalty = 0.0;
- while (!found_invalid_state && interpolation_fraction < 1.0)
+ costs(timestep) += state_validator_fn(current);
+ bool found_invalid_state = costs(timestep) > 0.0;
+
+ // If state is valid, interpolate towards the next waypoint if there is one
+ bool continue_interpolation =
+  !found_invalid_state && timestep < (values.cols() - 1) && interpolation_step_size > 0.0;
+ if (continue_interpolation)
  {
- Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next;
+ Eigen::VectorXd next = values.col(timestep + 1);
+ const double interpolation_step = std::min(0.5, interpolation_step_size / (next - current).norm());
+ for (double interpolation_fraction = interpolation_step; interpolation_fraction < 1.0;
+ interpolation_fraction += interpolation_step)
+ {
+ Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next;
 
- penalty = state_validator_fn(sample_vec);
- found_invalid_state = penalty > 0.0;
- interpolation_fraction += interpolation_step;
+ double penalty = state_validator_fn(sample_vec);
+ found_invalid_state = penalty > 0.0;
+ if (found_invalid_state)
+ {
+ // Apply weighted penalties -> This trajectory is definitely invalid
+ costs(timestep) += (1 - interpolation_fraction) * penalty;
+ costs(timestep + 1) += interpolation_fraction * penalty;
+ break;
+ }
+ }
  }
 
+ // Track groups of invalid states as "invalid windows" for subsequent smoothing
  if (found_invalid_state)
  {
- // Apply weighted penalties -> This trajectory is definitely invalid
- costs(timestep) = (1 - interpolation_fraction) * penalty;
- costs(timestep + 1) = interpolation_fraction * penalty;
+ // Mark solution as invalid
  validity = false;
 
- // Open new invalid window when this is the first detected invalid state in a group
+ // OPEN new invalid window when this is the first detected invalid state in a group
  if (!in_invalid_window)
  {
- invalid_windows.emplace_back(timestep, values.cols());
+ invalid_windows.emplace_back(timestep, timestep);
  in_invalid_window = true;
  }
+
+ // Update end of invalid window with the current invalid timestep
+ invalid_windows.back().second = timestep;
  }
  else
  {
- // Close current invalid window if the current state is valid
- if (in_invalid_window)
- {
- invalid_windows.back().second = timestep - 1;
- in_invalid_window = false;
- }
+ // CLOSE current invalid window if the current state is valid
+ in_invalid_window = false;
  }
  }
 
@@ -134,18 +148,31 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator
  // before and after the violation are penalized as well.
  for (const auto& [start, end] : invalid_windows)
  {
+ // Total cost of invalid states
+ // We are smoothing the exact same total cost over a wider neighborhood
+ const double window_cost = costs(Eigen::seq(start, end)).sum();
+
+ // window size defines 2 sigma of gaussian smoothing kernel
+ // which equals 68.2% of overall cost and about 25% of width
  const double window_size = static_cast<double>(end - start) + 1;
- const double sigma = window_size / 5.0;
+ const double sigma = std::max(1.0, 0.5 * window_size);
  const double mu = 0.5 * (start + end);
+
  // Iterate over waypoints in the range of +/-sigma (neighborhood)
- // and add a weighted and continuous cost value for each waypoint
- // based on a Gaussian distribution.
- for (auto j = std::max(0l, start - static_cast<long>(sigma));
- j <= std::min(values.cols() - 1, end + static_cast<long>(sigma)); ++j)
+ // and add a discrete cost value for each waypoint based on a Gaussian
+ // distribution.
+ const long kernel_start = mu - static_cast<long>(sigma) * 4;
+ const long kernel_end = mu + static_cast<long>(sigma) * 4;
+ const long bounded_kernel_start = std::max(0l, kernel_start);
+ const long bounded_kernel_end = std::min(values.cols() - 1, kernel_end);
+ for (auto j = bounded_kernel_start; j <= bounded_kernel_end; ++j)
  {
- costs(j) +=
- std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * mu)) * window_size;
+ costs(j) = std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * mu));
  }
+
+ // Normalize values to original total window cost
+ const double cost_sum = costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)).sum();
+ costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)) *= window_cost / cost_sum;
  }
 
  return true;

moveit_planners/stomp/test/CMakeLists.txt

@@ -0,0 +1,12 @@
+find_package(ament_cmake_gtest REQUIRED)
+ament_add_gtest(test_noise_generator test_noise_generator.cpp)
+ament_target_dependencies(test_noise_generator
+ rsl
+ tf2_eigen
+)
+
+ament_add_gtest(test_cost_functions test_cost_functions.cpp)
+ament_target_dependencies(test_cost_functions
+ rsl
+ moveit_core
+)

moveit_planners/stomp/test/test_cost_functions.cpp

@@ -0,0 +1,78 @@
+#include <gtest/gtest.h>
+#include <stomp_moveit/cost_functions.hpp>
+
+TEST(NoiseGeneratorTest, testGetCostFunctionAllValidStates)
+{
+ constexpr size_t TIMESTEPS = 100;
+ constexpr size_t VARIABLES = 6;
+
+ auto state_validator_fn = [](const Eigen::VectorXd& /* state_positions */) { return 0.0; };
+
+ auto cost_fn =
+ stomp_moveit::costs::getCostFunctionFromStateValidator(state_validator_fn, 0.1 /* interpolation_step_size */);
+
+ // Build trajectory with TIMESTEPS states, interpolating from 0.0 to 1.0 joint values
+ Eigen::MatrixXd values = Eigen::MatrixXd::Zero(VARIABLES, TIMESTEPS);
+ const int last_timestep = values.cols() - 1;
+ for (int timestep = 0; timestep <= last_timestep; ++timestep)
+ {
+ values.col(timestep).fill(1.0 * timestep / last_timestep);
+ }
+
+ Eigen::VectorXd costs(TIMESTEPS);
+ bool validity;
+
+ ASSERT_TRUE(cost_fn(values, costs, validity));
+ EXPECT_TRUE(validity);
+ EXPECT_EQ(costs.sum(), 0.0);
+}
+
+TEST(NoiseGeneratorTest, testGetCostFunctionInvalidStates)
+{
+ constexpr size_t TIMESTEPS = 100;
+ constexpr size_t VARIABLES = 6;
+ constexpr double PENALTY = 1.0;
+
+ // We are simulating a state validator by tagging specific timesteps as invalid.
+ // The state validation function is called once per timestep since interpolation is disabled.
+ // These assumptions are tested
+ static const std::set<int> invalid_timesteps(
+ { 10, 11, 12, 13, 25, 26, 27, 46, 63, 64, 65, 66, 67, 68, 69, 97, 98, 99 });
+ size_t timestep_counter = 0;
+ auto state_validator_fn = [&](const Eigen::VectorXd& /* state_positions */) {
+ return PENALTY * invalid_timesteps.count(timestep_counter++);
+ };
+
+ auto cost_fn =
+ stomp_moveit::costs::getCostFunctionFromStateValidator(state_validator_fn, 0.0 /* interpolation disabled */);
+
+ // Build trajectory with TIMESTEPS states, interpolating from 0.0 to 1.0 joint values
+ Eigen::MatrixXd values = Eigen::MatrixXd::Zero(VARIABLES, TIMESTEPS);
+ const int last_timestep = values.cols() - 1;
+ for (int timestep = 0; timestep <= last_timestep; ++timestep)
+ {
+ values.col(timestep).fill(1.0 * timestep / last_timestep);
+ }
+
+ // Test boundary assumptions about cost function outputs, costs and validity
+ Eigen::VectorXd costs(TIMESTEPS);
+ bool validity;
+ ASSERT_TRUE(cost_fn(values, costs, validity));
+ EXPECT_EQ(timestep_counter, 100u); // 100 timesteps checked without interpolation
+ EXPECT_FALSE(validity); // invalid states must result in an invalid trajectory
+ EXPECT_LE(costs.maxCoeff(), PENALTY); // the highest cost must not be higher than the configured penalty
+ EXPECT_GE(costs.minCoeff(), 0.0); // no negative cost values should be computed
+
+ // The total cost must equal the sum of penalties produced by all invalid timesteps
+ EXPECT_DOUBLE_EQ(costs.sum(), PENALTY * invalid_timesteps.size());
+ // Invalid timesteps should account for the majority of the total cost.
+ // We expect that invalid windows cover at least 2*sigma (=68.1%) of each cost distribution.
+ const std::vector<int> invalid_timesteps_vec(invalid_timesteps.begin(), invalid_timesteps.end());
+ EXPECT_GE(costs(invalid_timesteps_vec).sum(), 0.681 * PENALTY);
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}

moveit_planners/stomp/test/test_noise_generator.cpp

@@ -0,0 +1,32 @@
+#include <gtest/gtest.h>
+#include <stomp_moveit/noise_generators.hpp>
+
+TEST(NoiseGeneratorTest, testStartEndUnchanged)
+{
+ constexpr size_t TIMESTEPS = 100;
+ constexpr size_t VARIABLES = 6;
+ static const std::vector<double> STDDEV(VARIABLES, 0.2);
+
+ auto noise_gen = stomp_moveit::noise::getNormalDistributionGenerator(TIMESTEPS, STDDEV);
+
+ const Eigen::MatrixXd VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 1.0);
+ const Eigen::MatrixXd NOISY_VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
+ const Eigen::MatrixXd NOISE = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
+
+ auto noise = NOISE;
+ auto noisy_values = NOISY_VALUES;
+
+ noise_gen(VALUES, noisy_values, noise);
+
+ EXPECT_NE(noise, NOISE);
+ EXPECT_NE(noisy_values, NOISY_VALUES);
+
+ EXPECT_EQ(noise.size(), NOISE.size());
+ EXPECT_EQ(noisy_values.size(), NOISY_VALUES.size());
+}
+
+int main(int argc, char** argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}