irsim.lib.path_planners.probabilistic_road_map#

Probabilistic Road Map (PRM) Planner.

Collision precedence:

  1. Grid lookup when env_map.grid is not None; if occupied, collision.

2. When the grid reports free or is unavailable, Shapely vs. obstacle_list. (Grid and obstacle_list are combined when both are present.)

author: Atsushi Sakai (@Atsushi_twi)

adapted by: Reinis Cimurs

Classes#

Node

Node class for dijkstra search

PRMPlanner

Initialize the PRM planner.

Module Contents#

class irsim.lib.path_planners.probabilistic_road_map.Node(x: float, y: float, cost: float, parent_index: int)[source]#

Node class for dijkstra search

Initialize Node

Parameters:

  • x (float) – x position of the node

  • y (float) – y position of the node

  • cost (float) – heuristic cost of the node

  • parent_index (int) – Nodes parent index

x#
y#
cost#
parent_index#
class irsim.lib.path_planners.probabilistic_road_map.PRMPlanner(env_map: irsim.world.map.EnvGridMap, robot_radius: float, n_sample: int = 500, n_knn: int = 10, max_edge_len: float = 30.0)[source]#

Initialize the PRM planner.

Parameters:

  • env_map – Environment map (any EnvGridMap compatible object).

  • robot_radius – Robot radius modeled as a circle.

  • n_sample – Number of sampled points.

  • n_knn – Number of nearest neighbors per node.

  • max_edge_len – Maximum allowed edge length.

rr#
obstacle_list#
min_x#
min_y#
max_x#
max_y#
n_sample = 500#
n_knn = 10#
max_edge_len = 30.0#
planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, rng: Any | None = None, show_animation: bool = True) → tuple[list[float], list[float]] | None[source]#

Plan a path from start to goal using the PRM method.

Parameters:

  • start_pose (np.array) – start pose [x,y]

  • goal_pose (np.array) – goal pose [x,y]

  • rng (Optional) – Random generator

  • show_animation (bool) – If true, shows the animation of planning process

Returns:

xy position array of the final path

Return type:

(np.array)

check_node(x: float, y: float, rr: float) → bool[source]#

Check position for a collision.

Parameters:

  • x – World x coordinate.

  • y – World y coordinate.

  • rr – Robot radius for the check.

Returns:

True if a collision is detected.

is_collision(sx: float, sy: float, gx: float, gy: float) → bool[source]#

Check if line between points is acceptable - within edge limits and free of collisions

Parameters:

  • sx (float) – start x position

  • sy (float) – start y position

  • gx (float) – goal x position

  • gy (float) – goal y position

Returns:

True if node is not acceptable. False otherwise

Return type:

result (bool)

generate_road_map(sample_x: list[float], sample_y: list[float]) → list[list[int]][source]#

Road map generation

Parameters:

  • sample_x (List) – [m] x positions of sampled points

  • sample_y (List) – [m] y positions of sampled points

Returns:

list of edge ids

Return type:

road_map (List)

static dijkstra_planning(sx: float, sy: float, gx: float, gy: float, road_map: list[list[int]], sample_x: list[float], sample_y: list[float], show_animation: bool) → tuple[list[float], list[float]] | None[source]#
Parameters:

  • sx (float) – start x position [m]

  • sy (float) – start y position [m]

  • gx (float) – goal x position [m]

  • gy (float) – goal y position [m]

  • road_map (list) – list of edge ids

  • sample_x (float) – ??? [m]

  • sample_y (float) – ??? [m]

Returns:

Two lists of path coordinates ([x1, x2, …], [y1, y2, …]), empty list when no path was found

Return type:

(tuple(list, list))

static plot_road_map(road_map: list[list[int]], sample_x: list[float], sample_y: list[float]) → None[source]#
sample_points(sx: float, sy: float, gx: float, gy: float, rng: Any | None) → tuple[list[float], list[float]][source]#

Generate sample points

Parameters:

  • sx (float) – start x position [m]

  • sy (float) – start y position [m]

  • gx (float) – goal x position [m]

  • gy (float) – goal y position [m]

  • rng – Random generator

Returns:

sample positions

Return type:

sample (tuple (list, list))