irsim.lib.path_planners.rrt_star#

Path planning Sample Code with RRT*

author: Atsushi Sakai(@Atsushi_twi)

adapted by: Reinis Cimurs

Classes#

RRTStar

Class for RRT Star planning

Module Contents#

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

  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[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