RRT: Add maximum time and check collisions at goal first

examples/example_rrt.py

@@ -102,21 +102,23 @@ def prepare_scene(visual_model, collision_model):
     # Search for a path
     options = RRTPlannerOptions()
     options.max_angle_step = 0.05
-    options.max_connection_dist = 0.25
+    options.max_connection_dist = 0.5
     options.goal_biasing_probability = 0.15
+    options.max_planning_time = 5.0
 
     planner = RRTPlanner(model, collision_model)
     path = planner.plan(q_start, q_end, options=options)
     planner.visualize(viz, "panda_hand", show_tree=True)
 
     # Animate the path
-    input("Press 'Enter' to animate the path.")
-    for idx in range(1, len(path)):
-        segment_start = path[idx - 1]
-        segment_end = path[idx]
-        q_path = discretize_joint_space_path(
-            segment_start, segment_end, options.max_angle_step
-        )
-        for q in q_path:
-            viz.display(q)
-            time.sleep(0.05)
+    if path:
+        input("Press 'Enter' to animate the path.")
+        for idx in range(1, len(path)):
+            segment_start = path[idx - 1]
+            segment_end = path[idx]
+            q_path = discretize_joint_space_path(
+                segment_start, segment_end, options.max_angle_step
+            )
+            for q in q_path:
+                viz.display(q)
+                time.sleep(0.05)

src/pyroboplan/planning/rrt.py

@@ -1,6 +1,7 @@
 """ Utilities for manipulation-specific Rapidly-Expanding Random Trees (RRTs). """
 
 import numpy as np
+import time
 
 from ..core.utils import (
     check_collisions_at_state,
@@ -24,6 +25,9 @@ class RRTPlannerOptions:
     max_connection_dist = 0.2
     """ Maximum angular distance, in radians, for connecting nodes. """
 
+    max_planning_time = 10.0
+    """ Maximum planning time, in seconds. """
+
     goal_biasing_probability = 0.0
     """ Probability of sampling the goal configuration itself, which can help planning converge. """
 
@@ -67,6 +71,7 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
             options : `RRTPlannerOptions`, optional
                 The options to use for planning. If not specified, default options are used.
         """
+        t_start = time.time()
         self.options = options
         self.graph = Graph()
         start_node = Node(q_start, parent=None)
@@ -96,6 +101,11 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
             goal_found = True
 
         while not goal_found:
+            # Check for timeouts
+            if time.time() - t_start > options.max_planning_time:
+                print(f"Planning timed out after {options.max_planning_time} seconds.")
+                break
+
             # Sample a new configuration.
             if np.random.random() < self.options.goal_biasing_probability:
                 q_sample = q_goal
@@ -110,6 +120,9 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
                 q_sample = nearest_node.q + scale * (q_sample - nearest_node.q)
 
             # Add the node only if it is collision free.
+            if check_collisions_at_state(self.model, self.collision_model, q_sample):
+                continue
+
             path_to_node = discretize_joint_space_path(
                 nearest_node.q, q_sample, self.options.max_angle_step
             )
@@ -120,7 +133,7 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
                 self.graph.add_node(latest_node)
                 self.graph.add_edge(nearest_node, latest_node)
 
-                # Check if that latest node connects directly to goal.
+                # Check if latest node connects directly to goal.
                 path_to_goal = discretize_joint_space_path(
                     latest_node.q, q_goal, self.options.max_angle_step
                 )
@@ -133,16 +146,18 @@ def plan(self, q_start, q_goal, options=RRTPlannerOptions()):
                     goal_found = True
 
         # Back out the path by traversing the parents from the goal.
-        cur_node = goal_node
         self.latest_path = []
-        path_extracted = False
-        while not path_extracted:
-            if cur_node is None:
-                path_extracted = True
-            else:
-                self.latest_path.append(cur_node.q)
-                cur_node = cur_node.parent
-        self.latest_path.reverse()
+        if goal_found:
+            cur_node = goal_node
+            path_extracted = False
+            while not path_extracted:
+                if cur_node is None:
+                    path_extracted = True
+                else:
+                    self.latest_path.append(cur_node.q)
+                    cur_node = cur_node.parent
+            self.latest_path.reverse()
+
         return self.latest_path
 
     def visualize(