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, robot_radius, expand_dis=1.0, path_resolution=0.25, goal_sample_rate=5, max_iter=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, y)[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)[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, goal_pose, show_animation=True)[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, to_node, extend_length=float('inf'))[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)[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, y)[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()[source]#

Create random node

Returns:

new random node

Return type:

(Node)

draw_graph(rnd=None)[source]#
static plot_circle(x, y, size, color='-b')[source]#
static get_nearest_node_index(node_list, rnd_node)[source]#
static check_if_outside_play_area(node, play_area)[source]#
check_collision(node, robot_radius)[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, y, rr)[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, to_node)[source]#