irsim.lib.path_planners ======================= .. py:module:: irsim.lib.path_planners .. autoapi-nested-parse:: Path planning algorithms for IR-SIM simulation. This package contains: - a_star: A* path planning algorithm - jps: Jump Point Search (JPS) grid planning - rrt: Rapidly-exploring Random Tree algorithm - rrt_star: RRT* optimized path planning - informed_rrt_star: Informed RRT* optimal path planning - probabilistic_road_map: PRM path planning algorithm Submodules ---------- .. toctree:: :maxdepth: 1 /api/irsim/lib/path_planners/a_star/index /api/irsim/lib/path_planners/informed_rrt_star/index /api/irsim/lib/path_planners/jps/index /api/irsim/lib/path_planners/probabilistic_road_map/index /api/irsim/lib/path_planners/rrt/index /api/irsim/lib/path_planners/rrt_star/index Classes ------- .. autoapisummary:: irsim.lib.path_planners.AStarPlanner irsim.lib.path_planners.InformedRRTStar irsim.lib.path_planners.JPSPlanner irsim.lib.path_planners.PRMPlanner irsim.lib.path_planners.RRT irsim.lib.path_planners.RRTStar Package Contents ---------------- .. py:class:: AStarPlanner(env_map: irsim.world.map.EnvGridMap) Initialize A* planner. :param env_map: Environment map (any :class:`~irsim.world.map.EnvGridMap` compatible object). .. py:attribute:: obstacle_list .. py:attribute:: origin_x .. py:attribute:: origin_y .. py:attribute:: max_x .. py:attribute:: max_y .. py:attribute:: motion .. py:class:: Node(x: int, y: int, cost: float, parent_index: int) Node class Initialize Node :param x: x position of the node :type x: float :param y: y position of the node :type y: float :param cost: heuristic cost of the node :type cost: float :param parent_index: Nodes parent index :type parent_index: int .. py:attribute:: x .. py:attribute:: y .. py:attribute:: cost .. py:attribute:: parent_index .. py:method:: planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, show_animation: bool = True) -> tuple[list[float], list[float]] A star path search :param start_pose: start pose [x,y] :type start_pose: np.array :param goal_pose: goal pose [x,y] :type goal_pose: np.array :param show_animation: If true, shows the animation of planning process :type show_animation: bool :returns: xy position array of the final path :rtype: (np.array) .. py:method:: calc_final_path(goal_node: Node, closed_set: dict) -> tuple[list[float], list[float]] Generate the final path :param goal_node: final goal node :type goal_node: Node :param closed_set: dict of closed nodes :type closed_set: dict :returns: list of x positions of final path ry (list): list of y positions of final path :rtype: rx (list) .. py:method:: calc_heuristic(n1: Node, n2: Node) -> float :staticmethod: .. py:method:: calc_grid_position(index: int, min_position: float) -> float calc grid position :param index: index of a node :type index: int :param min_position: min value of search space :type min_position: float :returns: position of coordinates along the given axis :rtype: (float) .. py:method:: calc_xy_index(position: float, min_pos: float) -> int calc xy index of node :param position: position of a node :type position: float :param min_pos: min value of search space :type min_pos: float :returns: index of position along the given axis :rtype: (int) .. py:method:: calc_grid_index(node: Node) -> int calc grid index of node :param node: node to calculate the index for :type node: Node :returns: grid index of the node :rtype: (float) .. py:method:: verify_node(node: Node) -> bool Check if node is acceptable - within limits of search space and free of collisions :param node: node to check :type node: Node :returns: True if node is acceptable. False otherwise :rtype: (bool) .. py:method:: check_node(x: float, y: float) -> bool Check position for a collision. :param x: World x coordinate of the cell centre. :param y: World y coordinate of the cell centre. :returns: ``True`` if a collision is detected. .. py:method:: get_motion_model() -> list[list[float]] :staticmethod: .. py:class:: InformedRRTStar(env_map: irsim.world.map.EnvGridMap, robot: Any, expand_dis: float = 1.5, path_resolution: float = 0.25, goal_sample_rate: int = 10, max_iter: int = 500, connect_circle_dist: float = 50.0, search_until_max_iter: bool = True) Bases: :py:obj:`irsim.lib.path_planners.rrt_star.RRTStar` Informed RRT* path planner. After finding an initial feasible path the sampler switches from uniform random sampling to sampling inside an informed ellipsoidal set whose foci are the start and goal. This dramatically speeds up convergence towards the optimal path. Initialise the Informed RRT* planner. .. py:attribute:: Node .. py:method:: planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) -> numpy.ndarray | None Informed RRT* path planning. :param start_pose: Start position ``[x, y]``. :param goal_pose: Goal position ``[x, y]``. :param show_animation: Render tree, ellipse and best path during planning. :returns: ``(2, N)`` waypoint array or *None*. .. py:class:: JPSPlanner(env_map: irsim.world.map.EnvGridMap) Jump Point Search planner for uniform-cost 8-connected grids. When the environment map carries an occupancy grid (``env_map.grid``), collision checks use a fast O(1) grid lookup. Otherwise, the planner falls back to Shapely geometry intersection (same as :class:`AStarPlanner`). Initialize JPS planner. :param env_map: Environment map (any :class:`~irsim.world.map.EnvGridMap` compatible object). Resolution and bounds are taken from the map (same as :class:`AStarPlanner`). .. py:attribute:: origin_x .. py:attribute:: origin_y .. py:attribute:: max_x .. py:attribute:: max_y .. py:attribute:: obstacle_list .. py:method:: planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, show_animation: bool = True) -> numpy.ndarray | None JPS path search. :param start_pose: start pose [x, y] :type start_pose: np.ndarray :param goal_pose: goal pose [x, y] :type goal_pose: np.ndarray :param show_animation: If true, shows the animation of planning process :type show_animation: bool :returns: shape (2, N) array [rx, ry] of the final path, or None if the start or goal cell is not walkable, or if no path exists (open set exhausted). :rtype: np.ndarray | None .. py:method:: calc_grid_position(index: int, min_position: float) -> float .. py:method:: calc_xy_index(position: float, min_pos: float) -> int .. py:method:: calc_grid_index_from_xy(x: int, y: int) -> int .. py:method:: is_collision(x: float, y: float) -> bool True if world position ``(x, y)`` is in collision. .. py:class:: PRMPlanner(env_map: irsim.world.map.EnvGridMap, robot_radius: float, n_sample: int = 500, n_knn: int = 10, max_edge_len: float = 30.0) Initialize the PRM planner. :param env_map: Environment map (any :class:`~irsim.world.map.EnvGridMap` compatible object). :param robot_radius: Robot radius modeled as a circle. :param n_sample: Number of sampled points. :param n_knn: Number of nearest neighbors per node. :param max_edge_len: Maximum allowed edge length. .. py:attribute:: rr .. py:attribute:: obstacle_list .. py:attribute:: min_x .. py:attribute:: min_y .. py:attribute:: max_x .. py:attribute:: max_y .. py:attribute:: n_sample :value: 500 .. py:attribute:: n_knn :value: 10 .. py:attribute:: max_edge_len :value: 30.0 .. py:method:: planning(start_pose: numpy.ndarray, goal_pose: numpy.ndarray, rng: Any | None = None, show_animation: bool = True) -> tuple[list[float], list[float]] | None Plan a path from start to goal using the PRM method. :param start_pose: start pose [x,y] :type start_pose: np.array :param goal_pose: goal pose [x,y] :type goal_pose: np.array :param rng: Random generator :type rng: Optional :param show_animation: If true, shows the animation of planning process :type show_animation: bool :returns: xy position array of the final path :rtype: (np.array) .. py:method:: check_node(x: float, y: float, rr: float) -> bool Check position for a collision. :param x: World x coordinate. :param y: World y coordinate. :param rr: Robot radius for the check. :returns: ``True`` if a collision is detected. .. py:method:: is_collision(sx: float, sy: float, gx: float, gy: float) -> bool Check if line between points is acceptable - within edge limits and free of collisions :param sx: start x position :type sx: float :param sy: start y position :type sy: float :param gx: goal x position :type gx: float :param gy: goal y position :type gy: float :returns: True if node is not acceptable. False otherwise :rtype: result (bool) .. py:method:: generate_road_map(sample_x: list[float], sample_y: list[float]) -> list[list[int]] Road map generation :param sample_x: [m] x positions of sampled points :type sample_x: List :param sample_y: [m] y positions of sampled points :type sample_y: List :returns: list of edge ids :rtype: road_map (List) .. py:method:: 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 :staticmethod: :param sx: start x position [m] :type sx: float :param sy: start y position [m] :type sy: float :param gx: goal x position [m] :type gx: float :param gy: goal y position [m] :type gy: float :param road_map: list of edge ids :type road_map: list :param sample_x: ??? [m] :type sample_x: float :param sample_y: ??? [m] :type sample_y: float :returns: Two lists of path coordinates ([x1, x2, ...], [y1, y2, ...]), empty list when no path was found :rtype: (tuple(list, list)) .. py:method:: plot_road_map(road_map: list[list[int]], sample_x: list[float], sample_y: list[float]) -> None :staticmethod: .. py:method:: sample_points(sx: float, sy: float, gx: float, gy: float, rng: Any | None) -> tuple[list[float], list[float]] Generate sample points :param sx: start x position [m] :type sx: float :param sy: start y position [m] :type sy: float :param gx: goal x position [m] :type gx: float :param gy: goal y position [m] :type gy: float :param rng: Random generator :returns: sample positions :rtype: sample (tuple (list, list)) .. 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. .. py:class:: RRTStar(env_map: irsim.world.map.EnvGridMap, robot: Any, 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) Bases: :py:obj:`irsim.lib.path_planners.rrt.RRT` RRT* path planner with asymptotic optimality. Extends :class:`RRT` with: * **Neighbour search** — after steering, all nodes within a shrinking radius are considered as potential parents (``_choose_parent``). * **Rewiring** — after inserting a new node, nearby nodes are checked to see whether their cost can be reduced by re-parenting through the new node (``_rewire``). * **Efficient cost propagation** — ``_change_node_parent`` uses BFS through the ``children`` list for O(subtree) updates. Initialise the RRT* planner. .. py:attribute:: Node .. py:attribute:: connect_circle_dist :value: 0.5 .. py:attribute:: search_until_max_iter :value: False .. py:method:: planning(start_pose: list[float], goal_pose: list[float], show_animation: bool = True) -> numpy.ndarray | None RRT* path planning. :param start_pose: Start position ``[x, y]``. :param goal_pose: Goal position ``[x, y]``. :param show_animation: Render the tree during planning. :returns: ``(2, N)`` waypoint array or *None*.