Add an option keep planning after goal is reached in RRT (sea-bass#40)

examples/example_rrt.py

@@ -40,12 +40,13 @@
     options = RRTPlannerOptions(
         max_angle_step=0.05,
         max_connection_dist=0.25,
-        goal_biasing_probability=0.15,
-        max_planning_time=10.0,
         rrt_connect=False,
         bidirectional_rrt=False,
         rrt_star=False,
         max_rewire_dist=3.0,
+        max_planning_time=10.0,
+        fast_return=True,
+        goal_biasing_probability=0.15,
     )
 
     planner = RRTPlanner(model, collision_model, options=options)

src/pyroboplan/planning/graph.py

@@ -191,6 +191,8 @@ def remove_edge(self, edge):
     def get_nearest_node(self, q):
         """
         Gets the nearest node to a specified robot configuration.
+        If the configuration is in the graph the corresponding node will be returned. Namely,
+        the node with distance 0.
 
         Parameters
         ----------
@@ -204,6 +206,9 @@ def get_nearest_node(self, q):
         """
         nearest_node = None
         min_dist = np.inf
+        chk_node = Node(q)
+        if chk_node in self.nodes:
+            return self.nodes[chk_node]
 
         for node in self.nodes:
             dist = configuration_distance(q, node.q)

src/pyroboplan/planning/rrt.py

@@ -28,6 +28,7 @@ def __init__(
         rrt_star=False,
         max_rewire_dist=np.inf,
         max_planning_time=10.0,
+        fast_return=True,
         goal_biasing_probability=0.0,
     ):
         """
@@ -53,6 +54,9 @@ def __init__(
                 If set to `np.inf`, all nodes in the trees will be considered for rewiring.
             max_planning_time : float
                 Maximum planning time, in seconds.
+            fast_return : bool
+                If True, return as soon as a solution is found. Otherwise continuing building the tree
+                until max_planning_time is reached.
             goal_biasing_probability : float
                 Probability of sampling the goal configuration itself, which can help planning converge.
         """
@@ -63,6 +67,7 @@ def __init__(
         self.rrt_star = rrt_star
         self.max_rewire_dist = max_rewire_dist
         self.max_planning_time = max_planning_time
+        self.fast_return = fast_return
         self.goal_biasing_probability = goal_biasing_probability
 
 
@@ -143,11 +148,16 @@ def plan(self, q_start, q_goal):
             goal_found = True
 
         start_tree_phase = True
-        while not goal_found:
+        while True:
+            # Only return on success if specified in the options.
+            if goal_found and self.options.fast_return:
+                break
+
             # Check for timeouts.
             if time.time() - t_start > self.options.max_planning_time:
+                message = "succeeded" if goal_found else "timed out"
                 print(
-                    f"Planning timed out after {self.options.max_planning_time} seconds."
+                    f"Planning {message} after {self.options.max_planning_time} seconds."
                 )
                 break
 
@@ -218,6 +228,10 @@ def extend_or_connect(self, parent_node, q_sample):
             q_sample : array-like
                 The robot configuration sample to extend or connect towards.
         """
+        # If they are the same node there's nothing to do.
+        if np.array_equal(parent_node.q, q_sample):
+            return None
+
         q_diff = q_sample - parent_node.q
         q_increment = self.options.max_connection_dist * q_diff / np.linalg.norm(q_diff)