Add functions for path shortening

Author: eholum

roboplan/include/roboplan/core/path_utils.hpp

@@ -28,4 +28,34 @@ std::vector<Eigen::Matrix4d> computeFramePath(const Scene& scene, const Eigen::V
 bool hasCollisionsAlongPath(const Scene& scene, const Eigen::VectorXd& q_start,
                             const Eigen::VectorXd& q_end, const double max_step_size);
 
+/// @brief Attempts to shortcut the path with random sampling and checking connections.
+/// @details This implementation is based on section 3.5.3 of:
+/// https://motion.cs.illinois.edu/RoboticSystems/MotionPlanningHigherDimensions.html
+/// @param scene The scene for checking connectability between joint poses.
+/// @param path The JointPath to try to shorten.
+/// @param max_step_size Maximum step size to use in collision checking.
+/// @param max_iters Maximum number of iterations of random sampling (default 100).
+/// @return A shortcutted JointPath, if available.
+JointPath shortcutPath(const Scene& scene, const JointPath& path, double max_step_size,
+                       unsigned int max_iters = 100);
+
+/// @brief Helper function to compute length-normalized scaling values along a JointPath.
+/// @param scene The scene to compute configuration distances.
+/// @param path The path to length-normalize.
+/// @return A vector of scaling values between 0.0 and 1.0 at each point in the path.
+std::vector<double> getNormalizedPathScaling(const Scene& scene, const JointPath& path);
+
+/// @brief Helper function to get joint configurations from a path with normalized joint scalings.
+/// @param scene The scene to use for joint interpolation.
+/// @param path A JointPath of joint poses.
+/// @param path_scalings The corresponding path scalings (between 0 and 1) to the provided path.
+/// @param value A value between 0.0 and 1.0, at which to get the joint configuration along the
+/// path.
+/// @return a tuple containing the joint configuration at the specified scaling value along the
+/// path,
+///         as well as the index corresponding to the next point along the path.
+std::pair<Eigen::VectorXd, size_t>
+getConfigurationFromNormalizedPathScaling(const Scene& scene, const JointPath& path,
+                                          const std::vector<double>& path_scalings, double value);
+
 }  // namespace roboplan

roboplan/src/core/path_utils.cpp

@@ -47,4 +47,115 @@ bool hasCollisionsAlongPath(const Scene& scene, const Eigen::VectorXd& q_start,
   return false;
 }
 
+JointPath shortcutPath(const Scene& scene, const JointPath& path, double max_step_size,
+                       unsigned int max_iters) {
+  // Make a copy of the provided path.
+  JointPath shortened_path = path;
+
+  // Cannot shorten paths if they don't have enough points.
+  if (path.positions.size() < 3) {
+    return shortened_path;
+  }
+
+  std::random_device rd;
+  std::mt19937 gen(rd());
+  std::uniform_real_distribution<double> dis(0.0, 1.0);
+
+  for (unsigned int i = 0; i < max_iters; ++i) {
+
+    // The the path is at maximum shortcutted-ness
+    if (shortened_path.positions.size() < 3) {
+      return shortened_path;
+    }
+
+    // Randomly sample two points along the scaled path
+    const auto path_scalings = getNormalizedPathScaling(scene, shortened_path);
+    double low_point = dis(gen);
+    double high_point = dis(gen);
+    if (low_point > high_point) {
+      std::swap(low_point, high_point);
+    }
+
+    const auto [q_low, idx_low] =
+        getConfigurationFromNormalizedPathScaling(scene, shortened_path, path_scalings, low_point);
+    const auto [q_high, idx_high] =
+        getConfigurationFromNormalizedPathScaling(scene, shortened_path, path_scalings, high_point);
+
+    if (idx_low == idx_high) {
+      continue;
+    }
+
+    // Check if the sampled segment is collision free. If it is, shortcut the path.
+    if (!hasCollisionsAlongPath(scene, q_low, q_high, max_step_size)) {
+      std::vector<Eigen::VectorXd> new_positions;
+      new_positions.reserve(idx_low + 2 + (shortened_path.positions.size() - idx_high));
+
+      for (size_t i = 0; i < idx_low; ++i) {
+        new_positions.push_back(shortened_path.positions[i]);
+      }
+
+      new_positions.push_back(q_low);
+      new_positions.push_back(q_high);
+
+      for (size_t i = idx_high; i < shortened_path.positions.size(); ++i) {
+        new_positions.push_back(shortened_path.positions[i]);
+      }
+
+      shortened_path.positions = std::move(new_positions);
+    }
+  }
+
+  return shortened_path;
+}
+
+std::vector<double> getNormalizedPathScaling(const Scene& scene, const JointPath& path) {
+  if (path.positions.empty()) {
+    return {};
+  }
+  if (path.positions.size() == 1) {
+    return {1.0};
+  }
+
+  std::vector<double> path_length_list;
+  path_length_list.reserve(path.positions.size());
+
+  // Iteratively compute path lengths from start to finish
+  double path_length = 0.0;
+  path_length_list.push_back(path_length);
+  for (size_t idx = 0; idx < path.positions.size() - 1; ++idx) {
+    path_length += scene.configurationDistance(path.positions[idx], path.positions[idx + 1]);
+    path_length_list.push_back(path_length);
+  }
+
+  // Normalize
+  for (auto& length : path_length_list) {
+    length /= path_length;
+  }
+
+  return path_length_list;
+}
+
+std::pair<Eigen::VectorXd, size_t>
+getConfigurationFromNormalizedPathScaling(const Scene& scene, const JointPath& path,
+                                          const std::vector<double>& path_scalings, double value) {
+
+  for (size_t idx = 0; idx < path_scalings.size(); ++idx) {
+    // Find the smallest index that is less than value
+    if (value > path_scalings[idx]) {
+      continue;
+    }
+
+    // Interpolate to the joint configuration
+    const double delta_scale =
+        (value - path_scalings[idx - 1]) / (path_scalings[idx] - path_scalings[idx - 1]);
+    const Eigen::VectorXd q_interp =
+        scene.interpolate(path.positions[idx - 1], path.positions[idx], delta_scale);
+
+    return {q_interp, idx};
+  }
+
+  // This shouldn't be possible
+  return {path.positions.back(), path.positions.size() - 1};
+}
+
 }  // namespace roboplan