irsim.lib.path_planners#

Path planning algorithms for IR-SIM simulation.

This package contains: - a_star: A* path planning algorithm - rrt: Rapidly-exploring Random Tree algorithm - rrt_star: RRT* optimized path planning - probabilistic_road_map: PRM path planning algorithm

Submodules#

Classes#

`AStarPlanner`	Initialize A* planner.
`PRMPlanner`	Initialize the PRM planner.
`RRT`	Class for RRT planning
`RRTStar`	Class for RRT Star planning

Package Contents#

class irsim.lib.path_planners.AStarPlanner(env_map: irsim.world.map.Map, resolution: float)[source]#

Initialize A* planner.

Parameters:

env_map (Map) – Environment map where planning takes place.
resolution (float) – Grid resolution in meters.

resolution#

obstacle_list#

x_width#

y_width#

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: int, y: int) → bool[source]#

Check positon for a collision

Parameters:

x (float) – x value of the position
y (float) – y value of the position

Returns:

True if there is a collision. False otherwise

Return type:

result (bool)

static get_motion_model() → list[list[float]][source]#

class irsim.lib.path_planners.PRMPlanner(env_map: irsim.world.map.Map, 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 (Map) – Environment map where planning takes place.
robot_radius (float) – Robot radius modeled as a circle.
n_sample (int) – Number of sampled points.
n_knn (int) – Number of nearest neighbors per node.
max_edge_len (float) – Maximum allowed edge length.

rr#

obstacle_list#

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 positon for a collision

Parameters:

x (float) – x value of the position
y (float) – y value of the position

Returns:

True if there is a collision. False otherwise

Return type:

(bool)

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.Map, robot_radius: float, expand_dis: float = 1.0, path_resolution: float = 0.25, goal_sample_rate: int = 5, max_iter: int = 500)[source]#

Class for RRT planning

Initialize RRT planner

Parameters:

env_map (Env) – environment map where the planning will take place
robot_radius (float) – robot body modeled as circle with given radius
expand_dis (float) – expansion distance
path_resolution (float) – resolution of the path
goal_sample_rate (int) – goal sample rate
max_iter (int) – max iteration count

class Node(x: float, y: float)[source]#

RRT Node

Initialize Node

Parameters:

x (float) – x position of the node
y (float) – y position of the node

x#

y#

path_x = []#

path_y = []#

parent = None#

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

Area Bounds

Initialize AreaBounds

Parameters:: env_map (EnvBase) – environment where the planning will take place

obstacle_list#

play_area#

min_rand = 0.0#

max_rand#

expand_dis = 1.0#

path_resolution = 0.25#

goal_sample_rate = 5#

max_iter = 500#

node_list = []#

robot_radius#

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

rrt path planning

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)

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

Generate a new node by steering from from_node towards to_node.

This method incrementally moves from from_node in the direction of to_node, using a fixed step size (self.path_resolution) and not exceeding the specified extend_length. The result is a new node that approximates a path from the start node toward the goal, constrained by resolution and maximum step distance.

If the final position is within one resolution step of to_node, it snaps the new node exactly to to_node.

Parameters:

from_node (Node) – The node from which to begin extending.
to_node (Node) – The target node to steer toward.
extend_length (float, optional) – The maximum length to extend. Defaults to infinity.

Returns:

A new node with updated position, path history (path_x, path_y),

Return type:

(Node)

generate_final_course(goal_ind: int) → tuple[list[float], list[float]][source]#

Generate the final path

Parameters:: goal_ind (int) – index of the final goal
Returns:: xy position array of the final path
Return type:: (np.array)

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

Calculate distance to goal

Parameters:

x (float) – x coordinate of the position
y (float) – y coordinate of the position

Returns:

distance to the goal

Return type:

(float)

get_random_node() → Node[source]#

Create random node

Returns:: new random node
Return type:: (Node)

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

Render the RRT tree and optional sampled node for visualization.

Parameters:: rnd (Node | None) – Optional random node to highlight on the plot.

static plot_circle(x: float, y: float, size: float, color: str = '-b') → None[source]#

Plot a circle at a given position.

Parameters:

x (float) – Center x coordinate.
y (float) – Center y coordinate.
size (float) – Circle radius.
color (str) – Matplotlib color/style string.

