irsim.lib.path_planners#

Path planning algorithms for IR-SIM simulation.

This package contains: - a_star: A* path planning algorithm - jps: Jump Point Search (JPS) grid planning - rrt: Rapidly-exploring Random Tree algorithm - rrt_star: RRT* optimized path planning - informed_rrt_star: Informed RRT* optimal path planning - probabilistic_road_map: PRM path planning algorithm

Submodules#

Classes#

AStarPlanner

Initialize A* planner.

InformedRRTStar

Informed RRT* path planner.

JPSPlanner

Jump Point Search planner for uniform-cost 8-connected grids.

PRMPlanner

Initialize the PRM planner.

RRT

Rapidly-exploring Random Tree (RRT) path planner.

RRTStar

RRT* path planner with asymptotic optimality.

Package Contents#

class irsim.lib.path_planners.AStarPlanner(env_map: irsim.world.map.EnvGridMap)[source]#

Initialize A* planner.

Parameters:

env_map – Environment map (any EnvGridMap compatible object).

obstacle_list#
origin_x#
origin_y#
max_x#
max_y#
motion#
class Node(x: int, y: int, cost: float, parent_index: int)[source]#

Node class

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#
planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, show_animation: bool = True) → tuple[list[float], list[float]][source]#

A star path search

Parameters:

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

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

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

Returns:

xy position array of the final path

Return type:

(np.array)

calc_final_path(goal_node: Node, closed_set: dict) → tuple[list[float], list[float]][source]#

Generate the final path

Parameters:

  • goal_node (Node) – final goal node

  • closed_set (dict) – dict of closed nodes

Returns:

list of x positions of final path ry (list): list of y positions of final path

Return type:

rx (list)

static calc_heuristic(n1: Node, n2: Node) → float[source]#
calc_grid_position(index: int, min_position: float) → float[source]#

calc grid position

Parameters:

  • index (int) – index of a node

  • min_position (float) – min value of search space

Returns:

position of coordinates along the given axis

Return type:

(float)

calc_xy_index(position: float, min_pos: float) → int[source]#

calc xy index of node

Parameters:

  • position (float) – position of a node

  • min_pos (float) – min value of search space

Returns:

index of position along the given axis

Return type:

(int)

calc_grid_index(node: Node) → int[source]#

calc grid index of node

Parameters:

node (Node) – node to calculate the index for

Returns:

grid index of the node

Return type:

(float)

verify_node(node: Node) → bool[source]#

Check if node is acceptable - within limits of search space and free of collisions

Parameters:

node (Node) – node to check

Returns:

True if node is acceptable. False otherwise

Return type:

(bool)

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

Check position for a collision.

Parameters:

  • x – World x coordinate of the cell centre.

  • y – World y coordinate of the cell centre.

Returns:

True if a collision is detected.

static get_motion_model() → list[list[float]][source]#
class irsim.lib.path_planners.InformedRRTStar(env_map: irsim.world.map.EnvGridMap, robot: Any, expand_dis: float = 1.5, path_resolution: float = 0.25, goal_sample_rate: int = 10, max_iter: int = 500, connect_circle_dist: float = 50.0, search_until_max_iter: bool = True)[source]#

Bases: irsim.lib.path_planners.rrt_star.RRTStar

Informed RRT* path planner.

After finding an initial feasible path the sampler switches from uniform random sampling to sampling inside an informed ellipsoidal set whose foci are the start and goal. This dramatically speeds up convergence towards the optimal path.

Initialise the Informed RRT* planner.

Node#
planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) → numpy.ndarray | None[source]#

Informed RRT* path planning.

Parameters:

  • start_pose – Start position [x, y].

  • goal_pose – Goal position [x, y].

  • show_animation – Render tree, ellipse and best path during planning.

Returns:

(2, N) waypoint array or None.

class irsim.lib.path_planners.JPSPlanner(env_map: irsim.world.map.EnvGridMap)[source]#

Jump Point Search planner for uniform-cost 8-connected grids.

When the environment map carries an occupancy grid (env_map.grid), collision checks use a fast O(1) grid lookup. Otherwise, the planner falls back to Shapely geometry intersection (same as AStarPlanner).

Initialize JPS planner.

Parameters:

env_map – Environment map (any EnvGridMap compatible object). Resolution and bounds are taken from the map (same as AStarPlanner).

origin_x#
origin_y#
max_x#
max_y#
obstacle_list#
planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, show_animation: bool = True) → numpy.ndarray | None[source]#

JPS path search.

Parameters:

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

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

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

Returns:

