Fix penalty-based cost function in STOMP (#2625)

* 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
* bug in Gaussian producing infinite values with invalid start states

* Improve documentation

---------

moveit_planners/stomp/CMakeLists.txt

@@ -29,9 +29,12 @@ ament_target_dependencies(stomp_moveit_plugin
   std_msgs
   tf2_eigen
   visualization_msgs
-  rsl
 )
-target_link_libraries(stomp_moveit_plugin stomp::stomp stomp_moveit_parameters)
+target_link_libraries(stomp_moveit_plugin
+  stomp::stomp
+  stomp_moveit_parameters
+  rsl::rsl
+)
 
 pluginlib_export_plugin_description_file(moveit_core stomp_moveit_plugin_description.xml)
 
@@ -57,6 +60,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

@@ -60,72 +60,90 @@ constexpr double COL_CHECK_DISTANCE = 0.05;
 constexpr double CONSTRAINT_CHECK_DISTANCE = 0.05;
 
 /**
- * Creates a cost function from a binary robot state validation function.
- * This is used for computing smooth cost profiles for binary state conditions like collision checks and constraints.
+ * Creates a cost function from a robot state validation function.
+ * This is used for computing smooth cost profiles for waypoint state conditions like collision checks and constraints.
  * The validator function is applied for all states in the validated path while also considering interpolated states.
  * If a waypoint or an interpolated state is invalid, a local penalty is being applied to the path.
  * Penalty costs are being smoothed out using a Gaussian so that valid neighboring states (near collisions) are
  * optimized as well.
+ * This implementation does not support cost thresholds, non-zero local costs will render the trajectory as invalid.
  *
- * @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 state_validator_fn      The validator function that produces local costs for all waypoints
+ * @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
  */
 CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator_fn, double interpolation_step_size)
 {
   CostFn cost_fn = [=](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) {
+    // Assume zero cost and valid trajectory from the start
     costs.setZero(values.cols());
-
     validity = true;
-    std::vector<std::pair<long, long>> invalid_windows;
-    bool in_invalid_window = false;
 
     // Iterate over sample waypoint pairs and check for validity in each segment.
     // If an invalid state is found, weighted penalty costs are applied to both waypoints.
     // 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)
+    // Invalid windows are represented as pairs of start and end timesteps.
+    std::vector<std::pair<long, long>> invalid_windows;
+    bool in_invalid_window = false;
+    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);
+        // Interpolate waypoints at least once, even if interpolation_step_size exceeds the waypoint distance
+        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());
+          // new windows only include a single timestep as start and end state
+          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 +152,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 * M_PI));
       }
+
+      // 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,16 @@
+find_package(ament_cmake_gtest REQUIRED)
+ament_add_gtest(test_noise_generator test_noise_generator.cpp)
+ament_target_dependencies(test_noise_generator
+  tf2_eigen
+)
+target_link_libraries(test_noise_generator
+  rsl::rsl
+)
+
+ament_add_gtest(test_cost_functions test_cost_functions.cpp)
+ament_target_dependencies(test_cost_functions
+  moveit_core
+)
+target_link_libraries(test_cost_functions
+  rsl::rsl
+)

moveit_planners/stomp/test/test_cost_functions.cpp

@@ -0,0 +1,117 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/** @file
+ * @author Henning Kayser
+ */
+
+#include <gtest/gtest.h>
+#include <stomp_moveit/cost_functions.hpp>
+
+constexpr size_t TIMESTEPS = 100;
+constexpr size_t VARIABLES = 6;
+constexpr double PENALTY = 1.0;
+
+TEST(NoiseGeneratorTest, testGetCostFunctionAllValidStates)
+{
+  // GIVEN a cost function with a state validator that only returns valid costs of 0.0
+  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 */);
+
+  // GIVEN a trajectory with TIMESTEPS states, with waypoints 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(static_cast<double>(timestep) / last_timestep);
+  }
+
+  // WHEN the cost function is applied to the trajectory
+  Eigen::VectorXd costs(TIMESTEPS);
+  bool validity;
+  ASSERT_TRUE(cost_fn(values, costs, validity));
+
+  // THEN the trajectory must be valid and have zero costs
+  EXPECT_TRUE(validity);
+  EXPECT_EQ(costs.sum(), 0.0);
+}
+
+TEST(NoiseGeneratorTest, testGetCostFunctionInvalidStates)
+{
+  // GIVEN a cost function with a simulated state validator that tags selected timesteps as invalid.
+  // The state validation function is called once per timestep since interpolation is disabled.
+  // This assumption is confirmed as boundary assumption after calling the solver.
+  static const std::set<int> INVALID_TIMESTEPS(
+      { 0, 10, 11, 12, 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 */);
+
+  // GIVEN a trajectory with TIMESTEPS states, with waypoints 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(static_cast<double>(timestep) / last_timestep);
+  }
+
+  // WHEN the cost function is applied to the trajectory
+  Eigen::VectorXd costs(TIMESTEPS);
+  bool validity;
+  ASSERT_TRUE(cost_fn(values, costs, validity));
+
+  // THEN the following boundary assumptions about cost function outputs, costs and validity need to be met
+  EXPECT_FALSE(validity);                // invalid states must result in an invalid trajectory
+  EXPECT_EQ(timestep_counter, 100u);     // 100 timesteps checked without interpolation
+  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
+
+  // THEN the total cost must equal the sum of penalties produced by all invalid timesteps
+  EXPECT_DOUBLE_EQ(costs.sum(), PENALTY * INVALID_TIMESTEPS.size());
+
+  // THEN invalid timesteps must 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,76 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2023, PickNik Inc.
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of PickNik Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/** @file
+ * @author Henning Kayser
+ */
+
+#include <gtest/gtest.h>
+#include <stomp_moveit/noise_generators.hpp>
+
+constexpr size_t TIMESTEPS = 100;
+constexpr size_t VARIABLES = 6;
+static const std::vector<double> STDDEV(VARIABLES, 0.2);
+static const Eigen::MatrixXd VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 1.0);
+static const Eigen::MatrixXd NOISY_VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
+static const Eigen::MatrixXd NOISE = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0);
+
+TEST(NoiseGeneratorTest, testStartEndUnchanged)
+{
+  auto noise_gen = stomp_moveit::noise::getNormalDistributionGenerator(TIMESTEPS, STDDEV);
+
+  auto noise = NOISE;
+  auto noisy_values = NOISY_VALUES;
+
+  noise_gen(VALUES, noisy_values, noise);
+
+  // Test that noise creates output unlike the input
+  EXPECT_NE(noise, NOISE);
+  EXPECT_NE(noisy_values, NOISY_VALUES);
+  EXPECT_NE(VALUES, noisy_values);
+
+  // Test that the dimensions of the output matches the input
+  EXPECT_EQ(noise.size(), NOISE.size());
+  EXPECT_EQ(noisy_values.size(), NOISY_VALUES.size());
+
+  // Test that start and end columns (=states) are not modified
+  EXPECT_EQ(VALUES.col(0), noisy_values.col(0));
+  EXPECT_EQ(VALUES.col(TIMESTEPS - 1), noisy_values.col(TIMESTEPS - 1));
+}
+
+int main(int argc, char** argv)
+{
+  testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}