static get_nearest_node_index(node_list: list[Node], rnd_node: Node) → int[source]#

Return the index of the nearest node in the list to a target node.

Parameters:

node_list (list[Node]) – List of existing nodes.
rnd_node (Node) – Target node to compare distances against.

Returns:

Index of the nearest node.

Return type:

int

static check_if_outside_play_area(node: Node, play_area: AreaBounds) → bool[source]#

Check whether the node is inside the defined play area bounds.

Parameters:

node (Node) – Node to check.
play_area (AreaBounds) – World bounds.

Returns:

True if inside bounds (or no bounds defined); False otherwise.

Return type:

bool

check_collision(node: Node, robot_radius: float) → bool[source]#

Check if node is acceptable - free of collisions

Parameters:

node (Node) – node to check
robot_radius (float) – robot radius

Returns:

True if there is no collision. False otherwise

Return type:

(bool)

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

Check positon for a collision

Parameters:

x (float) – x value of the position
y (float) – y value of the position
rr (float) – robot radius

Returns:

True if there is a collision. False otherwise

Return type:

(bool)

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

Compute Euclidean distance and heading from one node to another.

Parameters:

from_node (Node) – Start node.
to_node (Node) – Target node.

Returns:

Distance and angle (radians) toward the target.

Return type:

tuple[float, float]

class irsim.lib.path_planners.RRTStar(env_map: irsim.world.map.Map, robot_radius: float, 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

Class for RRT Star planning

Initialize RRT planner

Parameters:

env_map (Env) – environment map where the planning will take place
robot_radius (float) – robot body modeled as circle with given radius
expand_dis (float) – expansion distance
path_resolution (float) – resolution of the path
goal_sample_rate (int) – goal sample rate
max_iter (int) – max iteration count
connect_circle_dist (float) – maximum distance for nearby node connection ir rewiring
search_until_max_iter (bool) – if to search for path until the max_iter count

class Node(x: float, y: float)[source]#

Bases: irsim.lib.path_planners.rrt.RRT.Node

RRT Node

Initialize Node

Parameters:

x (float) – x position of the node
y (float) – y position of the node

cost = 0.0#

connect_circle_dist = 0.5#

search_until_max_iter = False#

node_list = []#

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

rrt star path planning

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)

choose_parent(new_node: Node, near_inds: list[int]) → Node | None[source]#

Computes the cheapest point to new_node contained in the list near_inds and set such a node as the parent of new_node.

Parameters:

new_node (Node) – randomly generated node with a path from its neared point
near_inds (List) – indices of the nodes what are near to new_node

Returns:

a copy of new_node

Return type:

(Node)

search_best_goal_node() → int | None[source]#

Search for the best goal node in the current RRT* tree.

Returns:: Index of the best goal node if found; otherwise None.
Return type:: Optional[int]

find_near_nodes(new_node: Node) → list[int][source]#

defines a ball centered on new_node
Returns all nodes of the three that are inside this ball

Args:
new_node (Node): new randomly generated node, without collisions between it and its nearest node

Returns:
list[int]: Indices of nodes inside the ball radius.

rewire(new_node: Node, near_inds: list[int]) → None[source]#

Rewire the tree to improve path cost by checking nearby nodes.

For each node in near_inds, this method checks whether it is more cost-effective to reach it via new_node. If so, the parent of that node is updated to new_node, and the cost is propagated to its children.

Parameters:

new_node (Node) – The newly added node that may provide a better path.
near_inds (list of int) – Indices of nodes within the connection radius that are candidates for rewiring.

Returns:

None

calc_new_cost(from_node: Node, to_node: Node) → float[source]#

Calculate the cost of reaching to_node from from_node.

Parameters:

from_node (Node) – The starting node.
to_node (Node) – The target node.

Returns:

The total cost to reach to_node via from_node.

Return type:

float

propagate_cost_to_leaves(parent_node: Node) → None[source]#

Recursively update the cost of all descendant nodes.

This function updates the cost of all child nodes of the given parent_node by recalculating their cost, and then propagates the update down to their children.

Parameters:: parent_node (Node) – The node from which to start cost propagation.
Returns:: None

irsim.lib.path_planners#

Submodules#

Classes#

Package Contents#

This Page