diff --git a/PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py b/PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py similarity index 100% rename from PathPlanning/BatchInformedRRTStar/batch_informed_rrtstar.py rename to PathPlanning/BatchInformedRRTStar/batch_informed_rrt_star.py diff --git a/PathPlanning/Dijkstra/dijkstra.py b/PathPlanning/Dijkstra/dijkstra.py index 8a585e4b18..f5a4703910 100644 --- a/PathPlanning/Dijkstra/dijkstra.py +++ b/PathPlanning/Dijkstra/dijkstra.py @@ -12,7 +12,7 @@ show_animation = True -class Dijkstra: +class DijkstraPlanner: def __init__(self, ox, oy, resolution, robot_radius): """ @@ -246,7 +246,7 @@ def main(): plt.grid(True) plt.axis("equal") - dijkstra = Dijkstra(ox, oy, grid_size, robot_radius) + dijkstra = DijkstraPlanner(ox, oy, grid_size, robot_radius) rx, ry = dijkstra.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover diff --git a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py index a73797dacb..e72d33261e 100644 --- a/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py +++ b/PathPlanning/Eta3SplineTrajectory/eta3_spline_trajectory.py @@ -29,7 +29,7 @@ def __init__(self, actual_vel, max_vel): self.message = f'Actual velocity {actual_vel} does not equal desired max velocity {max_vel}!' -class eta3_trajectory(Eta3Path): +class Eta3SplineTrajectory(Eta3Path): """ eta3_trajectory @@ -300,8 +300,8 @@ def test1(max_vel=0.5): trajectory_segments.append(Eta3PathSegment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) - traj = eta3_trajectory(trajectory_segments, - max_vel=max_vel, max_accel=0.5) + traj = Eta3SplineTrajectory(trajectory_segments, + max_vel=max_vel, max_accel=0.5) # interpolate at several points along the path times = np.linspace(0, traj.total_time, 101) @@ -334,8 +334,8 @@ def test2(max_vel=0.5): trajectory_segments.append(Eta3PathSegment( start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) - traj = eta3_trajectory(trajectory_segments, - max_vel=max_vel, max_accel=0.5) + traj = Eta3SplineTrajectory(trajectory_segments, + max_vel=max_vel, max_accel=0.5) # interpolate at several points along the path times = np.linspace(0, traj.total_time, 101) @@ -400,8 +400,8 @@ def test3(max_vel=2.0): start_pose=start_pose, end_pose=end_pose, eta=eta, kappa=kappa)) # construct the whole path - traj = eta3_trajectory(trajectory_segments, - max_vel=max_vel, max_accel=0.5, max_jerk=1) + traj = Eta3SplineTrajectory(trajectory_segments, + max_vel=max_vel, max_accel=0.5, max_jerk=1) # interpolate at several points along the path times = np.linspace(0, traj.total_time, 1001) diff --git a/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst index 9d9b3de709..19fb89a1b1 100644 --- a/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst +++ b/docs/modules/5_path_planning/bezier_path/bezier_path_main.rst @@ -13,8 +13,15 @@ You can get different Beizer course: .. image:: Figure_2.png +Code Link +~~~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.BezierPath.bezier_path.calc_4points_bezier_path + + Reference +~~~~~~~~~~~~~~~ - `Continuous Curvature Path Generation Based on Bezier Curves for Autonomous - Vehicles ` + Vehicles `__ diff --git a/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst index e734352e34..00e5ef2fdb 100644 --- a/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst +++ b/docs/modules/5_path_planning/bspline_path/bspline_path_main.rst @@ -105,8 +105,8 @@ The default spline degree is 3, so curvature changes smoothly. .. image:: interp_and_curvature.png -API -++++ +Code link +++++++++++ .. autofunction:: PathPlanning.BSplinePath.bspline_path.interpolate_b_spline_path @@ -133,8 +133,8 @@ The default spline degree is 3, so curvature changes smoothly. .. image:: approx_and_curvature.png -API -++++ +Code Link +++++++++++ .. autofunction:: PathPlanning.BSplinePath.bspline_path.approximate_b_spline_path diff --git a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst index 81c91c0313..e1cd2fe353 100644 --- a/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst +++ b/docs/modules/5_path_planning/bugplanner/bugplanner_main.rst @@ -5,4 +5,12 @@ This is a 2D planning with Bug algorithm. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif +Code Link +~~~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.BugPlanning.bug.main + +Reference +~~~~~~~~~~~~ + - `ECE452 Bug Algorithms `_ diff --git a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst index 72e558c486..fa2a2ff72b 100644 --- a/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst +++ b/docs/modules/5_path_planning/catmull_rom_spline/catmull_rom_spline_main.rst @@ -88,8 +88,8 @@ Catmull-Rom Spline API This section provides an overview of the functions used for Catmull-Rom spline path planning. -API -++++ +Code Link +++++++++++ .. autofunction:: PathPlanning.Catmull_RomSplinePath.catmull_rom_spline_path.catmull_rom_point diff --git a/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst index 1e8cee694a..16d0ec03c1 100644 --- a/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst +++ b/docs/modules/5_path_planning/clothoid_path/clothoid_path_main.rst @@ -73,6 +73,11 @@ The final clothoid path can be calculated with the path parameters and Fresnel i &y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau \end{aligned} +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.ClothoidPath.clothoid_path_planner.generate_clothoid_path + References ~~~~~~~~~~ diff --git a/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst index 0a688b5ed2..eaa876c80b 100644 --- a/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst +++ b/docs/modules/5_path_planning/coverage_path/coverage_path_main.rst @@ -8,6 +8,11 @@ This is a 2D grid based sweep coverage path planner simulation: .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.GridBasedSweepCPP.grid_based_sweep_coverage_path_planner.planning + Spiral Spanning Tree ~~~~~~~~~~~~~~~~~~~~ @@ -17,6 +22,14 @@ This is a 2D grid based spiral spanning tree coverage path planner simulation: .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation2.gif .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation3.gif +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.SpiralSpanningTreeCPP.spiral_spanning_tree_coverage_path_planner.main + +Reference ++++++++++++++ + - `Spiral-STC: An On-Line Coverage Algorithm of Grid Environments by a Mobile Robot `_ @@ -29,6 +42,14 @@ This is a 2D grid based wavefront coverage path planner simulation: .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation2.gif .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation3.gif +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.WavefrontCPP.wavefront_coverage_path_planner.wavefront + +Reference ++++++++++++++ + - `Planning paths of complete coverage of an unstructured environment by a mobile robot `_ diff --git a/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst index 33471f7c85..a110217a2e 100644 --- a/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst +++ b/docs/modules/5_path_planning/cubic_spline/cubic_spline_main.rst @@ -171,8 +171,8 @@ the second derivative by: These equations can be calculated by differentiating the cubic polynomial. -API -=== +Code Link +========== This is the 1D cubic spline class API: @@ -199,8 +199,8 @@ Curvature of each point can be also calculated analytically by: :math:`\kappa=\frac{y^{\prime \prime} x^{\prime}-x^{\prime \prime} y^{\prime}}{\left(x^{\prime2}+y^{\prime2}\right)^{\frac{2}{3}}}` -API -=== +Code Link +========== .. autoclass:: PathPlanning.CubicSpline.cubic_spline_planner.CubicSpline2D :members: diff --git a/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst index 12172fb51e..5a3d14464b 100644 --- a/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst +++ b/docs/modules/5_path_planning/dubins_path/dubins_path_main.rst @@ -62,7 +62,7 @@ You can generate a path from these information and the maximum curvature informa A path type which has minimum course length among 6 types is selected, and then a path is constructed based on the selected type and its distances. -API +Code Link ~~~~~~~~~~~~~~~~~~~~ .. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path diff --git a/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst index d645838597..ac5322df94 100644 --- a/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst +++ b/docs/modules/5_path_planning/dynamic_window_approach/dynamic_window_approach_main.rst @@ -5,7 +5,17 @@ Dynamic Window Approach This is a 2D navigation sample code with Dynamic Window Approach. +.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif + +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.DynamicWindowApproach.dynamic_window_approach.dwa_control + + +Reference +~~~~~~~~~~~~ + - `The Dynamic Window Approach to Collision Avoidance `__ -.. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif diff --git a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst index 8a3e517105..d0109d4ec3 100644 --- a/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst +++ b/docs/modules/5_path_planning/elastic_bands/elastic_bands_main.rst @@ -5,6 +5,11 @@ This is a path planning with Elastic Bands. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ElasticBands/animation.gif +Code Link ++++++++++++++ + +.. autoclass:: PathPlanning.ElasticBands.elastic_bands.ElasticBands + Core Concept ~~~~~~~~~~~~ @@ -69,6 +74,6 @@ Dynamic Path Maintenance - Remove redundant nodes if adjacent nodes are too close References -~~~~~~~~~~~~~~~~~~~~~~~ ++++++++++++++ - `Elastic Bands: Connecting Path Planning and Control `__ diff --git a/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst index 82e0a71044..9ee343e8a7 100644 --- a/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst +++ b/docs/modules/5_path_planning/eta3_spline/eta3_spline_main.rst @@ -7,7 +7,14 @@ Eta^3 Spline path planning This is a path planning with Eta^3 spline. +Code Link +~~~~~~~~~~~~~~~ + +.. autoclass:: PathPlanning.Eta3SplineTrajectory.eta3_spline_trajectory.Eta3SplineTrajectory + + Reference +~~~~~~~~~~~~~~~ - `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots `__ diff --git a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst index 371d536e3f..0f84d381ea 100644 --- a/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst +++ b/docs/modules/5_path_planning/frenet_frame_path/frenet_frame_path_main.rst @@ -7,6 +7,12 @@ The cyan line is the target course and black crosses are obstacles. The red line is predicted path. +Code Link +~~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory.main + + High Speed and Velocity Keeping Scenario ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ diff --git a/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst index bf82c9f391..c4aa6882aa 100644 --- a/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst +++ b/docs/modules/5_path_planning/grid_base_search/grid_base_search_main.rst @@ -10,6 +10,12 @@ This is a 2D grid based path planning with Breadth first search algorithm. In the animation, cyan points are searched nodes. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.BreadthFirstSearch.breadth_first_search.BreadthFirstSearchPlanner + + Depth First Search ~~~~~~~~~~~~~~~~~~~~ @@ -19,6 +25,12 @@ This is a 2D grid based path planning with Depth first search algorithm. In the animation, cyan points are searched nodes. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.DepthFirstSearch.depth_first_search.DepthFirstSearchPlanner + + .. _dijkstra: Dijkstra algorithm @@ -30,6 +42,12 @@ This is a 2D grid based shortest path planning with Dijkstra's algorithm. In the animation, cyan points are searched nodes. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.Dijkstra.dijkstra.DijkstraPlanner + + .. _a*-algorithm: A\* algorithm @@ -43,6 +61,12 @@ In the animation, cyan points are searched nodes. Its heuristic is 2D Euclid distance. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.AStar.a_star.AStarPlanner + + Bidirectional A\* algorithm ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -52,6 +76,12 @@ This is a 2D grid based shortest path planning with bidirectional A star algorit In the animation, cyan points are searched nodes. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.BidirectionalAStar.bidirectional_a_star.BidirectionalAStarPlanner + + .. _D*-algorithm: D\* algorithm @@ -63,7 +93,14 @@ This is a 2D grid based shortest path planning with D star algorithm. The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm. +Code Link ++++++++++++++ + +.. autoclass:: PathPlanning.DStar.dstar.Dstar + + Reference +++++++++++++ - `D* search Wikipedia `__ @@ -74,7 +111,13 @@ This is a 2D grid based path planning and replanning with D star lite algorithm. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStarLite/animation.gif +Code Link ++++++++++++++ + +.. autoclass:: PathPlanning.DStarLite.d_star_lite.DStarLite + Reference +++++++++++++ - `Improved Fast Replanning for Robot Navigation in Unknown Terrain `_ @@ -88,7 +131,14 @@ This is a 2D grid based path planning with Potential Field algorithm. In the animation, the blue heat map shows potential value on each grid. +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.PotentialFieldPlanning.potential_field_planning.potential_field_planning + + Reference +++++++++++++ - `Robotic Motion Planning:Potential Functions `__ diff --git a/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst index a9876fa3d4..36f340e0c2 100644 --- a/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst +++ b/docs/modules/5_path_planning/hybridastar/hybridastar_main.rst @@ -4,3 +4,8 @@ Hybrid a star This is a simple vehicle model based hybrid A\* path planner. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/HybridAStar/animation.gif + +Code Link ++++++++++++++ + +.. autofunction:: PathPlanning.HybridAStar.hybrid_a_star.hybrid_a_star_planning diff --git a/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst index 788442ff63..1eb1e4d840 100644 --- a/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst +++ b/docs/modules/5_path_planning/lqr_path/lqr_path_main.rst @@ -4,3 +4,8 @@ LQR based path planning A sample code using LQR based path planning for double integrator model. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true + +Code Link ++++++++++++++ + +.. autoclass:: PathPlanning.LQRPlanner.lqr_planner.LQRPlanner diff --git a/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst index 1c5df1c9cc..76472a6792 100644 --- a/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst +++ b/docs/modules/5_path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst @@ -6,6 +6,12 @@ generator. This algorithm is used for state lattice planner. +Code Link +~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.ModelPredictiveTrajectoryGenerator.trajectory_generator.optimize_trajectory + + Path optimization sample ~~~~~~~~~~~~~~~~~~~~~~~~ @@ -17,6 +23,7 @@ Lookup table generation sample .. image:: lookup_table.png Reference +~~~~~~~~~~~~ - `Optimal rough terrain trajectory generation for wheeled mobile robots `__ diff --git a/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst index f816c5c1b9..d58d1e2633 100644 --- a/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst +++ b/docs/modules/5_path_planning/prm_planner/prm_planner_main.rst @@ -13,7 +13,14 @@ Cyan crosses means searched points with Dijkstra method, The red line is the final path of PRM. +Code Link +~~~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.ProbabilisticRoadMap.probabilistic_road_map.prm_planning + + Reference +~~~~~~~~~~~ - `Probabilistic roadmap - Wikipedia `__ diff --git a/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst index 66c3001c05..c7bc3fb55c 100644 --- a/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst +++ b/docs/modules/5_path_planning/quintic_polynomials_planner/quintic_polynomials_planner_main.rst @@ -9,6 +9,11 @@ Motion planning with quintic polynomials. It can calculate 2D path, velocity, and acceleration profile based on quintic polynomials. +Code Link +~~~~~~~~~~~~~~~ + +.. autofunction:: PathPlanning.QuinticPolynomialsPlanner.quintic_polynomials_planner.quintic_polynomials_planner + Quintic polynomials for one dimensional robot motion diff --git a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst index a4a5d28e01..4dd54d7c97 100644 --- a/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst +++ b/docs/modules/5_path_planning/reeds_shepp_path/reeds_shepp_path_main.rst @@ -5,6 +5,12 @@ A sample code with Reeds Shepp path planning. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true +Code Link +============== + +.. autofunction:: PathPlanning.ReedsSheppPath.reeds_shepp_path_planning.reeds_shepp_path_planning + + Mathematical Description of Individual Path Types ================================================= Here is an overview of mathematical derivations of formulae for individual path types. @@ -381,6 +387,7 @@ Hence, we have: Reference +============= - `15.3.2 Reeds-Shepp Curves `__ diff --git a/docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_1.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png similarity index 100% rename from docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_1.png rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_1.png diff --git a/docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_3.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png similarity index 100% rename from docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_3.png rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_3.png diff --git a/docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_4.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png similarity index 100% rename from docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_4.png rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_4.png diff --git a/docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_5.png b/docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png similarity index 100% rename from docs/modules/5_path_planning/closed_loop_rrt_star_car/Figure_5.png rename to docs/modules/5_path_planning/rrt/closed_loop_rrt_star_car/Figure_5.png diff --git a/docs/modules/5_path_planning/rrt/rrt_main.rst b/docs/modules/5_path_planning/rrt/rrt_main.rst index da3a4957a5..1bd5497f4c 100644 --- a/docs/modules/5_path_planning/rrt/rrt_main.rst +++ b/docs/modules/5_path_planning/rrt/rrt_main.rst @@ -14,6 +14,12 @@ This is a simple path planning code with Rapidly-Exploring Random Trees Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.RRT.rrt.RRT + + .. include:: rrt_star.rst @@ -24,6 +30,12 @@ RRT with dubins path Path planning for a car robot with RRT and dubins path planner. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.RRTDubins.rrt_dubins.RRTDubins + + .. _rrt*-with-dubins-path: RRT\* with dubins path @@ -33,6 +45,12 @@ RRT\* with dubins path Path planning for a car robot with RRT\* and dubins path planner. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.RRTStarDubins.rrt_star_dubins.RRTStarDubins + + .. _rrt*-with-reeds-sheep-path: RRT\* with reeds-sheep path @@ -42,6 +60,12 @@ RRT\* with reeds-sheep path Path planning for a car robot with RRT\* and reeds sheep path planner. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.RRTStarReedsShepp.rrt_star_reeds_shepp.RRTStarReedsShepp + + .. _informed-rrt*: Informed RRT\* @@ -53,7 +77,14 @@ This is a path planning code with Informed RRT*. The cyan ellipse is the heuristic sampling domain of Informed RRT*. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.InformedRRTStar.informed_rrt_star.InformedRRTStar + + Reference +^^^^^^^^^^ - `Informed RRT\*: Optimal Sampling-based Path Planning Focused via Direct Sampling of an Admissible Ellipsoidal @@ -68,12 +99,20 @@ Batch Informed RRT\* This is a path planning code with Batch Informed RRT*. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.BatchInformedRRTStar.batch_informed_rrt_star.BITStar + + Reference +^^^^^^^^^^^ - `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the Heuristically Guided Search of Implicit Random Geometric Graphs `__ + .. _closed-loop-rrt*: Closed Loop RRT\* @@ -87,7 +126,14 @@ In this code, pure-pursuit algorithm is used for steering control, PID is used for speed control. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.ClosedLoopRRTStar.closed_loop_rrt_star_car.ClosedLoopRRTStar + + Reference +^^^^^^^^^^^^ - `Motion Planning in Complex Environments using Closed-loop Prediction `__ @@ -98,6 +144,7 @@ Reference - `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning Using Closed-loop Prediction `__ + .. _lqr-rrt*: LQR-RRT\* @@ -109,7 +156,14 @@ A double integrator motion model is used for LQR local planner. .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.LQRRRTStar.lqr_rrt_star.LQRRRTStar + + Reference +~~~~~~~~~~~~~ - `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically Derived Extension diff --git a/docs/modules/5_path_planning/rrt/rrt_star.rst b/docs/modules/5_path_planning/rrt/rrt_star.rst index 6deb6b9172..960b9976d5 100644 --- a/docs/modules/5_path_planning/rrt/rrt_star.rst +++ b/docs/modules/5_path_planning/rrt/rrt_star.rst @@ -7,6 +7,12 @@ This is a path planning code with RRT\* Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions. +Code Link +^^^^^^^^^^ + +.. autoclass:: PathPlanning.RRTStar.rrt_star.RRTStar + + Simulation ^^^^^^^^^^ diff --git a/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst index bf89ac11ae..9f8cc0c50f 100644 --- a/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst +++ b/docs/modules/5_path_planning/state_lattice_planner/state_lattice_planner_main.rst @@ -12,17 +12,34 @@ Uniform polar sampling .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif +Code Link +^^^^^^^^^^^^^ + +.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_uniform_polar_states + + Biased polar sampling ~~~~~~~~~~~~~~~~~~~~~ .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif +Code Link +^^^^^^^^^^^^^ +.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_biased_polar_states + + Lane sampling ~~~~~~~~~~~~~ .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif +Code Link +^^^^^^^^^^^^^ +.. autofunction:: PathPlanning.StateLatticePlanner.state_lattice_planner.calc_lane_states + + Reference +~~~~~~~~~~~~~~~ - `Optimal rough terrain trajectory generation for wheeled mobile robots `__ diff --git a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst index 04001d4504..9517e95b31 100644 --- a/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst +++ b/docs/modules/5_path_planning/time_based_grid_search/time_based_grid_search_main.rst @@ -33,6 +33,10 @@ After:: When starting at (1, 11) in the structured obstacle arrangement (second of the two gifs above). +Code Link +^^^^^^^^^^^^^ +.. autoclass:: PathPlanning.TimeBasedPathPlanning.SpaceTimeAStar.SpaceTimeAStar + Safe Interval Path Planning ~~~~~~~~~~~~~~~~~~~~~~~~~~~ @@ -73,6 +77,11 @@ Arrangement 1 starting at (1, 18):: .. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/refs/heads/master/PathPlanning/TimeBasedPathPlanning/SafeIntervalPathPlanner/path_animation2.gif +Code Link +^^^^^^^^^^^^^ +.. autoclass:: PathPlanning.TimeBasedPathPlanning.SafeInterval.SafeIntervalPathPlanner + + References ~~~~~~~~~~~ diff --git a/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst index 3c9b7c008c..aac96d6e19 100644 --- a/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst +++ b/docs/modules/5_path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst @@ -13,6 +13,11 @@ red crosses are visibility nodes, and blue lines area collision free visibility The red line is the final path searched by dijkstra algorithm frm the visibility graphs. +Code Link +~~~~~~~~~~~~ +.. autoclass:: PathPlanning.VisibilityRoadMap.visibility_road_map.VisibilityRoadMap + + Algorithms ~~~~~~~~~~ @@ -64,7 +69,7 @@ The red line is searched path in the figure: You can find the details of Dijkstra algorithm in :ref:`dijkstra`. References -^^^^^^^^^^ +~~~~~~~~~~~~ - `Visibility graph - Wikipedia `_ diff --git a/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst index 65e0e53465..a9a41aab74 100644 --- a/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst +++ b/docs/modules/5_path_planning/vrm_planner/vrm_planner_main.rst @@ -11,7 +11,13 @@ Cyan crosses mean searched points with Dijkstra method, The red line is the final path of Vornoi Road-Map. +Code Link +~~~~~~~~~~~~~~~ +.. autoclass:: PathPlanning.VoronoiRoadMap.voronoi_road_map.VoronoiRoadMapPlanner + + Reference +~~~~~~~~~~~~ - `Robotic Motion Planning `__ diff --git a/tests/test_batch_informed_rrt_star.py b/tests/test_batch_informed_rrt_star.py index 3ad78bdb23..cf0a9827a8 100644 --- a/tests/test_batch_informed_rrt_star.py +++ b/tests/test_batch_informed_rrt_star.py @@ -1,7 +1,7 @@ import random import conftest -from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m +from PathPlanning.BatchInformedRRTStar import batch_informed_rrt_star as m def test_1():