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 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 (Env) – environment map where the planning will take place

  • resolution (float) – grid resolution [m]

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: list[float], goal_pose: list[float], 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 PRM planner

Parameters:

  • env_map (Env) – environment map where the planning will take place

  • robot_radius (float) – robot body modeled as circle with given radius

  • n_sample (int) – number of samples

  • n_knn (int) – number of edges

  • max_edge_len (float) – max edge length

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

A star path search

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]#
static plot_circle(x: float, y: float, size: float, color: str = '-b') → None[source]#
static get_nearest_node_index(node_list: list[Node], rnd_node: Node) → int[source]#
static check_if_outside_play_area(node: Node, play_area: AreaBounds) → bool[source]#
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]#
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 in the node list if found, otherwise None.

Return type:

(int or None)

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

  1. defines a ball centered on new_node

  2. 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): List with the indices of the 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

On this page

This Page