Coverage for src/pyroboplan/planning/prm.py: 71%
109 statements
« prev ^ index » next coverage.py v7.6.12, created at 2025-03-02 22:03 -0500
1"""Utilities for manipulation-specific Probabilistic Roadmaps (PRMs)."""
3import numpy as np
4import time
6from pyroboplan.planning.graph_search import astar
8from ..core.utils import (
9 check_collisions_at_state,
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 discretize_joint_space_path, has_collision_free_path
19def random_model_state_generator(model):
20 while True:
21 yield get_random_state(model)
24class PRMPlannerOptions:
25 """Options for Probabilistic Roadmap (PRM) planning."""
27 def __init__(
28 self,
29 max_step_size=0.05,
30 max_neighbor_radius=0.5,
31 max_neighbor_connections=15,
32 max_construction_nodes=5000,
33 construction_timeout=10.0,
34 rng_seed=None,
35 prm_star=False,
36 prm_file=None,
37 ):
38 """
39 Initializes a set of PRM planner options.
41 Parameters
42 ----------
43 max_step_size : float
44 Maximum joint configuration step size for collision checking along path segments.
45 max_neighbor_radius : float
46 The maximum allowable connectable distance between nodes.
47 max_neighbor_connections : float
48 The maximum number of neighbors to check when adding a node to the roadmap.
49 max_construction_nodes : int
50 The maximum number of samples to generate in the configuration space when growing the graph.
51 construction_timeout : float
52 Maximum time allotted to sample the configuration space per call.
53 rng_seed : int, optional
54 Sets the seed for random number generation. Use to generate deterministic results.
55 prm_star : str
56 If True, use the PRM* approach to dynamically select the radius and max number of neighbors
57 during construction of the roadmap.
58 prm_file : str, optional
59 Full file path of a persisted PRM graph to use in the planner.
60 If this is not specified, the PRM will be constructed from scratch.
61 """
62 self.max_step_size = max_step_size
63 self.max_neighbor_radius = max_neighbor_radius
64 self.max_neighbor_connections = max_neighbor_connections
65 self.rng_seed = rng_seed
66 self.construction_timeout = construction_timeout
67 self.max_construction_nodes = max_construction_nodes
68 self.prm_star = prm_star
69 self.prm_file = prm_file
72class PRMPlanner:
73 """Probabilistic Roadmap (PRM) planner.
75 This is a sampling-based motion planner that constructs a graph in the free configuration
76 space and then searches for a path using standard graph search functions.
78 Graphs can be persisted to disk for use in future applications.
80 Some helpful resources:
81 * The original publication:
82 https://www.kavrakilab.org/publications/kavraki-svestka1996probabilistic-roadmaps-for.pdf
83 * Modifications of PRM including PRM*:
84 https://people.eecs.berkeley.edu/~pabbeel/cs287-fa19/optreadings/rrtstar.pdf
85 * A nice guide to higher dimensional motion planning:
86 https://motion.cs.illinois.edu/RoboticSystems/MotionPlanningHigherDimensions.html
88 """
90 def __init__(self, model, collision_model, options=PRMPlannerOptions()):
91 """
92 Creates an instance of a PRM planner.
94 Parameters
95 ----------
96 model : `pinocchio.Model`
97 The model to use for this solver.
98 collision_model : `pinocchio.Model`
99 The model to use for collision checking.
100 options : `PRMPlannerOptions`, optional
101 The options to use for planning. If not specified, default options are used.
102 """
103 self.model = model
104 self.collision_model = collision_model
105 self.data = self.model.createData()
106 self.collision_data = self.collision_model.createData()
107 self.options = options
108 self.latest_path = None
110 if not self.options.prm_file: 110 ↛ 113line 110 didn't jump to line 113 because the condition on line 110 was always true
111 self.graph = Graph()
112 else:
113 self.graph = Graph.load_from_file(self.options.prm_file)
115 def construct_roadmap(self, sample_generator=None):
116 """
117 Grows the graph by sampling nodes using the provided generator, then connecting
118 them to the Graph. The caller can optionally override the default generator, if desired.
120 Parameters
121 ----------
122 sample_generator : Generator[array-like, None, None]
123 The sample function to use in construction of the roadmap.
124 Defaults to randomly sampling the robot's configuration space.
125 """
126 # Default to randomly sampling the model if no sample function is provided.
127 np.random.seed(self.options.rng_seed)
128 if not sample_generator: 128 ↛ 131line 128 didn't jump to line 131 because the condition on line 128 was always true
129 sample_generator = random_model_state_generator(self.model)
131 t_start = time.time()
132 added_nodes = 0
133 while added_nodes < self.options.max_construction_nodes:
134 if time.time() - t_start > self.options.construction_timeout:
135 print(
136 f"Roadmap construction timed out after {self.options.construction_timeout} seconds."
137 )
138 break
140 # At each iteration we naively sample a valid random state and attempt to connect it to the roadmap.
141 q_sample = next(sample_generator)
142 if check_collisions_at_state( 142 ↛ 149line 142 didn't jump to line 149 because the condition on line 142 was never true
143 self.model,
144 self.collision_model,
145 q_sample,
146 self.data,
147 self.collision_data,
148 ):
149 continue
151 radius = self.options.max_neighbor_radius
152 max_neighbors = self.options.max_neighbor_connections
154 # If using PRM* we dynamically scale the radius and max number of connections
155 # each iteration. The scaling is a function of log(num_nodes). For more info refer to section
156 # 3.3 of https://people.eecs.berkeley.edu/~pabbeel/cs287-fa19/optreadings/rrtstar.pdf.
157 if self.options.prm_star:
158 num_nodes = len(self.graph.nodes)
159 dimension = len(q_sample)
160 if num_nodes > 0:
161 radius = radius * (np.log(num_nodes) / num_nodes) ** (1 / dimension)
162 max_neighbors = int(max_neighbors * np.log(num_nodes))
164 # It's a valid configuration so add it to the roadmap
165 new_node = Node(q_sample)
166 self.graph.add_node(new_node)
167 self.connect_node(new_node, radius, max_neighbors)
168 added_nodes += 1
170 def connect_node(self, new_node, radius, k):
171 """
172 Identifies all neighbors and makes connections to the added node.
174 Parameters
175 ----------
176 parent_node : `pyroboplan.planning.graph.Node`
177 The node to add.
178 radius : float
179 Only consider connections within the provided radius.
180 k : int
181 Only consider up to a maximum of k neighbors to make connections.
183 Returns
184 -------
185 bool :
186 True if the node was connected to the graph. False otherwise.
187 """
188 # Add the node and find all neighbors.
189 neighbors = self.graph.get_nearest_neighbors(new_node.q, radius)
191 # Attempt to connect at most `max_neighbor_connections` neighbors.
192 success = False
193 for node, _ in neighbors[0:k]:
194 # If the nodes are connectable then add an edge.
195 if has_collision_free_path( 195 ↛ 193line 195 didn't jump to line 193 because the condition on line 195 was always true
196 node.q,
197 new_node.q,
198 self.options.max_step_size,
199 self.model,
200 self.collision_model,
201 ):
202 self.graph.add_edge(node, new_node)
203 success |= True
205 return success
207 def reset(self):
208 """
209 Resets the PRM's transient data between queries.
210 """
211 self.latest_path = None
212 for node in self.graph.nodes:
213 node.parent = None
214 node.cost = None
216 def connect_planning_nodes(self, start_node, goal_node):
217 """
218 Ensures the start and goal configurations can be connected to the PRM.
220 Parameters
221 ----------
222 start_node : `pyroboplan.planning.graph.Node`
223 The start node to connect.
224 goal_node : `pyroboplan.planning.graph.Node`
225 The goal node to connect.
227 Returns
228 -------
229 bool :
230 True if the nodes were able to be connected, False otherwise.
231 """
232 success = True
234 # If we cannot connect the start and goal nodes then there is no recourse.
235 if not self.connect_node( 235 ↛ 240line 235 didn't jump to line 240 because the condition on line 235 was never true
236 start_node,
237 self.options.max_neighbor_radius,
238 self.options.max_neighbor_connections,
239 ):
240 print("Failed to connect the start configuration to the PRM.")
241 success = False
242 if not self.connect_node( 242 ↛ 247line 242 didn't jump to line 247 because the condition on line 242 was never true
243 goal_node,
244 self.options.max_neighbor_radius,
245 self.options.max_neighbor_connections,
246 ):
247 print("Failed to connect the goal configuration to the PRM.")
248 success = False
250 return success
252 def plan(self, q_start, q_goal):
253 """
254 Plans a path from a start to a goal configuration using the constructed graph.
256 Parameters
257 ----------
258 q_start : array-like
259 The starting robot configuration.
260 q_goal : array-like
261 The goal robot configuration.
263 Returns
264 -------
265 list[array-like] :
266 A path from the start to the goal state, if one exists. Otherwise None.
267 """
269 # Check start and end pose collisions.
270 if check_collisions_at_state( 270 ↛ 273line 270 didn't jump to line 273 because the condition on line 270 was never true
271 self.model, self.collision_model, q_start, self.data, self.collision_data
272 ):
273 print("Start configuration in collision.")
274 return None
275 if check_collisions_at_state( 275 ↛ 278line 275 didn't jump to line 278 because the condition on line 275 was never true
276 self.model, self.collision_model, q_goal, self.data, self.collision_data
277 ):
278 print("Goal configuration in collision.")
279 return None
281 # Ensure the start and goal nodes are in the graph.
282 start_node = Node(q_start)
283 self.graph.add_node(start_node)
284 goal_node = Node(q_goal)
285 self.graph.add_node(goal_node)
287 # Ensure the start and goal nodes are connected before attempting to plan
288 path = None
289 if self.connect_planning_nodes(start_node, goal_node): 289 ↛ 299line 289 didn't jump to line 299 because the condition on line 289 was always true
291 # Use a graph search to determine if there is a path between the start and goal poses.
292 node_path = astar(self.graph, start_node, goal_node)
294 # Reconstruct the path if it exists
295 path = [node.q for node in node_path] if node_path else None
296 self.latest_path = path
298 # Always remove the start and end nodes from the PRM.
299 self.graph.remove_node(start_node)
300 self.graph.remove_node(goal_node)
302 return path
304 def visualize(
305 self,
306 visualizer,
307 frame_name,
308 path_name="planned_path",
309 graph_name="prm",
310 show_path=True,
311 show_graph=False,
312 ):
313 """
314 Visualizes the PRM and the latest path found within it.
316 Parameters
317 ----------
318 visualizer : `pinocchio.visualize.meshcat_visualizer.MeshcatVisualizer`, optional
319 The visualizer to use for this solver.
320 frame_name : str
321 The name of the frame to use when visualizing paths in Cartesian space.
322 path_name : str, optional
323 The name of the MeshCat component for the path.
324 graph_name : str, optional
325 The name of the MeshCat component for the tree.
326 show_path : bool, optional
327 If true, shows the final path from start to goal.
328 show_graph : bool, optional
329 If true, shows the entire PRM graph.
330 """
331 if show_graph:
332 path_tforms = []
333 for edge in self.graph.edges:
334 q_path = discretize_joint_space_path(
335 [edge.nodeA.q, edge.nodeB.q], self.options.max_step_size
336 )
337 path_tforms.append(
338 extract_cartesian_poses(self.model, frame_name, q_path)
339 )
340 visualize_paths(
341 visualizer,
342 f"{graph_name}/edges",
343 path_tforms,
344 line_width=0.5,
345 line_color=[0.9, 0.0, 0.9],
346 )
348 visualizer.viewer[path_name].delete()
349 if show_path and self.latest_path:
350 q_path = discretize_joint_space_path(
351 self.latest_path, self.options.max_step_size
352 )
354 target_tforms = extract_cartesian_poses(self.model, frame_name, q_path)
355 visualize_frames(
356 visualizer, path_name, target_tforms, line_length=0.05, line_width=2.0
357 )