Add path shortening utils and consolidate examples

bindings/examples/example_path_shortcutting.py

@@ -0,0 +1,96 @@
+from pathlib import Path
+import time
+
+import numpy as np
+import pinocchio as pin
+from roboplan import (
+    computeFramePath,
+    get_package_share_dir,
+    shortcutPath,
+    JointConfiguration,
+    JointPath,
+    Scene,
+    RRTOptions,
+    RRT,
+)
+from roboplan.viser_visualizer import ViserVisualizer
+
+
+def visualizePath(
+    viz: ViserVisualizer,
+    scene: Scene,
+    path: JointPath,
+    frame_name: str,
+    max_step_size: float,
+    colors: tuple,
+    path_name: str,
+) -> None:
+
+    path_segments = []
+    for idx in range(len(path.positions) - 1):
+        q_start = path.positions[idx]
+        q_end = path.positions[idx + 1]
+        frame_path = computeFramePath(scene, q_start, q_end, frame_name, max_step_size)
+        for idx in range(len(frame_path) - 1):
+            path_segments.append([frame_path[idx][:3, 3], frame_path[idx + 1][:3, 3]])
+
+    viz.viewer.scene.add_line_segments(
+        path_name,
+        points=np.array(path_segments),
+        colors=colors,
+        line_width=3.0,
+    )
+
+
+if __name__ == "__main__":
+
+    roboplan_examples_dir = Path(get_package_share_dir())
+    urdf_path = roboplan_examples_dir / "ur_robot_model" / "ur5_gripper.urdf"
+    srdf_path = roboplan_examples_dir / "ur_robot_model" / "ur5_gripper.srdf"
+    package_paths = [roboplan_examples_dir]
+
+    scene = Scene("test_scene", urdf_path, srdf_path, package_paths)
+
+    # Create a redundant Pinocchio model just for visualization.
+    # When Pinocchio 4.x releases nanobind bindings, we should be able to directly grab the model from the scene instead.
+    model, collision_model, visual_model = pin.buildModelsFromUrdf(
+        urdf_path, package_dirs=package_paths
+    )
+    viz = ViserVisualizer(model, collision_model, visual_model)
+    viz.initViewer(open=True, loadModel=True)
+
+    # Set up an RRT with a very small connection distance to demonstrate path shortening.
+    options = RRTOptions()
+    options.max_connection_distance = 1.0
+    options.collision_check_step_size = 0.01
+    options.max_planning_time = 5.0
+    options.rrt_connect = True
+    rrt = RRT(scene, options)
+
+    start = JointConfiguration()
+    start.positions = scene.randomCollisionFreePositions()
+    assert start.positions is not None
+
+    goal = JointConfiguration()
+    goal.positions = scene.randomCollisionFreePositions()
+    assert goal.positions is not None
+
+    path = rrt.plan(start, goal)
+    assert path is not None
+    print("Original path (in red):")
+    print(path)
+
+    # After planning attempt to shorten the path
+    shortcut_path = shortcutPath(scene, path, options.collision_check_step_size, 1000)
+    print("Shortcutted path (in green):")
+    print(shortcut_path)
+
+    # Visualize the tree, path, and shortcutted path
+    viz.display(start.positions)
+    visualizePath(viz, scene, path, "tool0", 0.05, (100, 0, 0), "/rrt/path")
+    visualizePath(
+        viz, scene, shortcut_path, "tool0", 0.05, (0, 100, 0), "/rrt/shortcut_path"
+    )
+
+    while True:
+        time.sleep(10.0)

bindings/src/roboplan/__init__.py

