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)[源代码]#

Initialize A* planner.

参数:: 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)[源代码]#

Node class

Initialize Node

参数:

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]][源代码]#

A star path search

参数:

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

返回:

xy position array of the final path

返回类型:

(np.array)

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

Generate the final path

参数:

goal_node (Node) -- final goal node
closed_set (dict) -- dict of closed nodes

返回:

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

返回类型:

rx (list)

static calc_heuristic(n1: Node, n2: Node) → float[源代码]#

calc_grid_position(index: int, min_position: float) → float[源代码]#

calc grid position

参数:

index (int) -- index of a node
min_position (float) -- min value of search space

返回:

position of coordinates along the given axis

返回类型:

(float)

calc_xy_index(position: float, min_pos: float) → int[源代码]#

calc xy index of node

参数:

position (float) -- position of a node
min_pos (float) -- min value of search space

返回:

index of position along the given axis

返回类型:

(int)

calc_grid_index(node: Node) → int[源代码]#

calc grid index of node

参数:: node (Node) -- node to calculate the index for
返回:: grid index of the node
返回类型:: (float)

verify_node(node: Node) → bool[源代码]#

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

参数:: node (Node) -- node to check
返回:: True if node is acceptable. False otherwise
返回类型:: (bool)

check_node(x: float, y: float) → bool[源代码]#

Check position for a collision.

参数:

x -- World x coordinate of the cell centre.
y -- World y coordinate of the cell centre.

返回:

True if a collision is detected.

static get_motion_model() → list[list[float]][源代码]#

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)[源代码]#

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[源代码]#

Informed RRT* path planning.

参数:

start_pose -- Start position [x, y].
goal_pose -- Goal position [x, y].
show_animation -- Render tree, ellipse and best path during planning.

返回:

(2, N) waypoint array or None.

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

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.

参数:: 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[源代码]#

JPS path search.

参数:

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

返回:

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).

返回类型:

np.ndarray | None

calc_grid_position(index: int, min_position: float) → float[源代码]#

calc_xy_index(position: float, min_pos: float) → int[源代码]#

calc_grid_index_from_xy(x: int, y: int) → int[源代码]#

is_collision(x: float, y: float) → bool[源代码]#: 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)[源代码]#

Initialize the PRM planner.

参数:

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[源代码]#

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

参数:

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

返回:

xy position array of the final path

返回类型:

(np.array)

check_node(x: float, y: float, rr: float) → bool[源代码]#

Check position for a collision.

参数:

x -- World x coordinate.
y -- World y coordinate.
rr -- Robot radius for the check.

返回:

True if a collision is detected.

is_collision(sx: float, sy: float, gx: float, gy: float) → bool[源代码]#

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

参数:

sx (float) -- start x position
sy (float) -- start y position
gx (float) -- goal x position
gy (float) -- goal y position

返回:

True if node is not acceptable. False otherwise

返回类型:

result (bool)

generate_road_map(sample_x: list[float], sample_y: list[float]) → list[list[int]][源代码]#

Road map generation

参数:

sample_x (List) -- [m] x positions of sampled points
sample_y (List) -- [m] y positions of sampled points

返回:

list of edge ids

返回类型:

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[源代码]#

参数:

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]

返回:

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

返回类型:

(tuple(list, list))

static plot_road_map(road_map: list[list[int]], sample_x: list[float], sample_y: list[float]) → None[源代码]#

sample_points(sx: float, sy: float, gx: float, gy: float, rng: Any | None) → tuple[list[float], list[float]][源代码]#

Generate sample points

参数:

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

返回:

sample positions

返回类型:

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)[源代码]#

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).

参数:

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)[源代码]#

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[源代码]#

Run RRT path planning.

参数:

start_pose -- Start position [x, y].
goal_pose -- Goal position [x, y].
show_animation -- Render the exploration tree during planning.

返回:

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

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

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[源代码]#: Uniform random sample with goal-bias.

is_collision(node: TreeNode) → bool[源代码]#

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[源代码]#: Render the RRT tree on the active matplotlib axes.

calc_dist_to_goal(x: float, y: float) → float[源代码]#: Euclidean distance from (x, y) to the goal.

static calc_distance_and_angle(from_node: TreeNode, to_node: TreeNode) → tuple[float, float][源代码]#: 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)[源代码]#

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[源代码]#

RRT* path planning.

参数:

start_pose -- Start position [x, y].
goal_pose -- Goal position [x, y].
show_animation -- Render the tree during planning.

返回:

(2, N) waypoint array or None.