shape (2, N) array [rx, ry] of the final path, or None if the start or goal cell is not walkable, or if no path exists (open set exhausted).

Return type:

np.ndarray | None

calc_grid_position(index: int, min_position: float) → float[source]#
calc_xy_index(position: float, min_pos: float) → int[source]#
calc_grid_index_from_xy(x: int, y: int) → int[source]#
is_collision(x: float, y: float) → bool[source]#

True if world position (x, y) is in collision.

class irsim.lib.path_planners.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))

class irsim.lib.path_planners.RRT(env_map: irsim.world.map.EnvGridMap, robot: irsim.world.object_base.ObjectBase, expand_dis: float = 1.0, path_resolution: float = 0.25, goal_sample_rate: int = 5, max_iter: int = 500)[source]#

Rapidly-exploring Random Tree (RRT) path planner.

Algorithmic improvements over the naive implementation (inspired by ZJU-FAST-Lab/sampling-based-path-finding):

  • Nodes track their children, enabling O(subtree) cost propagation via BFS instead of O(n) linear scans.

  • scipy.spatial.KDTree is used for nearest-neighbour and range queries when available, falling back to numpy otherwise.

  • A dedicated goal node is kept in the tree; whenever a new node can reach the goal at a lower cost the goal’s parent is rewired.

Initialise the RRT planner.

Robot shape is taken from robot.original_geometry (e.g. pass robot=env.robot).

Parameters:

  • env_map – Environment map (EnvGridMap).

  • robot – Robot object; its original_geometry is used for collision.

  • expand_dis – Maximum extension distance per steer step.

  • path_resolution – Discretisation resolution along steered edges.

  • goal_sample_rate – Percentage chance of sampling the goal.

  • max_iter – Maximum number of iterations.

Node#
class AreaBounds(env_map: irsim.world.map.EnvGridMap)[source]#

Rectangular play-area bounds in world coordinates.

xmin: float#
ymin: float#
xmax: float#
ymax: float#
obstacle_list#
play_area#
max_x#
max_y#
min_rand#
max_rand#
expand_dis = 1.0#
path_resolution = 0.25#
goal_sample_rate = 5#
max_iter = 500#
node_list = []#
start = None#
end = None#
robot_radius#
planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) → numpy.ndarray | None[source]#

Run RRT path planning.

Parameters:

  • start_pose – Start position [x, y].

  • goal_pose – Goal position [x, y].

  • show_animation – Render the exploration tree during planning.

Returns:

(2, N) numpy array [rx, ry] or None.

steer(from_node: TreeNode, to_node: TreeNode, extend_length: float = float('inf')) → TreeNode[source]#

Steer from from_node towards to_node.

Returns a temporary TreeNode (not yet registered in the tree) with path_x/path_y populated and parent set to from_node.

get_random_node() → TreeNode[source]#

Uniform random sample with goal-bias.

is_collision(node: TreeNode) → bool[source]#

Check whether node’s edge is collision-free.

Uses _collision_geometry translated to each path point. Returns True if the path is collision-free.

draw_graph(rnd: TreeNode | None = None) → None[source]#

Render the RRT tree on the active matplotlib axes.

calc_dist_to_goal(x: float, y: float) → float[source]#

Euclidean distance from (x, y) to the goal.

static calc_distance_and_angle(from_node: TreeNode, to_node: TreeNode) → tuple[float, float][source]#

Euclidean distance and heading between two nodes.

class irsim.lib.path_planners.RRTStar(env_map: irsim.world.map.EnvGridMap, robot: Any, expand_dis: float = 1.5, path_resolution: float = 0.25, goal_sample_rate: int = 5, max_iter: int = 500, connect_circle_dist: float = 0.5, search_until_max_iter: bool = False)[source]#

Bases: irsim.lib.path_planners.rrt.RRT

RRT* path planner with asymptotic optimality.

Extends RRT with:

  • Neighbour search β€” after steering, all nodes within a shrinking radius are considered as potential parents (_choose_parent).

  • Rewiring β€” after inserting a new node, nearby nodes are checked to see whether their cost can be reduced by re-parenting through the new node (_rewire).

  • Efficient cost propagation β€” _change_node_parent uses BFS through the children list for O(subtree) updates.

Initialise the RRT* planner.

Node#
connect_circle_dist = 0.5#
search_until_max_iter = False#
planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) → numpy.ndarray | None[source]#

RRT* path planning.

Parameters:

  • start_pose – Start position [x, y].

  • goal_pose – Goal position [x, y].

  • show_animation – Render the tree during planning.

Returns:

(2, N) waypoint array or None.

On this page
Show Source