@@ -1,6 +1,10 @@
 from .roboplan.core import (
     computeFramePath,
     hasCollisionsAlongPath,
+    shortcutPath,
+    getPathLengths,
+    getNormalizedPathScaling,
+    getConfigurationFromNormalizedPathScaling,
     CartesianConfiguration,
     JointConfiguration,
     JointInfo,

bindings/src/roboplan/viser_visualizer.py

@@ -88,14 +88,24 @@ def initViewer(
 
         if open:
             import webbrowser
+            import threading
+
+            client_connected = threading.Event()
+
+            # Set up callback for when a client connects
+            @self.viewer.on_client_connect
+            def handle_client_connect(client):
+                print(f"Client connected: {client.client_id}")
+                client_connected.set()
 
             webbrowser.open(f"http://{self.viewer.get_host()}:{self.viewer.get_port()}")
 
-            # Wait until clients are reported.
-            # Otherwise, capturing an image too soon after opening a browser window
-            # may not register any clients.
-            while len(self.viewer.get_clients()) == 0:
-                time.sleep(0.1)
+            # Wait until clients are reported with a timeout.
+            if not client_connected.wait(timeout=3.0):
+                print(
+                    "Warning: No client connected within 3.0 seconds, open a browser to:"
+                )
+                print(f"    http://{self.viewer.get_host()}:{self.viewer.get_port()}")
 
         if loadModel:
             self.loadViewerModel()

bindings/src/roboplan_ext.cpp

@@ -96,6 +96,12 @@ NB_MODULE(roboplan, m) {
 
   m_core.def("computeFramePath", &computeFramePath);
   m_core.def("hasCollisionsAlongPath", &hasCollisionsAlongPath);
+  m_core.def("shortcutPath", &shortcutPath, "scene"_a, "path"_a, "max_step_size"_a,
+             "max_iters"_a = 100, "seed"_a = 0);
+  m_core.def("getPathLengths", &getPathLengths);
+  m_core.def("getNormalizedPathScaling", &getNormalizedPathScaling);
+  m_core.def("getConfigurationFromNormalizedPathScaling",
+             &getConfigurationFromNormalizedPathScaling);
 
   /// Examples module
   nanobind::module_ m_example_models = m.def_submodule("example_models", "Example models");

roboplan/include/roboplan/core/path_utils.hpp

@@ -28,4 +28,39 @@ 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).
+/// @param seed Seed for the random generator, if 0 will not be applied (default 0).
+/// @return A shortcutted JointPath, if available.
+JointPath shortcutPath(const Scene& scene, const JointPath& path, double max_step_size,
+                       unsigned int max_iters = 100, unsigned int seed = 0);
+
+/// @brief Computes configuration distances from the start to each pose in a path.
+/// @param scene The scene for checking distances between joint poses.
+/// @param path The JointPath to evaluate.
+/// @return A vector of incremental path distances, if there is sufficient data.
+std::optional<Eigen::VectorXd> getPathLengths(const Scene& scene, const JointPath& path);
+
+/// @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::optional<Eigen::VectorXd> 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 pointing to the intermediate point along the path.
+/// @return a pair containing the joint configuration at the scaled 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 Eigen::VectorXd& path_scalings, double value);
+
 }  // namespace roboplan

roboplan/src/core/path_utils.cpp

@@ -47,4 +47,118 @@ 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, unsigned int seed) {
+
+  // Make a copy of the provided path's configurations.
+  JointPath shortened_path = path;
+  auto& path_configs = shortened_path.positions;
+
+  // We sample in the range (0, 1] to prevent modification of the starting configuration.
+  std::random_device rd;
+  std::mt19937 gen(seed != 0 ? seed : rd());
+  std::uniform_real_distribution<double> dis(std::numeric_limits<double>::epsilon(), 1.0);
+
+  for (unsigned int i = 0; i < max_iters; ++i) {
+    // The path is at maximum shortcutted-ness
+    if (path_configs.size() < 3) {
+      return shortened_path;
+    }
+
+    // Recompute the path scalings every iteration. If we can't compute these we can
+    // assume we are done (the path is at maximum shortness).
+    const auto path_scalings_maybe = getNormalizedPathScaling(scene, shortened_path);
+    if (!path_scalings_maybe.has_value()) {
+      return shortened_path;
+    }
+    const auto path_scalings = path_scalings_maybe.value();
+
+    // Randomly sample two points along the scaled 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 by updating
+    // the configs vector in place. We connect the low and high configurations directly, and
+    // erase the intermediate nodes (if any).
+    if (!hasCollisionsAlongPath(scene, q_low, q_high, max_step_size)) {
+      path_configs[idx_low] = q_low;
+      path_configs[idx_high] = q_high;
+
+      if (idx_high > idx_low + 1) {
+        path_configs.erase(path_configs.begin() + idx_low + 1, path_configs.begin() + idx_high);
+      }
+    }
+  }
+
+  return shortened_path;
+}
+
+std::optional<Eigen::VectorXd> getPathLengths(const Scene& scene, const JointPath& path) {
+  if (path.positions.size() < 2) {
+    return std::nullopt;
+  }
+
+  Eigen::VectorXd path_length_list;
+  path_length_list.resize(path.positions.size());
+
+  // Iteratively compute path lengths from start to finish
+  double path_length = 0.0;
+  path_length_list(0) = 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(idx + 1) = path_length;
+  }
+
+  return path_length_list;
+}
+
+std::optional<Eigen::VectorXd> getNormalizedPathScaling(const Scene& scene, const JointPath& path) {
+  auto path_length_list_maybe = getPathLengths(scene, path);
+  if (!path_length_list_maybe.has_value()) {
+    return std::nullopt;
+  }
+  auto path_length_list = path_length_list_maybe.value();
+  auto path_length = path_length_list(path_length_list.size() - 1);
+
+  // Normalize and return
+  if (path_length > 0.0) {
+    path_length_list /= path_length;
+  }
+
+  return path_length_list;
+}
+
+std::pair<Eigen::VectorXd, size_t>
+getConfigurationFromNormalizedPathScaling(const Scene& scene, const JointPath& path,
+                                          const Eigen::VectorXd& path_scalings, double value) {
+
+  for (long idx = 1; idx < path_scalings.size() - 1; ++idx) {
+    // Find the smallest index that is less than the provided 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};
+  }
+
+  // If we get here then the index is the end of the list and we should just return the goal pose.
+  return {path.positions.back(), path.positions.size() - 1};
+}
+
 }  // namespace roboplan

roboplan/test/CMakeLists.txt

@@ -20,6 +20,15 @@ target_link_libraries(
   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
 
 # If ament_gtest is detected, make the tests visible in ROS 2.
@@ -30,7 +39,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()