Coverage for src/pyroboplan/planning/rrt.py: 77%
155 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Utilities for manipulation-specific Rapidly-Exploring Random Trees (RRTs)."""
3import numpy as np
4import time
6from ..core.utils import (
7 check_collisions_at_state,
8 check_collisions_along_path,
9 configuration_distance,
10 extract_cartesian_poses,
11 get_random_state,
12)
13from ..visualization.meshcat_utils import visualize_frames, visualize_paths
15from .graph import Node, Graph
16from .utils import (
17 discretize_joint_space_path,
18 extend_robot_state,
19 has_collision_free_path,
20 retrace_path,
21)
24class RRTPlannerOptions:
25 """Options for Rapidly-exploring Random Tree (RRT) planning."""
27 def __init__(
28 self,
29 max_step_size=0.05,
30 max_connection_dist=np.inf,
31 rrt_connect=False,
32 bidirectional_rrt=False,
33 rrt_star=False,
34 max_rewire_dist=np.inf,
35 max_planning_time=10.0,
36 rng_seed=None,
37 fast_return=True,
38 goal_biasing_probability=0.0,
39 collision_distance_padding=0.0,
40 ):
41 """
42 Initializes a set of RRT planner options.
44 Parameters
45 ----------
46 max_step_size : float
47 Maximum joint configuration step size for collision checking along path segments.
48 max_connection_dist : float
49 Maximum angular distance, in radians, for connecting nodes.
50 rrt_connect : bool
51 If true, enables the RRTConnect algorithm, which incrementally extends the most
52 recently sampled node in the tree until an invalid state is reached.
53 bidirectional_rrt : bool
54 If true, uses bidirectional RRTs from both start and goal nodes.
55 Otherwise, only grows a tree from the start node.
56 rrt_star : bool
57 If true, enables the RRT* algorithm to shortcut node connections during planning.
58 This in turn will use the `max_rewire_dist` parameter.
59 max_rewire_dist : float
60 Maximum angular distance, in radians, to consider rewiring nodes for RRT*.
61 If set to `np.inf`, all nodes in the trees will be considered for rewiring.
62 max_planning_time : float
63 Maximum planning time, in seconds.
64 rng_seed : int, optional
65 Sets the seed for random number generation. Use to generate deterministic results.
66 fast_return : bool
67 If True, return as soon as a solution is found. Otherwise continuing building the tree
68 until max_planning_time is reached.
69 goal_biasing_probability : float
70 Probability of sampling the goal configuration itself, which can help planning converge.
71 collision_distance_padding : float
72 The padding, in meters, to use for distance to nearest collision.
73 """
74 self.max_step_size = max_step_size
75 self.max_connection_dist = max_connection_dist
76 self.rrt_connect = rrt_connect
77 self.bidirectional_rrt = bidirectional_rrt
78 self.rrt_star = rrt_star
79 self.max_rewire_dist = max_rewire_dist
80 self.max_planning_time = max_planning_time
81 self.rng_seed = rng_seed
82 self.fast_return = fast_return
83 self.goal_biasing_probability = goal_biasing_probability
84 self.collision_distance_padding = collision_distance_padding
87class RRTPlanner:
88 """Rapidly-expanding Random Tree (RRT) planner.
90 This is a sampling-based motion planner that finds collision-free paths from a start to a goal configuration.
92 Some good resources:
93 * Original RRT paper: https://msl.cs.illinois.edu/~lavalle/papers/Lav98c.pdf
94 * RRTConnect paper: https://www.cs.cmu.edu/afs/cs/academic/class/15494-s14/readings/kuffner_icra2000.pdf
95 * RRT* and PRM* paper: https://arxiv.org/abs/1105.1186
96 """
98 def __init__(self, model, collision_model, options=RRTPlannerOptions()):
99 """
100 Creates an instance of an RRT planner.
102 Parameters
103 ----------
104 model : `pinocchio.Model`
105 The model to use for this solver.
106 collision_model : `pinocchio.Model`
107 The model to use for collision checking.
108 options : `RRTPlannerOptions`, optional
109 The options to use for planning. If not specified, default options are used.
110 """
111 self.model = model
112 self.collision_model = collision_model
113 self.data = self.model.createData()
114 self.collision_data = self.collision_model.createData()
115 self.options = options
116 self.reset()
118 def reset(self):
119 """Resets all the planning data structures."""
120 self.latest_path = None
121 self.start_tree = Graph()
122 self.goal_tree = Graph()
123 np.random.seed(self.options.rng_seed)
125 def plan(self, q_start, q_goal):
126 """
127 Plans a path from a start to a goal configuration.
129 Parameters
130 ----------
131 q_start : array-like
132 The starting robot configuration.
133 q_goal : array-like
134 The goal robot configuration.
135 """
136 self.reset()
137 t_start = time.time()
139 start_node = Node(q_start, parent=None, cost=0.0)
140 self.start_tree.add_node(start_node)
141 goal_node = Node(q_goal, parent=None, cost=0.0)
142 self.goal_tree.add_node(goal_node)
144 goal_found = False
145 latest_start_tree_node = start_node
146 latest_goal_tree_node = goal_node
148 # Check start and end pose collisions.
149 if check_collisions_at_state( 149 ↛ 157line 149 didn't jump to line 157 because the condition on line 149 was never true
150 self.model,
151 self.collision_model,
152 q_start,
153 self.data,
154 self.collision_data,
155 distance_padding=self.options.collision_distance_padding,
156 ):
157 print("Start configuration in collision.")
158 return None
159 if check_collisions_at_state( 159 ↛ 167line 159 didn't jump to line 167 because the condition on line 159 was never true
160 self.model,
161 self.collision_model,
162 q_goal,
163 self.data,
164 self.collision_data,
165 distance_padding=self.options.collision_distance_padding,
166 ):
167 print("Goal configuration in collision.")
168 return None
170 # Check direct connection to goal.
171 if configuration_distance(q_start, q_goal) <= self.options.max_connection_dist: 171 ↛ 187line 171 didn't jump to line 187 because the condition on line 171 was always true
172 path_to_goal = discretize_joint_space_path(
173 [q_start, q_goal], self.options.max_step_size
174 )
175 if not check_collisions_along_path(
176 self.model,
177 self.collision_model,
178 path_to_goal,
179 distance_padding=self.options.collision_distance_padding,
180 ):
181 latest_start_tree_node = self.add_node_to_tree(
182 self.start_tree, q_goal, start_node
183 )
184 print("Start and goal can be directly connected!")
185 goal_found = True
187 start_tree_phase = True
188 while True:
189 # Only return on success if specified in the options.
190 if goal_found and self.options.fast_return:
191 break
193 # Check for timeouts.
194 if time.time() - t_start > self.options.max_planning_time: 194 ↛ 195line 194 didn't jump to line 195 because the condition on line 194 was never true
195 message = "succeeded" if goal_found else "timed out"
196 print(
197 f"Planning {message} after {self.options.max_planning_time} seconds."
198 )
199 break
201 # Choose variables based on whether we're growing the start or goal tree.
202 tree = self.start_tree if start_tree_phase else self.goal_tree
203 other_tree = self.goal_tree if start_tree_phase else self.start_tree
205 # Sample a new configuration.
206 if np.random.random() < self.options.goal_biasing_probability: 206 ↛ 207line 206 didn't jump to line 207 because the condition on line 206 was never true
207 q_sample = q_goal if start_tree_phase else q_start
208 else:
209 q_sample = get_random_state(self.model)
211 # Run the extend or connect operation to connect the tree to the new node.
212 nearest_node = tree.get_nearest_node(q_sample)
213 q_new = self.extend_or_connect(nearest_node, q_sample)
215 # Only if extend/connect succeeded, add the new node to the tree.
216 if q_new is not None:
217 new_node = self.add_node_to_tree(tree, q_new, nearest_node)
218 if start_tree_phase:
219 latest_start_tree_node = new_node
220 else:
221 latest_goal_tree_node = new_node
223 # Check if latest node connects directly to the other tree.
224 # If so, add it to the tree and mark planning as complete.
225 nearest_node_in_other_tree = other_tree.get_nearest_node(new_node.q)
226 distance_to_other_tree = configuration_distance(
227 new_node.q, nearest_node_in_other_tree.q
228 )
229 if distance_to_other_tree <= self.options.max_connection_dist: 229 ↛ 253line 229 didn't jump to line 253 because the condition on line 229 was always true
230 path_to_other_tree = discretize_joint_space_path(
231 [new_node.q, nearest_node_in_other_tree.q],
232 self.options.max_step_size,
233 )
234 if not check_collisions_along_path(
235 self.model,
236 self.collision_model,
237 path_to_other_tree,
238 distance_padding=self.options.collision_distance_padding,
239 ):
240 if distance_to_other_tree > 0: 240 ↛ 244line 240 didn't jump to line 244 because the condition on line 240 was always true
241 new_node = self.add_node_to_tree(
242 tree, nearest_node_in_other_tree.q, new_node
243 )
244 if start_tree_phase:
245 latest_start_tree_node = new_node
246 latest_goal_tree_node = nearest_node_in_other_tree
247 else:
248 latest_start_tree_node = nearest_node_in_other_tree
249 latest_goal_tree_node = new_node
250 goal_found = True
252 # Switch to the other tree next iteration, if bidirectional mode is enabled.
253 if self.options.bidirectional_rrt:
254 start_tree_phase = not start_tree_phase
256 # Back out the path by traversing the parents from the goal.
257 self.latest_path = []
258 if goal_found: 258 ↛ 262line 258 didn't jump to line 262 because the condition on line 258 was always true
259 self.latest_path = self.extract_path_from_trees(
260 latest_start_tree_node, latest_goal_tree_node
261 )
262 return self.latest_path
264 def extend_or_connect(self, parent_node, q_sample):
265 """
266 Extends a tree towards a sampled node with steps no larger than the maximum connection distance.
268 Parameters
269 ----------
270 parent_node : `pyroboplan.planning.graph.Node`
271 The node from which to start extending or connecting towards the sample.
272 q_sample : array-like
273 The robot configuration sample to extend or connect towards.
274 """
275 # If they are the same node there's nothing to do.
276 if np.array_equal(parent_node.q, q_sample): 276 ↛ 277line 276 didn't jump to line 277 because the condition on line 276 was never true
277 return None
279 q_out = None
280 q_cur = parent_node.q
281 while True:
282 # Compute the next incremental robot configuration.
283 q_extend = extend_robot_state(
284 q_cur,
285 q_sample,
286 self.options.max_connection_dist,
287 )
289 # If we can connect then it is a valid state
290 if has_collision_free_path(
291 q_cur,
292 q_extend,
293 self.options.max_step_size,
294 self.model,
295 self.collision_model,
296 distance_padding=self.options.collision_distance_padding,
297 ):
298 q_out = q_cur = q_extend
299 # If we have reached the sampled state then we are done.
300 if np.array_equal(q_cur, q_sample): 300 ↛ 306line 300 didn't jump to line 306 because the condition on line 300 was always true
301 break
302 else:
303 break
305 # If RRTConnect is disabled, only one iteration is needed.
306 if not self.options.rrt_connect:
307 break
309 return q_out
311 def extract_path_from_trees(self, start_tree_final_node, goal_tree_final_node):
312 """
313 Extracts the final path from the RRT trees
315 from the start tree root to the goal tree root passing through both final nodes.
317 Parameters
318 ----------
319 start_tree_final_node : `pyroboplan.planning.graph.Node`
320 The last node of the start tree.
321 goal_tree_final_node : `pyroboplan.planning.graph.Node`, optional
322 The last node of the goal tree.
323 If None, this means the goal tree is ignored.
325 Return
326 ------
327 list[array-like]
328 A list of robot configurations describing the path waypoints in order.
329 """
330 path = retrace_path(start_tree_final_node)
332 # extract and reverse the goal tree path to append to the start tree path
333 if goal_tree_final_node: 333 ↛ 340line 333 didn't jump to line 340 because the condition on line 333 was always true
334 # the final node itself is already in the start path
335 goal_tree_path = retrace_path(goal_tree_final_node.parent)
336 goal_tree_path.reverse()
337 path += goal_tree_path
339 # Convert to robot configuration states
340 return [n.q for n in path]
342 def add_node_to_tree(self, tree, q_new, parent_node):
343 """
344 Add a new node to the tree. If the RRT* algorithm is enabled, will also rewire.
346 Parameters
347 ----------
348 tree : `pyroboplan.planning.graph.Graph`
349 The tree to which to add the new node.
350 q_new : array-like
351 The robot configuration from which to create a new tree node.
352 parent_node : `pyroboplan.planning.graph.Node`
353 The parent node to connect the new node to.
355 Returns
356 -------
357 `pyroboplan.planning.graph.Node`
358 The new node that was added to the tree.
359 """
360 # Add the new node to the tree
361 new_node = Node(q_new, parent=parent_node)
362 tree.add_node(new_node)
363 edge = tree.add_edge(parent_node, new_node)
364 new_node.cost = parent_node.cost + edge.cost
366 # If RRT* is enable it, rewire that node in the tree.
367 if self.options.rrt_star:
368 min_cost = new_node.cost
369 for other_node in tree.nodes:
370 # Do not consider trivial nodes.
371 if other_node == new_node or other_node == parent_node:
372 continue
373 # Do not consider nodes farther than the configured rewire distance,
374 new_distance = configuration_distance(other_node.q, q_new)
375 if new_distance > self.options.max_rewire_dist: 375 ↛ 376line 375 didn't jump to line 376 because the condition on line 375 was never true
376 continue
377 # Rewire if this new connections would be of lower cost and is collision free.
378 new_cost = other_node.cost + new_distance
379 if new_cost < min_cost:
380 new_path = discretize_joint_space_path(
381 [q_new, other_node.q], self.options.max_step_size
382 )
383 if not check_collisions_along_path(
384 self.model,
385 self.collision_model,
386 new_path,
387 distance_padding=self.options.collision_distance_padding,
388 ):
389 new_node.parent = other_node
390 new_node.cost = new_cost
391 tree.remove_edge(parent_node, new_node)
392 edge = tree.add_edge(other_node, new_node)
393 min_cost = new_cost
395 return new_node
397 def visualize(
398 self,
399 visualizer,
400 frame_name,
401 path_name="planned_path",
402 tree_name="rrt",
403 show_path=True,
404 show_tree=False,
405 ):
406 """
407 Visualizes the RRT path.
409 Parameters
410 ----------
411 visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional
412 The visualizer to use for this solver.
413 frame_name : str
414 The name of the frame to use when visualizing paths in Cartesian space.
415 path_name : str, optional
416 The name of the MeshCat component for the path.
417 tree_name : str, optional
418 The name of the MeshCat component for the tree.
419 show_path : bool, optional
420 If true, shows the final path from start to goal.
421 show_tree : bool, optional
422 If true, shows the entire sampled tree.
423 """
424 visualizer.viewer[path_name].delete()
425 if show_path:
426 q_path = discretize_joint_space_path(
427 self.latest_path, self.options.max_step_size
428 )
430 target_tforms = extract_cartesian_poses(self.model, frame_name, q_path)
431 visualize_frames(
432 visualizer, path_name, target_tforms, line_length=0.05, line_width=1.5
433 )
435 if show_tree:
436 start_path_tforms = []
437 for edge in self.start_tree.edges:
438 q_path = discretize_joint_space_path(
439 [edge.nodeA.q, edge.nodeB.q], self.options.max_step_size
440 )
441 start_path_tforms.append(
442 extract_cartesian_poses(self.model, frame_name, q_path)
443 )
444 visualize_paths(
445 visualizer,
446 f"{tree_name}_start/edges",
447 start_path_tforms,
448 line_width=0.5,
449 line_color=[0.9, 0.0, 0.9],
450 )
452 goal_path_tforms = []
453 for edge in self.goal_tree.edges:
454 q_path = discretize_joint_space_path(
455 [edge.nodeA.q, edge.nodeB.q], self.options.max_step_size
456 )
457 goal_path_tforms.append(
458 extract_cartesian_poses(self.model, frame_name, q_path)
459 )
460 visualize_paths(
461 visualizer,
462 f"{tree_name}_goal/edges",
463 goal_path_tforms,
464 line_width=0.5,
465 line_color=[0.0, 0.9, 0.9],
466 )