Improve documentation

moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp

@@ -60,38 +60,40 @@ 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 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.
+ // 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
+ // from the previous iteration.
  Eigen::VectorXd current = values.col(timestep);
  costs(timestep) += state_validator_fn(current);
  bool found_invalid_state = costs(timestep) > 0.0;
@@ -102,6 +104,7 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator
  if (continue_interpolation)
  {
  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)
@@ -129,6 +132,7 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator
  // OPEN new invalid window when this is the first detected invalid state in a group
  if (!in_invalid_window)
  {
+ // new windows only include a single timestep as start and end state
  invalid_windows.emplace_back(timestep, timestep);
  in_invalid_window = true;
  }

moveit_planners/stomp/test/test_cost_functions.cpp

@@ -45,63 +45,66 @@ constexpr double PENALTY = 1.0;
 
 TEST(NoiseGeneratorTest, testGetCostFunctionAllValidStates)
 {
- // State validator that
+ // 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 */);
 
- // Build trajectory with TIMESTEPS states, interpolating from 0.0 to 1.0 joint values
+ // 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(1.0 * timestep / last_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)
 {
- // We are simulating a state validator by tagging specific timesteps as invalid.
+ // 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.
- // These assumptions are tested
+ // 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 */);
 
- // Build trajectory with TIMESTEPS states, interpolating from 0.0 to 1.0 joint values
+ // 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(1.0 * timestep / last_timestep);
+ values.col(timestep).fill(static_cast<double>(timestep) / last_timestep);
  }
 
- // Test boundary assumptions about cost function outputs, costs and validity
+ // WHEN the cost function is applied to the trajectory
  Eigen::VectorXd costs(TIMESTEPS);
  bool validity;
  ASSERT_TRUE(cost_fn(values, costs, validity));
- EXPECT_EQ(timestep_counter, 100u); // 100 timesteps checked without interpolation
+
+ // 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
 
- // The total cost must equal the sum of penalties produced by all invalid timesteps
+ // THEN 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.
+
+ // 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);