irsim.lib.path_planners.rrt =========================== .. py:module:: irsim.lib.path_planners.rrt .. autoapi-nested-parse:: Rapidly-exploring Random Tree (RRT) path planner. Collision precedence (delegated to ``env_map.is_collision(geometry)``): 1. Grid lookup when ``env_map.grid`` is not ``None``; if occupied, collision. 2. When the grid reports free or is unavailable, geometry vs. obstacle_list. The planner builds the robot shape as a geometry (e.g. circle, or polygon for non-circular robots) and calls the unified interface; the map supports any Shapely geometry. Reference: S. M. LaValle, "Rapidly-Exploring Random Trees: A New Tool for Path Planning," 1998. Implementation reference: ZJU-FAST-Lab/sampling-based-path-finding https://github.com/ZJU-FAST-Lab/sampling-based-path-finding (C++ implementation with kd-tree and efficient tree management.) Adapted for ir-sim. Attributes ---------- .. autoapisummary:: irsim.lib.path_planners.rrt.logger Classes ------- .. autoapisummary:: irsim.lib.path_planners.rrt.TreeNode irsim.lib.path_planners.rrt.RRT Module Contents --------------- .. py:data:: logger .. py:class:: TreeNode A node in the RRT search tree. Identity-based equality (``eq=False``) is used so that nodes can be safely compared with ``==`` / ``is`` and used as dictionary keys without triggering recursive field comparisons. .. attribute:: x X position. .. attribute:: y Y position. .. attribute:: cost Cumulative cost from the start node (``cost_from_start``). .. attribute:: cost_from_parent Cost of the edge from parent to this node. .. attribute:: parent Parent node (``None`` for the root). .. attribute:: children Direct children in the tree. .. attribute:: path_x Discretised X coordinates from parent to this node (used for visualisation and per-point collision checking). .. attribute:: path_y Discretised Y coordinates from parent to this node. .. py:attribute:: x :type: float .. py:attribute:: y :type: float .. py:attribute:: cost :type: float :value: 0.0 .. py:attribute:: cost_from_parent :type: float :value: 0.0 .. py:attribute:: parent :type: TreeNode | None :value: None .. py:attribute:: children :type: list[TreeNode] :value: [] .. py:attribute:: path_x :type: list[float] :value: [] .. py:attribute:: path_y :type: list[float] :value: [] .. py:class:: RRT(env_map: irsim.world.map.EnvGridMap, robot: irsim.world.object_base.ObjectBase, expand_dis: float = 1.0, path_resolution: float = 0.25, goal_sample_rate: int = 5, max_iter: int = 500) Rapidly-exploring Random Tree (RRT) path planner. Algorithmic improvements over the naive implementation (inspired by ZJU-FAST-Lab/sampling-based-path-finding): * Nodes track their children, enabling O(subtree) cost propagation via BFS instead of O(n) linear scans. * ``scipy.spatial.KDTree`` is used for nearest-neighbour and range queries when available, falling back to numpy otherwise. * A dedicated *goal node* is kept in the tree; whenever a new node can reach the goal at a lower cost the goal's parent is rewired. Initialise the RRT planner. Robot shape is taken from ``robot.original_geometry`` (e.g. pass ``robot=env.robot``). :param env_map: Environment map (:class:`~irsim.world.map.EnvGridMap`). :param robot: Robot object; its :attr:`~irsim.world.object_base.ObjectBase.original_geometry` is used for collision. :param expand_dis: Maximum extension distance per steer step. :param path_resolution: Discretisation resolution along steered edges. :param goal_sample_rate: Percentage chance of sampling the goal. :param max_iter: Maximum number of iterations. .. py:attribute:: Node .. py:class:: AreaBounds(env_map: irsim.world.map.EnvGridMap) Rectangular play-area bounds in world coordinates. .. py:attribute:: xmin :type: float .. py:attribute:: ymin :type: float .. py:attribute:: xmax :type: float .. py:attribute:: ymax :type: float .. py:attribute:: obstacle_list .. py:attribute:: play_area .. py:attribute:: max_x .. py:attribute:: max_y .. py:attribute:: min_rand .. py:attribute:: max_rand .. py:attribute:: expand_dis :value: 1.0 .. py:attribute:: path_resolution :value: 0.25 .. py:attribute:: goal_sample_rate :value: 5 .. py:attribute:: max_iter :value: 500 .. py:attribute:: node_list :value: [] .. py:attribute:: start :value: None .. py:attribute:: end :value: None .. py:attribute:: robot_radius .. py:method:: planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) -> numpy.ndarray | None Run RRT path planning. :param start_pose: Start position ``[x, y]``. :param goal_pose: Goal position ``[x, y]``. :param show_animation: Render the exploration tree during planning. :returns: ``(2, N)`` numpy array ``[rx, ry]`` or *None*. .. py:method:: steer(from_node: TreeNode, to_node: TreeNode, extend_length: float = float('inf')) -> TreeNode Steer from *from_node* towards *to_node*. Returns a **temporary** ``TreeNode`` (not yet registered in the tree) with ``path_x``/``path_y`` populated and ``parent`` set to *from_node*. .. py:method:: get_random_node() -> TreeNode Uniform random sample with goal-bias. .. py:method:: is_collision(node: TreeNode) -> bool Check whether *node*'s edge is collision-free. Uses :attr:`_collision_geometry` translated to each path point. Returns *True* if the path is **collision-free**. .. py:method:: draw_graph(rnd: TreeNode | None = None) -> None Render the RRT tree on the active matplotlib axes. .. py:method:: calc_dist_to_goal(x: float, y: float) -> float Euclidean distance from ``(x, y)`` to the goal. .. py:method:: calc_distance_and_angle(from_node: TreeNode, to_node: TreeNode) -> tuple[float, float] :staticmethod: Euclidean distance and heading between two nodes.