irsim.lib.path_planners.rrt#

Path planning Sample Code with Randomized Rapidly-Exploring Random Trees (RRT)

author: AtsushiSakai(@Atsushi_twi)

adapted by: Reinis Cimurs

Classes#

RRT

Class for RRT planning

Module Contents#

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