Add path shortening utils and consolidate examples

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,113 @@ 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 = dis(gen);
+    double high = dis(gen);
+    if (low > high) {
+      std::swap(low, high);
+    }
+    const auto [q_low, idx_low] =
+        getConfigurationFromNormalizedPathScaling(scene, shortened_path, path_scalings, low);
+    const auto [q_high, idx_high] =
+        getConfigurationFromNormalizedPathScaling(scene, shortened_path, path_scalings, high);
+    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

roboplan/test/CMakeLists.txt

@@ -19,6 +19,14 @@ target_link_libraries(
   GTest::gtest_main
   roboplan_example_models::roboplan_example_models
 )
+add_executable(test_path_utils test_path_utils.cpp)
+set_property(TARGET test_path_utils PROPERTY CXX_STANDARD 20)
+target_link_libraries(
+  test_path_utils
+  roboplan
+  GTest::gtest_main
+  roboplan_example_models::roboplan_example_models
+)
 
 # TODO: Define tests as a variable
 
@@ -30,7 +38,8 @@ if ( ament_cmake_gtest_FOUND )
 
     ament_add_gtest_test(test_types)
     ament_add_gtest_test(test_scene)
+    ament_add_gtest_test(test_path_utils)
 else()
     include(GoogleTest)
-    gtest_discover_tests(test_types test_scene)
+    gtest_discover_tests(test_types test_scene test_path_utils)
 endif()

roboplan/test/test_path_utils.cpp

@@ -0,0 +1,69 @@
+#include <gtest/gtest.h>
+
+#include <roboplan/core/path_utils.hpp>
+#include <roboplan/core/scene.hpp>
+#include <roboplan_example_models/resources.hpp>
+
+namespace roboplan {
+
+class RoboPlanPathUtilsTest : public ::testing::Test {
+protected:
+  void SetUp() override {
+    const auto share_prefix = roboplan_example_models::get_package_share_dir();
+    const auto urdf_path = share_prefix / "ur_robot_model" / "ur5_gripper.urdf";
+    const auto srdf_path = share_prefix / "ur_robot_model" / "ur5_gripper.srdf";
+    const std::vector<std::filesystem::path> package_paths = {share_prefix};
+    scene_ = std::make_shared<Scene>("test_scene", urdf_path, srdf_path, package_paths);
+  }
+
+public:
+  // No default constructors, so must be pointers.
+  std::shared_ptr<Scene> scene_;
+
+  JointPath getTestPath(const size_t num_points) {
+    JointPath test_path;
+    test_path.joint_names = scene_->getJointNames();
+
+    if (num_points == 0)
+      return test_path;
+
+    Eigen::VectorXd q1(6);
+    Eigen::VectorXd q2(6);
+    Eigen::VectorXd q3(6);
+    q1 << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+    q2 << 0.5, 0.0, 0.0, 0.0, 0.0, 0.0;
+    q3 << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0;
+
+    if (num_points >= 1)
+      test_path.positions.push_back(q1);
+    if (num_points >= 2)
+      test_path.positions.push_back(q2);
+    if (num_points >= 3)
+      test_path.positions.push_back(q3);
+
+    return test_path;
+  }
+};
+
+TEST_F(RoboPlanPathUtilsTest, testGetNormalizedPathScaling) {
+
+  JointPath empty_path = getTestPath(0);
+  auto empty_scalings = getNormalizedPathScaling(*scene_, empty_path);
+  EXPECT_TRUE(empty_scalings.empty());
+
+  JointPath length_1_path = getTestPath(1);
+  auto length_1_path_scalings = getNormalizedPathScaling(*scene_, length_1_path);
+  ASSERT_EQ(length_1_path_scalings.size(), 1);
+  EXPECT_DOUBLE_EQ(length_1_path_scalings[0], 1.0);
+
+  // Path with 3 evenly spaced points
+  auto test_path = getTestPath(3);
+  auto path_scalings = getNormalizedPathScaling(*scene_, test_path);
+
+  ASSERT_EQ(path_scalings.size(), 3);
+  EXPECT_DOUBLE_EQ(path_scalings[0], 0.0);
+  EXPECT_DOUBLE_EQ(path_scalings[1], 0.5);
+  EXPECT_DOUBLE_EQ(path_scalings[2], 1.0);
+}
+
+}  // namespace roboplan