├── .circleci └── config.yml ├── .github ├── FUNDING.yml ├── ISSUE_TEMPLATE │ └── bug_report.md ├── codeql │ └── codeql-config.yml ├── dependabot.yml ├── pull_request_template.md └── workflows │ ├── Linux_CI.yml │ ├── MacOS_CI.yml │ ├── circleci-artifacts-redirector.yml │ ├── codeql.yml │ └── gh-pages.yml ├── .gitignore ├── .lgtm.yml ├── AerialNavigation ├── __init__.py ├── drone_3d_trajectory_following │ ├── Quadrotor.py │ ├── TrajectoryGenerator.py │ ├── __init__.py │ └── drone_3d_trajectory_following.py └── rocket_powered_landing │ └── rocket_powered_landing.py ├── ArmNavigation ├── __init__.py ├── arm_obstacle_navigation │ ├── arm_obstacle_navigation.py │ └── arm_obstacle_navigation_2.py ├── n_joint_arm_3d │ ├── NLinkArm3d.py │ ├── __init__.py │ ├── random_forward_kinematics.py │ └── random_inverse_kinematics.py ├── n_joint_arm_to_point_control │ ├── NLinkArm.py │ ├── __init__.py │ └── n_joint_arm_to_point_control.py ├── rrt_star_seven_joint_arm_control │ └── rrt_star_seven_joint_arm_control.py └── two_joint_arm_to_point_control │ └── two_joint_arm_to_point_control.py ├── Bipedal ├── __init__.py └── bipedal_planner │ ├── __init__.py │ └── bipedal_planner.py ├── CODE_OF_CONDUCT.md ├── CONTRIBUTING.md ├── Control ├── __init__.py ├── inverted_pendulum │ ├── inverted_pendulum_lqr_control.py │ └── inverted_pendulum_mpc_control.py └── move_to_pose │ ├── __init__.py │ ├── move_to_pose.py │ └── move_to_pose_robot.py ├── LICENSE ├── Localization ├── __init__.py ├── cubature_kalman_filter │ └── cubature_kalman_filter.py ├── ensemble_kalman_filter │ └── ensemble_kalman_filter.py ├── extended_kalman_filter │ └── extended_kalman_filter.py ├── histogram_filter │ └── histogram_filter.py ├── particle_filter │ └── particle_filter.py └── unscented_kalman_filter │ └── unscented_kalman_filter.py ├── Mapping ├── __init__.py ├── circle_fitting │ └── circle_fitting.py ├── gaussian_grid_map │ └── gaussian_grid_map.py ├── grid_map_lib │ ├── __init__.py │ └── grid_map_lib.py ├── kmeans_clustering │ └── kmeans_clustering.py ├── lidar_to_grid_map │ ├── lidar01.csv │ └── lidar_to_grid_map.py ├── raycasting_grid_map │ └── raycasting_grid_map.py └── rectangle_fitting │ ├── __init_.py │ ├── rectangle_fitting.py │ └── simulator.py ├── PathPlanning ├── AStar │ ├── a_star.py │ ├── a_star_searching_from_two_side.py │ └── a_star_variants.py ├── BSplinePath │ └── bspline_path.py ├── BatchInformedRRTStar │ └── batch_informed_rrtstar.py ├── BezierPath │ └── bezier_path.py ├── BidirectionalAStar │ └── bidirectional_a_star.py ├── BidirectionalBreadthFirstSearch │ └── bidirectional_breadth_first_search.py ├── BreadthFirstSearch │ └── breadth_first_search.py ├── BugPlanning │ └── bug.py ├── ClosedLoopRRTStar │ ├── closed_loop_rrt_star_car.py │ ├── pure_pursuit.py │ └── unicycle_model.py ├── ClothoidPath │ └── clothoid_path_planner.py ├── CubicSpline │ ├── __init__.py │ ├── cubic_spline_planner.py │ └── spline_continuity.py ├── DStar │ └── dstar.py ├── DStarLite │ └── d_star_lite.py ├── DepthFirstSearch │ └── depth_first_search.py ├── Dijkstra │ └── dijkstra.py ├── DubinsPath │ ├── __init__.py │ └── dubins_path_planner.py ├── DynamicMovementPrimitives │ └── dynamic_movement_primitives.py ├── DynamicWindowApproach │ └── dynamic_window_approach.py ├── Eta3SplinePath │ └── eta3_spline_path.py ├── Eta3SplineTrajectory │ └── eta3_spline_trajectory.py ├── FlowField │ └── flowfield.py ├── FrenetOptimalTrajectory │ └── frenet_optimal_trajectory.py ├── GreedyBestFirstSearch │ └── greedy_best_first_search.py ├── GridBasedSweepCPP │ └── grid_based_sweep_coverage_path_planner.py ├── HybridAStar │ ├── __init__.py │ ├── car.py │ ├── dynamic_programming_heuristic.py │ ├── hybrid_a_star.py │ ├── my_hybrid_a_star.py │ ├── robot.py │ ├── tempCodeRunnerFile.py │ └── test.py ├── InformedRRTStar │ └── informed_rrt_star.py ├── LQRPlanner │ └── lqr_planner.py ├── LQRRRTStar │ └── lqr_rrt_star.py ├── ModelPredictiveTrajectoryGenerator │ ├── __init__.py │ ├── lookup_table_generator.py │ ├── motion_model.py │ └── trajectory_generator.py ├── PotentialFieldPlanning │ └── potential_field_planning.py ├── ProbabilisticRoadMap │ └── probabilistic_road_map.py ├── QuinticPolynomialsPlanner │ └── quintic_polynomials_planner.py ├── RRT │ ├── __init__.py │ ├── rrt.py │ ├── rrt_with_pathsmoothing.py │ ├── rrt_with_sobol_sampler.py │ └── sobol │ │ ├── __init__.py │ │ └── sobol.py ├── RRTDubins │ ├── __init__.py │ └── rrt_dubins.py ├── RRTStar │ ├── __init__.py │ └── rrt_star.py ├── RRTStarDubins │ └── rrt_star_dubins.py ├── RRTStarReedsShepp │ └── rrt_star_reeds_shepp.py ├── ReedsSheppPath │ └── reeds_shepp_path_planning.py ├── SpiralSpanningTreeCPP │ ├── map │ │ ├── test.png │ │ ├── test_2.png │ │ └── test_3.png │ └── spiral_spanning_tree_coverage_path_planner.py ├── StateLatticePlanner │ ├── __init__.py │ ├── lookup_table.csv │ └── state_lattice_planner.py ├── VisibilityRoadMap │ ├── __init__.py │ ├── geometry.py │ └── visibility_road_map.py ├── VoronoiRoadMap │ ├── __init__.py │ ├── dijkstra_search.py │ └── voronoi_road_map.py ├── WavefrontCPP │ ├── map │ │ ├── test.png │ │ ├── test_2.png │ │ └── test_3.png │ └── wavefront_coverage_path_planner.py └── __init__.py ├── PathTracking ├── __init__.py ├── cgmres_nmpc │ └── cgmres_nmpc.py ├── lqr_speed_steer_control │ └── lqr_speed_steer_control.py ├── lqr_steer_control │ ├── __init__.py │ └── lqr_steer_control.py ├── model_predictive_speed_and_steer_control │ └── model_predictive_speed_and_steer_control.py ├── pure_pursuit │ └── pure_pursuit.py ├── rear_wheel_feedback │ └── rear_wheel_feedback.py └── stanley_controller │ └── stanley_controller.py ├── README.md ├── SECURITY.md ├── SLAM ├── EKFSLAM │ └── ekf_slam.py ├── FastSLAM1 │ └── fast_slam1.py ├── FastSLAM2 │ └── fast_slam2.py ├── GraphBasedSLAM │ ├── data │ │ ├── README.rst │ │ └── input_INTEL.g2o │ ├── graph_based_slam.py │ └── graphslam │ │ ├── __init__.py │ │ ├── edge │ │ ├── __init__.py │ │ └── edge_odometry.py │ │ ├── graph.py │ │ ├── load.py │ │ ├── pose │ │ ├── __init__.py │ │ └── se2.py │ │ ├── util.py │ │ └── vertex.py ├── __init__.py └── iterative_closest_point │ └── iterative_closest_point.py ├── __init__.py ├── _config.yml ├── appveyor.yml ├── docs ├── Makefile ├── README.md ├── _static │ ├── .gitkeep │ ├── custom.css │ └── img │ │ └── doc_ci.png ├── _templates │ └── layout.html ├── conf.py ├── doc_requirements.txt ├── getting_started_main.rst ├── how_to_contribute_main.rst ├── index_main.rst ├── make.bat └── modules │ ├── aerial_navigation │ ├── aerial_navigation_main.rst │ ├── drone_3d_trajectory_following │ │ └── drone_3d_trajectory_following_main.rst │ └── rocket_powered_landing │ │ └── rocket_powered_landing_main.rst │ ├── appendix │ ├── Kalmanfilter_basics_2_files │ │ └── Kalmanfilter_basics_2_5_0.png │ ├── Kalmanfilter_basics_2_main.rst │ ├── Kalmanfilter_basics_files │ │ ├── Kalmanfilter_basics_14_1.png │ │ ├── Kalmanfilter_basics_16_0.png │ │ ├── Kalmanfilter_basics_19_1.png │ │ ├── Kalmanfilter_basics_21_1.png │ │ ├── Kalmanfilter_basics_22_0.png │ │ └── Kalmanfilter_basics_28_1.png │ ├── Kalmanfilter_basics_main.rst │ └── appendix_main.rst │ ├── arm_navigation │ ├── Planar_Two_Link_IK_files │ │ ├── Planar_Two_Link_IK_12_0.png │ │ ├── Planar_Two_Link_IK_5_0.png │ │ ├── Planar_Two_Link_IK_7_0.png │ │ └── Planar_Two_Link_IK_9_0.png │ ├── arm_navigation_main.rst │ ├── n_joint_arm_to_point_control_main.rst │ ├── obstacle_avoidance_arm_navigation_main.rst │ └── planar_two_link_ik_main.rst │ ├── bipedal │ ├── bipedal_main.rst │ └── bipedal_planner │ │ └── bipedal_planner_main.rst │ ├── control │ ├── control_main.rst │ ├── inverted_pendulum_control │ │ ├── inverted-pendulum.png │ │ └── inverted_pendulum_control_main.rst │ └── move_to_a_pose_control │ │ └── move_to_a_pose_control_main.rst │ ├── introduction_main.rst │ ├── localization │ ├── ensamble_kalman_filter_localization_files │ │ └── ensamble_kalman_filter_localization_main.rst │ ├── extended_kalman_filter_localization_files │ │ ├── extended_kalman_filter_localization_1_0.png │ │ └── extended_kalman_filter_localization_main.rst │ ├── histogram_filter_localization │ │ ├── 1.png │ │ ├── 2.png │ │ ├── 3.png │ │ ├── 4.png │ │ └── histogram_filter_localization_main.rst │ ├── localization_main.rst │ ├── particle_filter_localization │ │ └── particle_filter_localization_main.rst │ └── unscented_kalman_filter_localization │ │ └── unscented_kalman_filter_localization_main.rst │ ├── mapping │ ├── circle_fitting │ │ └── circle_fitting_main.rst │ ├── gaussian_grid_map │ │ └── gaussian_grid_map_main.rst │ ├── k_means_object_clustering │ │ └── k_means_object_clustering_main.rst │ ├── lidar_to_grid_map_tutorial │ │ ├── grid_map_example.png │ │ ├── lidar_to_grid_map_tutorial_12_0.png │ │ ├── lidar_to_grid_map_tutorial_14_1.png │ │ ├── lidar_to_grid_map_tutorial_5_0.png │ │ ├── lidar_to_grid_map_tutorial_7_0.png │ │ ├── lidar_to_grid_map_tutorial_8_0.png │ │ └── lidar_to_grid_map_tutorial_main.rst │ ├── mapping_main.rst │ ├── ray_casting_grid_map │ │ └── ray_casting_grid_map_main.rst │ └── rectangle_fitting │ │ └── rectangle_fitting_main.rst │ ├── path_planning │ ├── bezier_path │ │ ├── Figure_1.png │ │ ├── Figure_2.png │ │ └── bezier_path_main.rst │ ├── bspline_path │ │ ├── approx_and_curvature.png │ │ ├── approximation1.png │ │ ├── basis_functions.png │ │ ├── bspline_path_main.rst │ │ ├── bspline_path_planning.png │ │ ├── interp_and_curvature.png │ │ └── interpolation1.png │ ├── bugplanner │ │ └── bugplanner_main.rst │ ├── closed_loop_rrt_star_car │ │ ├── Figure_1.png │ │ ├── Figure_3.png │ │ ├── Figure_4.png │ │ └── Figure_5.png │ ├── clothoid_path │ │ └── clothoid_path_main.rst │ ├── coverage_path │ │ └── coverage_path_main.rst │ ├── cubic_spline │ │ ├── cubic_spline_1d.png │ │ ├── cubic_spline_2d_curvature.png │ │ ├── cubic_spline_2d_path.png │ │ ├── cubic_spline_2d_yaw.png │ │ ├── cubic_spline_main.rst │ │ ├── spline.png │ │ └── spline_continuity.png │ ├── dubins_path │ │ ├── RLR.jpg │ │ ├── RSR.jpg │ │ ├── dubins_path.jpg │ │ └── dubins_path_main.rst │ ├── dynamic_window_approach │ │ └── dynamic_window_approach_main.rst │ ├── eta3_spline │ │ └── eta3_spline_main.rst │ ├── frenet_frame_path │ │ └── frenet_frame_path_main.rst │ ├── grid_base_search │ │ └── grid_base_search_main.rst │ ├── hybridastar │ │ └── hybridastar_main.rst │ ├── lqr_path │ │ └── lqr_path_main.rst │ ├── model_predictive_trajectory_generator │ │ ├── lookup_table.png │ │ └── model_predictive_trajectory_generator_main.rst │ ├── path_planning_main.rst │ ├── prm_planner │ │ └── prm_planner_main.rst │ ├── quintic_polynomials_planner │ │ └── quintic_polynomials_planner_main.rst │ ├── reeds_shepp_path │ │ └── reeds_shepp_path_main.rst │ ├── rrt │ │ ├── figure_1.png │ │ ├── rrt_main.rst │ │ ├── rrt_star.rst │ │ ├── rrt_star │ │ │ └── rrt_star_1_0.png │ │ └── rrt_star_reeds_shepp │ │ │ └── figure_1.png │ ├── state_lattice_planner │ │ ├── Figure_1.png │ │ ├── Figure_2.png │ │ ├── Figure_3.png │ │ ├── Figure_4.png │ │ ├── Figure_5.png │ │ ├── Figure_6.png │ │ └── state_lattice_planner_main.rst │ ├── visibility_road_map_planner │ │ ├── step0.png │ │ ├── step1.png │ │ ├── step2.png │ │ ├── step3.png │ │ └── visibility_road_map_planner_main.rst │ └── vrm_planner │ │ └── vrm_planner_main.rst │ ├── path_tracking │ ├── cgmres_nmpc │ │ ├── cgmres_nmpc_1_0.png │ │ ├── cgmres_nmpc_2_0.png │ │ ├── cgmres_nmpc_3_0.png │ │ ├── cgmres_nmpc_4_0.png │ │ └── cgmres_nmpc_main.rst │ ├── lqr_speed_and_steering_control │ │ └── lqr_speed_and_steering_control_main.rst │ ├── lqr_steering_control │ │ └── lqr_steering_control_main.rst │ ├── model_predictive_speed_and_steering_control │ │ └── model_predictive_speed_and_steering_control_main.rst │ ├── path_tracking_main.rst │ ├── pure_pursuit_tracking │ │ └── pure_pursuit_tracking_main.rst │ ├── rear_wheel_feedback_control │ │ └── rear_wheel_feedback_control_main.rst │ └── stanley_control │ │ └── stanley_control_main.rst │ ├── slam │ ├── FastSLAM1 │ │ ├── FastSLAM1_12_0.png │ │ ├── FastSLAM1_12_1.png │ │ ├── FastSLAM1_1_0.png │ │ └── FastSLAM1_main.rst │ ├── FastSLAM2 │ │ └── FastSLAM2_main.rst │ ├── ekf_slam │ │ ├── ekf_slam_1_0.png │ │ └── ekf_slam_main.rst │ ├── graph_slam │ │ ├── graphSLAM_SE2_example.rst │ │ ├── graphSLAM_SE2_example_files │ │ │ ├── Graph_SLAM_optimization.gif │ │ │ ├── graphSLAM_SE2_example_13_0.png │ │ │ ├── graphSLAM_SE2_example_15_0.png │ │ │ ├── graphSLAM_SE2_example_16_0.png │ │ │ ├── graphSLAM_SE2_example_4_0.png │ │ │ ├── graphSLAM_SE2_example_8_0.png │ │ │ └── graphSLAM_SE2_example_9_0.png │ │ ├── graphSLAM_doc.rst │ │ ├── graphSLAM_doc_files │ │ │ ├── graphSLAM_doc_11_1.png │ │ │ ├── graphSLAM_doc_11_2.png │ │ │ ├── graphSLAM_doc_2_0.png │ │ │ ├── graphSLAM_doc_2_2.png │ │ │ ├── graphSLAM_doc_4_0.png │ │ │ └── graphSLAM_doc_9_1.png │ │ ├── graphSLAM_formulation.rst │ │ └── graph_slam_main.rst │ ├── iterative_closest_point_matching │ │ └── iterative_closest_point_matching_main.rst │ └── slam_main.rst │ └── utils │ ├── plot │ ├── curvature_plot.png │ └── plot_main.rst │ └── utils_main.rst ├── icon.png ├── mypy.ini ├── requirements ├── environment.yml └── requirements.txt ├── runtests.sh ├── tests ├── __init__.py ├── conftest.py ├── test_LQR_planner.py ├── test_a_star.py ├── test_a_star_searching_two_side.py ├── test_a_star_variants.py ├── test_batch_informed_rrt_star.py ├── test_bezier_path.py ├── test_bipedal_planner.py ├── test_breadth_first_search.py ├── test_bspline_path.py ├── test_bug.py ├── test_cgmres_nmpc.py ├── test_circle_fitting.py ├── test_closed_loop_rrt_star_car.py ├── test_clothoidal_paths.py ├── test_cubature_kalman_filter.py ├── test_d_star_lite.py ├── test_depth_first_search.py ├── test_diff_codestyle.py ├── test_dijkstra.py ├── test_drone_3d_trajectory_following.py ├── test_dstar.py ├── test_dubins_path_planning.py ├── test_dynamic_movement_primitives.py ├── test_dynamic_window_approach.py ├── test_ekf_slam.py ├── test_eta3_spline_path.py ├── test_extended_kalman_filter.py ├── test_fast_slam1.py ├── test_fast_slam2.py ├── test_flow_field.py ├── test_frenet_optimal_trajectory.py ├── test_gaussian_grid_map.py ├── test_graph_based_slam.py ├── test_greedy_best_first_search.py ├── test_grid_based_sweep_coverage_path_planner.py ├── test_grid_map_lib.py ├── test_histogram_filter.py ├── test_hybrid_a_star.py ├── test_informed_rrt_star.py ├── test_inverted_pendulum_lqr_control.py ├── test_inverted_pendulum_mpc_control.py ├── test_iterative_closest_point.py ├── test_kmeans_clustering.py ├── test_lqr_rrt_star.py ├── test_lqr_speed_steer_control.py ├── test_lqr_steer_control.py ├── test_model_predictive_speed_and_steer_control.py ├── test_move_to_pose.py ├── test_move_to_pose_robot.py ├── test_mypy_type_check.py ├── test_n_joint_arm_to_point_control.py ├── test_particle_filter.py ├── test_potential_field_planning.py ├── test_probabilistic_road_map.py ├── test_pure_pursuit.py ├── test_quintic_polynomials_planner.py ├── test_raycasting_grid_map.py ├── test_rear_wheel_feedback.py ├── test_rectangle_fitting.py ├── test_reeds_shepp_path_planning.py ├── test_rocket_powered_landing.py ├── test_rrt.py ├── test_rrt_dubins.py ├── test_rrt_star.py ├── test_rrt_star_dubins.py ├── test_rrt_star_reeds_shepp.py ├── test_rrt_star_seven_joint_arm.py ├── test_rrt_with_sobol_sampler.py ├── test_spiral_spanning_tree_coverage_path_planner.py ├── test_stanley_controller.py ├── test_state_lattice_planner.py ├── test_two_joint_arm_to_point_control.py ├── test_unscented_kalman_filter.py ├── test_utils.py ├── test_visibility_road_map_planner.py ├── test_voronoi_road_map_planner.py └── test_wavefront_coverage_path_planner.py ├── users_comments.md └── utils ├── __init__.py ├── angle.py └── plot.py /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2.1 2 | 3 | orbs: 4 | python: circleci/python@1.4.0 5 | 6 | jobs: 7 | build_doc: 8 | docker: 9 | - image: cimg/python:3.10 10 | steps: 11 | - checkout 12 | - run: 13 | name: doc_build 14 | command: | 15 | python --version 16 | python -m venv venv 17 | . venv/bin/activate 18 | pip install -r requirements/requirements.txt 19 | pip install -r docs/doc_requirements.txt 20 | cd docs;make html 21 | - store_artifacts: 22 | path: docs/_build/html/ 23 | destination: html 24 | 25 | workflows: 26 | main: 27 | jobs: 28 | - build_doc 29 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | github: AtsushiSakai 3 | patreon: myenigma 4 | custom: https://www.paypal.me/myenigmapay/ 5 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **Expected behavior** 14 | A clear and concise description of what you expected to happen. 15 | 16 | **Screenshots** 17 | If applicable, add screenshots to help explain your problem. 18 | 19 | **Desktop (please complete the following information):** 20 | - Python version (This repo only supports Python 3.9.x or higher). 21 | - Each library version 22 | - OS version 23 | -------------------------------------------------------------------------------- /.github/codeql/codeql-config.yml: -------------------------------------------------------------------------------- 1 | name: "Extended CodeQL Config" 2 | 3 | # This file adds additional queries to the default configuration to make it equivalent to what LGTM checks. 4 | queries: 5 | - name: Security and quality queries 6 | uses: security-and-quality 7 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: pip 4 | directory: "/requirements" 5 | schedule: 6 | interval: weekly 7 | time: "20:00" 8 | open-pull-requests-limit: 10 9 | -------------------------------------------------------------------------------- /.github/pull_request_template.md: -------------------------------------------------------------------------------- 1 | 11 | 12 | #### Reference issue 13 | 14 | 15 | #### What does this implement/fix? 16 | 17 | 18 | #### Additional information 19 | 20 | 21 | #### CheckList 22 | - [ ] Did you add an unittest for your new example or defect fix? 23 | - [ ] Did you add documents for your new example? 24 | - [ ] All CIs are green? (You can check it after submitting) 25 | -------------------------------------------------------------------------------- /.github/workflows/Linux_CI.yml: -------------------------------------------------------------------------------- 1 | name: Linux_CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | 9 | jobs: 10 | build: 11 | 12 | runs-on: ubuntu-latest 13 | strategy: 14 | matrix: 15 | python-version: [ '3.10.6' ] # For mypy error Ref: https://github.com/python/mypy/issues/13627 16 | 17 | name: Python ${{ matrix.python-version }} CI 18 | 19 | steps: 20 | - uses: actions/checkout@v2 21 | - run: git fetch --prune --unshallow 22 | 23 | - name: Setup python 24 | uses: actions/setup-python@v2 25 | with: 26 | python-version: ${{ matrix.python-version }} 27 | - name: Install dependencies 28 | run: | 29 | python --version 30 | python -m pip install --upgrade pip 31 | python -m pip install -r requirements/requirements.txt 32 | - name: do all unit tests 33 | run: bash runtests.sh 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /.github/workflows/MacOS_CI.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: MacOS_CI 4 | 5 | # Controls when the action will run. Triggers the workflow on push or pull request 6 | # events but only for the master branch 7 | on: 8 | push: 9 | branches: 10 | - master 11 | pull_request: 12 | 13 | 14 | jobs: 15 | build: 16 | runs-on: macos-latest 17 | strategy: 18 | matrix: 19 | python-version: [ '3.10.6' ] # For mypy error Ref: https://github.com/python/mypy/issues/13627 20 | name: Python ${{ matrix.python-version }} CI 21 | steps: 22 | - uses: actions/checkout@v2 23 | - run: git fetch --prune --unshallow 24 | 25 | - name: Update bash 26 | run: brew install bash 27 | 28 | - name: Setup python 29 | uses: actions/setup-python@v2 30 | with: 31 | python-version: ${{ matrix.python-version }} 32 | 33 | - name: Install dependencies 34 | run: | 35 | python --version 36 | python -m pip install --upgrade pip 37 | pip install -r requirements/requirements.txt 38 | - name: do all unit tests 39 | run: bash runtests.sh -------------------------------------------------------------------------------- /.github/workflows/circleci-artifacts-redirector.yml: -------------------------------------------------------------------------------- 1 | name: circleci-artifacts-redirector-action 2 | on: [status] 3 | jobs: 4 | circleci_artifacts_redirector_job: 5 | runs-on: ubuntu-latest 6 | name: Run CircleCI artifacts redirector!! 7 | steps: 8 | - name: run-circleci-artifacts-redirector 9 | uses: larsoner/circleci-artifacts-redirector-action@0.3.1 10 | with: 11 | repo-token: ${{ secrets.GITHUB_TOKEN }} 12 | artifact-path: 0/html/index.html 13 | circleci-jobs: build_doc 14 | -------------------------------------------------------------------------------- /.github/workflows/codeql.yml: -------------------------------------------------------------------------------- 1 | name: "Code scanning - action" 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | pull_request: 8 | schedule: 9 | - cron: '0 19 * * 0' 10 | 11 | jobs: 12 | CodeQL-Build: 13 | 14 | # CodeQL runs on ubuntu-latest and windows-latest 15 | runs-on: ubuntu-latest 16 | 17 | steps: 18 | - name: Checkout repository 19 | uses: actions/checkout@v2 20 | with: 21 | # We must fetch at least the immediate parents so that if this is 22 | # a pull request then we can checkout the head. 23 | fetch-depth: 2 24 | 25 | # Initializes the CodeQL tools for scanning. 26 | - name: Initialize CodeQL 27 | uses: github/codeql-action/init@v1 28 | with: 29 | config-file: ./.github/codeql/codeql-config.yml 30 | # Override language selection by uncommenting this and choosing your languages 31 | # with: 32 | # languages: go, javascript, csharp, python, cpp, java 33 | 34 | # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). 35 | # If this step fails, then you should remove it and run the build manually (see below) 36 | - name: Autobuild 37 | uses: github/codeql-action/autobuild@v1 38 | 39 | # ℹ️ Command-line programs to run using the OS shell. 40 | # 📚 https://git.io/JvXDl 41 | 42 | # ✏️ If the Autobuild fails above, remove it and uncomment the following three lines 43 | # and modify them (or add more) to build your code if your project 44 | # uses a compiled language 45 | 46 | #- run: | 47 | # make bootstrap 48 | # make release 49 | 50 | - name: Perform CodeQL Analysis 51 | uses: github/codeql-action/analyze@v1 52 | -------------------------------------------------------------------------------- /.github/workflows/gh-pages.yml: -------------------------------------------------------------------------------- 1 | name: Pages 2 | on: 3 | push: 4 | branches: 5 | - master 6 | jobs: 7 | build: 8 | runs-on: ubuntu-latest 9 | steps: 10 | - name: Setup python 11 | uses: actions/setup-python@v3 12 | - name: Checkout 13 | uses: actions/checkout@master 14 | with: 15 | fetch-depth: 0 # otherwise, you will fail to push refs to dest repo 16 | - name: Install dependencies 17 | run: | 18 | python --version 19 | python -m pip install --upgrade pip 20 | python -m pip install -r requirements/requirements.txt 21 | - name: Build and Commit 22 | uses: sphinx-notes/pages@v2 23 | with: 24 | requirements_path: ./docs/doc_requirements.txt 25 | - name: Push changes 26 | uses: ad-m/github-push-action@master 27 | with: 28 | github_token: ${{ secrets.GITHUB_TOKEN }} 29 | branch: gh-pages 30 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.csv 2 | *.gif 3 | *.g2o 4 | 5 | # Byte-compiled / optimized / DLL files 6 | __pycache__/ 7 | *.py[cod] 8 | *$py.class 9 | _build/ 10 | .idea/ 11 | 12 | # C extensions 13 | *.so 14 | 15 | # Distribution / packaging 16 | .Python 17 | env/ 18 | build/ 19 | develop-eggs/ 20 | dist/ 21 | downloads/ 22 | eggs/ 23 | .eggs/ 24 | lib/ 25 | lib64/ 26 | parts/ 27 | sdist/ 28 | var/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | 33 | .DS_Store 34 | 35 | # PyInstaller 36 | # Usually these files are written by a python script from a template 37 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 38 | *.manifest 39 | *.spec 40 | 41 | # Installer logs 42 | pip-log.txt 43 | pip-delete-this-directory.txt 44 | 45 | # Unit test / coverage reports 46 | htmlcov/ 47 | .tox/ 48 | .coverage 49 | .coverage.* 50 | .cache 51 | nosetests.xml 52 | coverage.xml 53 | *,cover 54 | .hypothesis/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | #Ipython Notebook 70 | .ipynb_checkpoints 71 | 72 | matplotrecorder/* 73 | .vscode/settings.json 74 | -------------------------------------------------------------------------------- /.lgtm.yml: -------------------------------------------------------------------------------- 1 | extraction: 2 | python: 3 | python_setup: 4 | version: 3 5 | -------------------------------------------------------------------------------- /AerialNavigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/AerialNavigation/__init__.py -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/Quadrotor.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for plotting a quadrotor 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | class Quadrotor(): 12 | def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): 13 | self.p1 = np.array([size / 2, 0, 0, 1]).T 14 | self.p2 = np.array([-size / 2, 0, 0, 1]).T 15 | self.p3 = np.array([0, size / 2, 0, 1]).T 16 | self.p4 = np.array([0, -size / 2, 0, 1]).T 17 | 18 | self.x_data = [] 19 | self.y_data = [] 20 | self.z_data = [] 21 | self.show_animation = show_animation 22 | 23 | if self.show_animation: 24 | plt.ion() 25 | fig = plt.figure() 26 | # for stopping simulation with the esc key. 27 | fig.canvas.mpl_connect('key_release_event', 28 | lambda event: [exit(0) if event.key == 'escape' else None]) 29 | 30 | self.ax = fig.add_subplot(111, projection='3d') 31 | 32 | self.update_pose(x, y, z, roll, pitch, yaw) 33 | 34 | def update_pose(self, x, y, z, roll, pitch, yaw): 35 | self.x = x 36 | self.y = y 37 | self.z = z 38 | self.roll = roll 39 | self.pitch = pitch 40 | self.yaw = yaw 41 | self.x_data.append(x) 42 | self.y_data.append(y) 43 | self.z_data.append(z) 44 | 45 | if self.show_animation: 46 | self.plot() 47 | 48 | def transformation_matrix(self): 49 | x = self.x 50 | y = self.y 51 | z = self.z 52 | roll = self.roll 53 | pitch = self.pitch 54 | yaw = self.yaw 55 | return np.array( 56 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], 57 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) 58 | * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], 59 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] 60 | ]) 61 | 62 | def plot(self): # pragma: no cover 63 | T = self.transformation_matrix() 64 | 65 | p1_t = np.matmul(T, self.p1) 66 | p2_t = np.matmul(T, self.p2) 67 | p3_t = np.matmul(T, self.p3) 68 | p4_t = np.matmul(T, self.p4) 69 | 70 | plt.cla() 71 | 72 | self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]], 73 | [p1_t[1], p2_t[1], p3_t[1], p4_t[1]], 74 | [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.') 75 | 76 | self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], 77 | [p1_t[2], p2_t[2]], 'r-') 78 | self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], 79 | [p3_t[2], p4_t[2]], 'r-') 80 | 81 | self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:') 82 | 83 | plt.xlim(-5, 5) 84 | plt.ylim(-5, 5) 85 | self.ax.set_zlim(0, 10) 86 | 87 | plt.pause(0.001) 88 | -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py: -------------------------------------------------------------------------------- 1 | """ 2 | Generates a quintic polynomial trajectory. 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | import numpy as np 8 | 9 | class TrajectoryGenerator(): 10 | def __init__(self, start_pos, des_pos, T, start_vel=[0,0,0], des_vel=[0,0,0], start_acc=[0,0,0], des_acc=[0,0,0]): 11 | self.start_x = start_pos[0] 12 | self.start_y = start_pos[1] 13 | self.start_z = start_pos[2] 14 | 15 | self.des_x = des_pos[0] 16 | self.des_y = des_pos[1] 17 | self.des_z = des_pos[2] 18 | 19 | self.start_x_vel = start_vel[0] 20 | self.start_y_vel = start_vel[1] 21 | self.start_z_vel = start_vel[2] 22 | 23 | self.des_x_vel = des_vel[0] 24 | self.des_y_vel = des_vel[1] 25 | self.des_z_vel = des_vel[2] 26 | 27 | self.start_x_acc = start_acc[0] 28 | self.start_y_acc = start_acc[1] 29 | self.start_z_acc = start_acc[2] 30 | 31 | self.des_x_acc = des_acc[0] 32 | self.des_y_acc = des_acc[1] 33 | self.des_z_acc = des_acc[2] 34 | 35 | self.T = T 36 | 37 | def solve(self): 38 | A = np.array( 39 | [[0, 0, 0, 0, 0, 1], 40 | [self.T**5, self.T**4, self.T**3, self.T**2, self.T, 1], 41 | [0, 0, 0, 0, 1, 0], 42 | [5*self.T**4, 4*self.T**3, 3*self.T**2, 2*self.T, 1, 0], 43 | [0, 0, 0, 2, 0, 0], 44 | [20*self.T**3, 12*self.T**2, 6*self.T, 2, 0, 0] 45 | ]) 46 | 47 | b_x = np.array( 48 | [[self.start_x], 49 | [self.des_x], 50 | [self.start_x_vel], 51 | [self.des_x_vel], 52 | [self.start_x_acc], 53 | [self.des_x_acc] 54 | ]) 55 | 56 | b_y = np.array( 57 | [[self.start_y], 58 | [self.des_y], 59 | [self.start_y_vel], 60 | [self.des_y_vel], 61 | [self.start_y_acc], 62 | [self.des_y_acc] 63 | ]) 64 | 65 | b_z = np.array( 66 | [[self.start_z], 67 | [self.des_z], 68 | [self.start_z_vel], 69 | [self.des_z_vel], 70 | [self.start_z_acc], 71 | [self.des_z_acc] 72 | ]) 73 | 74 | self.x_c = np.linalg.solve(A, b_x) 75 | self.y_c = np.linalg.solve(A, b_y) 76 | self.z_c = np.linalg.solve(A, b_z) -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pathlib 3 | sys.path.append(str(pathlib.Path(__file__).parent)) 4 | -------------------------------------------------------------------------------- /ArmNavigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/ArmNavigation/__init__.py -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/__init__.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pathlib 3 | sys.path.append(str(pathlib.Path(__file__).parent)) 4 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Forward Kinematics for an n-link arm in 3D 3 | Author: Takayuki Murooka (takayuki5168) 4 | """ 5 | import math 6 | from NLinkArm3d import NLinkArm 7 | import random 8 | 9 | 10 | def random_val(min_val, max_val): 11 | return min_val + random.random() * (max_val - min_val) 12 | 13 | 14 | def main(): 15 | print("Start solving Forward Kinematics 10 times") 16 | # init NLinkArm with Denavit-Hartenberg parameters of PR2 17 | n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], 18 | [math.pi / 2, math.pi / 2, 0., 0.], 19 | [0., -math.pi / 2, 0., .4], 20 | [0., math.pi / 2, 0., 0.], 21 | [0., -math.pi / 2, 0., .321], 22 | [0., math.pi / 2, 0., 0.], 23 | [0., 0., 0., 0.]]) 24 | # execute FK 10 times 25 | for _ in range(10): 26 | n_link_arm.set_joint_angles( 27 | [random_val(-1, 1) for _ in range(len(n_link_arm.link_list))]) 28 | 29 | n_link_arm.forward_kinematics(plot=True) 30 | 31 | 32 | if __name__ == "__main__": 33 | main() 34 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse Kinematics for an n-link arm in 3D 3 | Author: Takayuki Murooka (takayuki5168) 4 | """ 5 | import math 6 | from NLinkArm3d import NLinkArm 7 | import random 8 | 9 | 10 | def random_val(min_val, max_val): 11 | return min_val + random.random() * (max_val - min_val) 12 | 13 | 14 | def main(): 15 | print("Start solving Inverse Kinematics 10 times") 16 | # init NLinkArm with Denavit-Hartenberg parameters of PR2 17 | n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], 18 | [math.pi / 2, math.pi / 2, 0., 0.], 19 | [0., -math.pi / 2, 0., .4], 20 | [0., math.pi / 2, 0., 0.], 21 | [0., -math.pi / 2, 0., .321], 22 | [0., math.pi / 2, 0., 0.], 23 | [0., 0., 0., 0.]]) 24 | # execute IK 10 times 25 | for _ in range(10): 26 | n_link_arm.inverse_kinematics([random_val(-0.5, 0.5), 27 | random_val(-0.5, 0.5), 28 | random_val(-0.5, 0.5), 29 | random_val(-0.5, 0.5), 30 | random_val(-0.5, 0.5), 31 | random_val(-0.5, 0.5)], plot=True) 32 | 33 | 34 | if __name__ == "__main__": 35 | main() 36 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for controlling and plotting an arm with an arbitrary number of links. 3 | 4 | Author: Daniel Ingram 5 | """ 6 | 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | 11 | class NLinkArm(object): 12 | def __init__(self, link_lengths, joint_angles, goal, show_animation): 13 | self.show_animation = show_animation 14 | self.n_links = len(link_lengths) 15 | if self.n_links != len(joint_angles): 16 | raise ValueError() 17 | 18 | self.link_lengths = np.array(link_lengths) 19 | self.joint_angles = np.array(joint_angles) 20 | self.points = [[0, 0] for _ in range(self.n_links + 1)] 21 | 22 | self.lim = sum(link_lengths) 23 | self.goal = np.array(goal).T 24 | 25 | if show_animation: # pragma: no cover 26 | self.fig = plt.figure() 27 | self.fig.canvas.mpl_connect('button_press_event', self.click) 28 | 29 | plt.ion() 30 | plt.show() 31 | 32 | self.update_points() 33 | 34 | def update_joints(self, joint_angles): 35 | self.joint_angles = joint_angles 36 | 37 | self.update_points() 38 | 39 | def update_points(self): 40 | for i in range(1, self.n_links + 1): 41 | self.points[i][0] = self.points[i - 1][0] + \ 42 | self.link_lengths[i - 1] * \ 43 | np.cos(np.sum(self.joint_angles[:i])) 44 | self.points[i][1] = self.points[i - 1][1] + \ 45 | self.link_lengths[i - 1] * \ 46 | np.sin(np.sum(self.joint_angles[:i])) 47 | 48 | self.end_effector = np.array(self.points[self.n_links]).T 49 | if self.show_animation: # pragma: no cover 50 | self.plot() 51 | 52 | def plot(self): # pragma: no cover 53 | plt.cla() 54 | # for stopping simulation with the esc key. 55 | plt.gcf().canvas.mpl_connect('key_release_event', 56 | lambda event: [exit(0) if event.key == 'escape' else None]) 57 | 58 | for i in range(self.n_links + 1): 59 | if i is not self.n_links: 60 | plt.plot([self.points[i][0], self.points[i + 1][0]], 61 | [self.points[i][1], self.points[i + 1][1]], 'r-') 62 | plt.plot(self.points[i][0], self.points[i][1], 'ko') 63 | 64 | plt.plot(self.goal[0], self.goal[1], 'gx') 65 | 66 | plt.plot([self.end_effector[0], self.goal[0]], [ 67 | self.end_effector[1], self.goal[1]], 'g--') 68 | 69 | plt.xlim([-self.lim, self.lim]) 70 | plt.ylim([-self.lim, self.lim]) 71 | plt.draw() 72 | plt.pause(0.0001) 73 | 74 | def click(self, event): 75 | self.goal = np.array([event.xdata, event.ydata]).T 76 | self.plot() 77 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/ArmNavigation/n_joint_arm_to_point_control/__init__.py -------------------------------------------------------------------------------- /Bipedal/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Bipedal/__init__.py -------------------------------------------------------------------------------- /Bipedal/bipedal_planner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Bipedal/bipedal_planner/__init__.py -------------------------------------------------------------------------------- /CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as 6 | contributors and maintainers pledge to making participation in our project and 7 | our community a harassment-free experience for everyone, regardless of age, body 8 | size, disability, ethnicity, sex characteristics, gender identity and expression, 9 | level of experience, education, socio-economic status, nationality, personal 10 | appearance, race, religion, or sexual identity and orientation. 11 | 12 | ## Our Standards 13 | 14 | Examples of behavior that contributes to creating a positive environment 15 | include: 16 | 17 | * Using welcoming and inclusive language 18 | * Being respectful of differing viewpoints and experiences 19 | * Gracefully accepting constructive criticism 20 | * Focusing on what is best for the community 21 | * Showing empathy towards other community members 22 | 23 | Examples of unacceptable behavior by participants include: 24 | 25 | * The use of sexualized language or imagery and unwelcome sexual attention or 26 | advances 27 | * Trolling, insulting/derogatory comments, and personal or political attacks 28 | * Public or private harassment 29 | * Publishing others' private information, such as a physical or electronic 30 | address, without explicit permission 31 | * Other conduct which could reasonably be considered inappropriate in a 32 | professional setting 33 | 34 | ## Our Responsibilities 35 | 36 | Project maintainers are responsible for clarifying the standards of acceptable 37 | behavior and are expected to take appropriate and fair corrective action in 38 | response to any instances of unacceptable behavior. 39 | 40 | Project maintainers have the right and responsibility to remove, edit, or 41 | reject comments, commits, code, wiki edits, issues, and other contributions 42 | that are not aligned to this Code of Conduct, or to ban temporarily or 43 | permanently any contributor for other behaviors that they deem inappropriate, 44 | threatening, offensive, or harmful. 45 | 46 | ## Scope 47 | 48 | This Code of Conduct applies both within project spaces and in public spaces 49 | when an individual is representing the project or its community. Examples of 50 | representing a project or community include using an official project e-mail 51 | address, posting via an official social media account, or acting as an appointed 52 | representative at an online or offline event. Representation of a project may be 53 | further defined and clarified by project maintainers. 54 | 55 | ## Enforcement 56 | 57 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 58 | reported by contacting the project team at asakaig@gmail.com. All 59 | complaints will be reviewed and investigated and will result in a response that 60 | is deemed necessary and appropriate to the circumstances. The project team is 61 | obligated to maintain confidentiality with regard to the reporter of an incident. 62 | Further details of specific enforcement policies may be posted separately. 63 | 64 | Project maintainers who do not follow or enforce the Code of Conduct in good 65 | faith may face temporary or permanent repercussions as determined by other 66 | members of the project's leadership. 67 | 68 | ## Attribution 69 | 70 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], version 1.4, 71 | available at https://www.contributor-covenant.org/version/1/4/code-of-conduct.html 72 | 73 | [homepage]: https://www.contributor-covenant.org 74 | 75 | For answers to common questions about this code of conduct, see 76 | https://www.contributor-covenant.org/faq 77 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing to Python 2 | 3 | :+1::tada: First of off, thanks very much for taking the time to contribute! :tada::+1: 4 | 5 | The following is a set of guidelines for contributing to PythonRobotics. 6 | 7 | These are mostly guidelines, not rules. 8 | 9 | Use your best judgment, and feel free to propose changes to this document in a pull request. 10 | 11 | # Before contributing 12 | 13 | ## Taking a look at the paper. 14 | 15 | Please check this paper to understand the philosophy of this project. 16 | 17 | - [\[1808\.10703\] PythonRobotics: a Python code collection of robotics algorithms](https://arxiv.org/abs/1808.10703) ([BibTeX](https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib)) 18 | 19 | ## Check your Python version. 20 | 21 | We only accept a PR for Python 3.8.x or higher. 22 | 23 | We will not accept a PR for Python 2.x. 24 | -------------------------------------------------------------------------------- /Control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Control/__init__.py -------------------------------------------------------------------------------- /Control/move_to_pose/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Control/move_to_pose/__init__.py -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: 4 | https://github.com/AtsushiSakai/PythonRobotics/contributors 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in all 14 | copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 22 | SOFTWARE. 23 | -------------------------------------------------------------------------------- /Localization/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Localization/__init__.py -------------------------------------------------------------------------------- /Mapping/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Mapping/__init__.py -------------------------------------------------------------------------------- /Mapping/gaussian_grid_map/gaussian_grid_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | 2D gaussian grid map sample 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import norm 13 | 14 | EXTEND_AREA = 10.0 # [m] grid map extention length 15 | 16 | show_animation = True 17 | 18 | 19 | def generate_gaussian_grid_map(ox, oy, xyreso, std): 20 | 21 | minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) 22 | 23 | gmap = [[0.0 for i in range(yw)] for i in range(xw)] 24 | 25 | for ix in range(xw): 26 | for iy in range(yw): 27 | 28 | x = ix * xyreso + minx 29 | y = iy * xyreso + miny 30 | 31 | # Search minimum distance 32 | mindis = float("inf") 33 | for (iox, ioy) in zip(ox, oy): 34 | d = math.hypot(iox - x, ioy - y) 35 | if mindis >= d: 36 | mindis = d 37 | 38 | pdf = (1.0 - norm.cdf(mindis, 0.0, std)) 39 | gmap[ix][iy] = pdf 40 | 41 | return gmap, minx, maxx, miny, maxy 42 | 43 | 44 | def calc_grid_map_config(ox, oy, xyreso): 45 | minx = round(min(ox) - EXTEND_AREA / 2.0) 46 | miny = round(min(oy) - EXTEND_AREA / 2.0) 47 | maxx = round(max(ox) + EXTEND_AREA / 2.0) 48 | maxy = round(max(oy) + EXTEND_AREA / 2.0) 49 | xw = int(round((maxx - minx) / xyreso)) 50 | yw = int(round((maxy - miny) / xyreso)) 51 | 52 | return minx, miny, maxx, maxy, xw, yw 53 | 54 | 55 | def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): 56 | x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), 57 | slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] 58 | plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) 59 | plt.axis("equal") 60 | 61 | 62 | def main(): 63 | print(__file__ + " start!!") 64 | 65 | xyreso = 0.5 # xy grid resolution 66 | STD = 5.0 # standard diviation for gaussian distribution 67 | 68 | for i in range(5): 69 | ox = (np.random.rand(4) - 0.5) * 10.0 70 | oy = (np.random.rand(4) - 0.5) * 10.0 71 | gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map( 72 | ox, oy, xyreso, STD) 73 | 74 | if show_animation: # pragma: no cover 75 | plt.cla() 76 | # for stopping simulation with the esc key. 77 | plt.gcf().canvas.mpl_connect('key_release_event', 78 | lambda event: [exit(0) if event.key == 'escape' else None]) 79 | draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) 80 | plt.plot(ox, oy, "xr") 81 | plt.plot(0.0, 0.0, "ob") 82 | plt.pause(1.0) 83 | 84 | 85 | if __name__ == '__main__': 86 | main() 87 | -------------------------------------------------------------------------------- /Mapping/grid_map_lib/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/Mapping/grid_map_lib/__init__.py -------------------------------------------------------------------------------- /Mapping/rectangle_fitting/__init_.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pathlib 3 | sys.path.append(str(pathlib.Path(__file__).parent)) 4 | -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/unicycle_model.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Unicycle model class 4 | 5 | author Atsushi Sakai 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | 12 | dt = 0.05 # [s] 13 | L = 0.9 # [m] 14 | steer_max = np.deg2rad(40.0) 15 | curvature_max = math.tan(steer_max) / L 16 | curvature_max = 1.0 / curvature_max + 1.0 17 | 18 | accel_max = 5.0 19 | 20 | 21 | class State: 22 | 23 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 24 | self.x = x 25 | self.y = y 26 | self.yaw = yaw 27 | self.v = v 28 | 29 | 30 | def update(state, a, delta): 31 | 32 | state.x = state.x + state.v * math.cos(state.yaw) * dt 33 | state.y = state.y + state.v * math.sin(state.yaw) * dt 34 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 35 | state.yaw = pi_2_pi(state.yaw) 36 | state.v = state.v + a * dt 37 | 38 | return state 39 | 40 | 41 | def pi_2_pi(angle): 42 | return (angle + math.pi) % (2 * math.pi) - math.pi 43 | 44 | 45 | if __name__ == '__main__': # pragma: no cover 46 | print("start unicycle simulation") 47 | import matplotlib.pyplot as plt 48 | 49 | T = 100 50 | a = [1.0] * T 51 | delta = [np.deg2rad(1.0)] * T 52 | # print(delta) 53 | # print(a, delta) 54 | 55 | state = State() 56 | 57 | x = [] 58 | y = [] 59 | yaw = [] 60 | v = [] 61 | 62 | for (ai, di) in zip(a, delta): 63 | state = update(state, ai, di) 64 | 65 | x.append(state.x) 66 | y.append(state.y) 67 | yaw.append(state.yaw) 68 | v.append(state.v) 69 | 70 | plt.subplots(1) 71 | plt.plot(x, y) 72 | plt.axis("equal") 73 | plt.grid(True) 74 | 75 | plt.subplots(1) 76 | plt.plot(v) 77 | plt.grid(True) 78 | 79 | plt.show() 80 | -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/CubicSpline/__init__.py -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/spline_continuity.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | from scipy import interpolate 5 | 6 | 7 | class Spline2D: 8 | 9 | def __init__(self, x, y, kind="cubic"): 10 | self.s = self.__calc_s(x, y) 11 | self.sx = interpolate.interp1d(self.s, x, kind=kind) 12 | self.sy = interpolate.interp1d(self.s, y, kind=kind) 13 | 14 | def __calc_s(self, x, y): 15 | self.ds = np.hypot(np.diff(x), np.diff(y)) 16 | s = [0.0] 17 | s.extend(np.cumsum(self.ds)) 18 | return s 19 | 20 | def calc_position(self, s): 21 | x = self.sx(s) 22 | y = self.sy(s) 23 | return x, y 24 | 25 | 26 | def main(): 27 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 28 | y = [0.7, -6, -5, -3.5, 0.0, 5.0, -2.0] 29 | ds = 0.1 # [m] distance of each interpolated points 30 | 31 | plt.subplots(1) 32 | plt.plot(x, y, "xb", label="Data points") 33 | 34 | for (kind, label) in [("linear", "C0 (Linear spline)"), 35 | ("quadratic", "C0 & C1 (Quadratic spline)"), 36 | ("cubic", "C0 & C1 & C2 (Cubic spline)")]: 37 | rx, ry = [], [] 38 | sp = Spline2D(x, y, kind=kind) 39 | s = np.arange(0, sp.s[-1], ds) 40 | for i_s in s: 41 | ix, iy = sp.calc_position(i_s) 42 | rx.append(ix) 43 | ry.append(iy) 44 | plt.plot(rx, ry, "-", label=label) 45 | 46 | plt.grid(True) 47 | plt.axis("equal") 48 | plt.xlabel("x[m]") 49 | plt.ylabel("y[m]") 50 | plt.legend() 51 | plt.show() 52 | 53 | 54 | if __name__ == '__main__': 55 | main() 56 | -------------------------------------------------------------------------------- /PathPlanning/DubinsPath/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/DubinsPath/__init__.py -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/__init__.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__))) 5 | -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/car.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Car model for Hybrid A* path planning 4 | 5 | author: Zheng Zh (@Zhengzh) 6 | 7 | """ 8 | 9 | import sys 10 | import pathlib 11 | root_dir = pathlib.Path(__file__).parent.parent.parent 12 | sys.path.append(str(root_dir)) 13 | 14 | from math import cos, sin, tan, pi 15 | 16 | import matplotlib.pyplot as plt 17 | import numpy as np 18 | 19 | from utils.angle import rot_mat_2d 20 | 21 | WB = 3.0 # rear to front wheel 22 | W = 2.0 # width of car 23 | LF = 3.3 # distance from rear to vehicle front end 24 | LB = 1.0 # distance from rear to vehicle back end 25 | MAX_STEER = pi/5 # [rad] maximum steering angle 26 | 27 | BUBBLE_DIST = (LF - LB) / 2.0 # distance from rear to center of vehicle. 28 | BUBBLE_R = np.hypot((LF + LB) / 2.0, W / 2.0) # bubble radius 29 | 30 | # vehicle rectangle vertices 31 | VRX = [LF, LF, -LB, -LB, LF] 32 | VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2] 33 | 34 | 35 | def check_car_collision(x_list, y_list, yaw_list, ox, oy, kd_tree): 36 | for i_x, i_y, i_yaw in zip(x_list, y_list, yaw_list): 37 | cx = i_x + BUBBLE_DIST * cos(i_yaw) 38 | cy = i_y + BUBBLE_DIST * sin(i_yaw) 39 | 40 | ids = kd_tree.query_ball_point([cx, cy], BUBBLE_R) 41 | 42 | if not ids: 43 | continue 44 | 45 | if not rectangle_check(i_x, i_y, i_yaw, 46 | [ox[i] for i in ids], [oy[i] for i in ids]): 47 | return False # collision 48 | 49 | return True # no collision 50 | 51 | 52 | def rectangle_check(x, y, yaw, ox, oy): 53 | # transform obstacles to base link frame 54 | rot = rot_mat_2d(yaw) 55 | for iox, ioy in zip(ox, oy): 56 | tx = iox - x 57 | ty = ioy - y 58 | converted_xy = np.stack([tx, ty]).T @ rot 59 | rx, ry = converted_xy[0], converted_xy[1] 60 | 61 | if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0): 62 | return False # no collision 63 | 64 | return True # collision 65 | 66 | 67 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 68 | """Plot arrow.""" 69 | if not isinstance(x, float): 70 | for (i_x, i_y, i_yaw) in zip(x, y, yaw): 71 | plot_arrow(i_x, i_y, i_yaw) 72 | else: 73 | plt.arrow(x, y, length * cos(yaw), length * sin(yaw), 74 | fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4) 75 | 76 | 77 | def plot_car(x, y, yaw): 78 | car_color = '-k' 79 | c, s = cos(yaw), sin(yaw) 80 | rot = rot_mat_2d(-yaw) 81 | car_outline_x, car_outline_y = [], [] 82 | for rx, ry in zip(VRX, VRY): 83 | converted_xy = np.stack([rx, ry]).T @ rot 84 | car_outline_x.append(converted_xy[0]+x) 85 | car_outline_y.append(converted_xy[1]+y) 86 | 87 | arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw 88 | plot_arrow(arrow_x, arrow_y, arrow_yaw) 89 | 90 | plt.plot(car_outline_x, car_outline_y, car_color) 91 | 92 | 93 | def pi_2_pi(angle): 94 | return (angle + pi) % (2 * pi) - pi 95 | 96 | 97 | def move(x, y, yaw, distance, steer, L=WB): 98 | x += distance * cos(yaw) 99 | y += distance * sin(yaw) 100 | yaw += pi_2_pi(distance * tan(steer) / L) # distance/2 101 | # yaw += pi_2_pi(steer) 102 | 103 | return x, y, yaw 104 | 105 | 106 | def main(): 107 | x, y, yaw = 0., 0., 1. 108 | plt.axis('equal') 109 | plot_car(x, y, yaw) 110 | plt.show() 111 | 112 | 113 | if __name__ == '__main__': 114 | main() 115 | -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/robot.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import pathlib 3 | root_dir = pathlib.Path(__file__).parent.parent.parent 4 | sys.path.append(str(root_dir)) 5 | 6 | from math import cos, sin, tan, pi 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | 11 | from utils.angle import rot_mat_2d 12 | 13 | R = 1.74 # robot raduis 14 | WD = 1.07*2 # wheel distance 15 | MAX_STEER = pi/4 # [rad] maximum steering angle 16 | 17 | def check_collision(x_list, y_list, kd_tree): 18 | for i_x, i_y in zip(x_list, y_list): 19 | if (kd_tree.query_ball_point([i_x, i_y], R)): 20 | return True # collision 21 | 22 | return False # no collision 23 | 24 | def plot_arrow(x, y, yaw, length=2.0, width=0.5, fc="r", ec="k"): 25 | """Plot arrow.""" 26 | if not isinstance(x, float): 27 | for (i_x, i_y, i_yaw) in zip(x, y, yaw): 28 | plot_arrow(i_x, i_y, i_yaw) 29 | else: 30 | plt.arrow(x, y, length * cos(yaw), length * sin(yaw), 31 | fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4) 32 | 33 | def plot_robot(x, y, yaw): 34 | color = 'k' 35 | circle = plt.Circle((x, y), R, color=color, fill=False) 36 | plt.gca().add_patch(circle) 37 | plot_arrow(x, y, yaw) 38 | 39 | def pi_2_pi(angle): 40 | return (angle + pi) % (2 * pi) - pi 41 | 42 | def move(x, y, yaw, distance, steer): 43 | x += distance * cos(yaw) 44 | y += distance * sin(yaw) 45 | yaw += pi_2_pi(steer) 46 | 47 | return x, y, yaw 48 | 49 | def main(): 50 | x, y, yaw = 0., 0., 1. 51 | plt.axis('equal') 52 | plot_robot(x, y, yaw) 53 | plt.show() 54 | 55 | if __name__ == '__main__': 56 | main() -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/tempCodeRunnerFile.py: -------------------------------------------------------------------------------- 1 | def calc_motion_inputs(): 2 | for steer in np.concatenate((np.linspace(-MAX_STEER, MAX_STEER, 3 | N_STEER), [0.0])): 4 | for d in [1, -1]: 5 | yield [steer, d] -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/test.py: -------------------------------------------------------------------------------- 1 | import heapq 2 | 3 | heap = [10, 33, 13] 4 | 5 | # Insert elements into the heap 6 | heapq.heappush(heap, 4) 7 | heapq.heappush(heap, 5) 8 | heapq.heappush(heap, 3) 9 | heapq.heappush(heap, 2) 10 | heapq.heappush(heap, 1) 11 | 12 | print(heap) 13 | 14 | heapq.heappop(heap) 15 | 16 | print(heap) -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/lookup_table_generator.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Lookup Table generation for model predictive trajectory generator 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | import sys 9 | import pathlib 10 | path_planning_dir = pathlib.Path(__file__).parent.parent 11 | sys.path.append(str(path_planning_dir)) 12 | 13 | from matplotlib import pyplot as plt 14 | import numpy as np 15 | import math 16 | 17 | from ModelPredictiveTrajectoryGenerator import trajectory_generator,\ 18 | motion_model 19 | 20 | 21 | def calc_states_list(max_yaw=np.deg2rad(-30.0)): 22 | 23 | x = np.arange(10.0, 30.0, 5.0) 24 | y = np.arange(0.0, 20.0, 2.0) 25 | yaw = np.arange(-max_yaw, max_yaw, max_yaw) 26 | 27 | states = [] 28 | for iyaw in yaw: 29 | for iy in y: 30 | for ix in x: 31 | states.append([ix, iy, iyaw]) 32 | print("n_state:", len(states)) 33 | 34 | return states 35 | 36 | 37 | def search_nearest_one_from_lookup_table(tx, ty, tyaw, lookup_table): 38 | mind = float("inf") 39 | minid = -1 40 | 41 | for (i, table) in enumerate(lookup_table): 42 | 43 | dx = tx - table[0] 44 | dy = ty - table[1] 45 | dyaw = tyaw - table[2] 46 | d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2) 47 | if d <= mind: 48 | minid = i 49 | mind = d 50 | 51 | # print(minid) 52 | 53 | return lookup_table[minid] 54 | 55 | 56 | def save_lookup_table(file_name, table): 57 | np.savetxt(file_name, np.array(table), 58 | fmt='%s', delimiter=",", header="x,y,yaw,s,km,kf", comments="") 59 | 60 | print("lookup table file is saved as " + file_name) 61 | 62 | 63 | def generate_lookup_table(): 64 | states = calc_states_list(max_yaw=np.deg2rad(-30.0)) 65 | k0 = 0.0 66 | 67 | # x, y, yaw, s, km, kf 68 | lookup_table = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] 69 | 70 | for state in states: 71 | best_p = search_nearest_one_from_lookup_table( 72 | state[0], state[1], state[2], lookup_table) 73 | 74 | target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) 75 | init_p = np.array( 76 | [np.hypot(state[0], state[1]), best_p[4], best_p[5]]).reshape(3, 1) 77 | 78 | x, y, yaw, p = trajectory_generator.optimize_trajectory(target, 79 | k0, init_p) 80 | 81 | if x is not None: 82 | print("find good path") 83 | lookup_table.append( 84 | [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) 85 | 86 | print("finish lookup table generation") 87 | 88 | save_lookup_table("lookup_table.csv", lookup_table) 89 | 90 | for table in lookup_table: 91 | x_c, y_c, yaw_c = motion_model.generate_trajectory( 92 | table[3], table[4], table[5], k0) 93 | plt.plot(x_c, y_c, "-r") 94 | x_c, y_c, yaw_c = motion_model.generate_trajectory( 95 | table[3], -table[4], -table[5], k0) 96 | plt.plot(x_c, y_c, "-r") 97 | 98 | plt.grid(True) 99 | plt.axis("equal") 100 | plt.show() 101 | 102 | print("Done") 103 | 104 | 105 | def main(): 106 | generate_lookup_table() 107 | 108 | 109 | if __name__ == '__main__': 110 | main() 111 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import scipy.interpolate 4 | 5 | # motion parameter 6 | L = 1.0 # wheel base 7 | ds = 0.1 # course distanse 8 | v = 10.0 / 3.6 # velocity [m/s] 9 | 10 | 11 | class State: 12 | 13 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 14 | self.x = x 15 | self.y = y 16 | self.yaw = yaw 17 | self.v = v 18 | 19 | 20 | def pi_2_pi(angle): 21 | return (angle + math.pi) % (2 * math.pi) - math.pi 22 | 23 | 24 | def update(state, v, delta, dt, L): 25 | 26 | state.v = v 27 | state.x = state.x + state.v * math.cos(state.yaw) * dt 28 | state.y = state.y + state.v * math.sin(state.yaw) * dt 29 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 30 | state.yaw = pi_2_pi(state.yaw) 31 | 32 | return state 33 | 34 | 35 | def generate_trajectory(s, km, kf, k0): 36 | 37 | n = s / ds 38 | time = s / v # [s] 39 | 40 | if isinstance(time, type(np.array([]))): time = time[0] 41 | if isinstance(km, type(np.array([]))): km = km[0] 42 | if isinstance(kf, type(np.array([]))): kf = kf[0] 43 | 44 | tk = np.array([0.0, time / 2.0, time]) 45 | kk = np.array([k0, km, kf]) 46 | t = np.arange(0.0, time, time / n) 47 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 48 | kp = [fkp(ti) for ti in t] 49 | dt = float(time / n) 50 | 51 | # plt.plot(t, kp) 52 | # plt.show() 53 | 54 | state = State() 55 | x, y, yaw = [state.x], [state.y], [state.yaw] 56 | 57 | for ikp in kp: 58 | state = update(state, v, ikp, dt, L) 59 | x.append(state.x) 60 | y.append(state.y) 61 | yaw.append(state.yaw) 62 | 63 | return x, y, yaw 64 | 65 | 66 | def generate_last_state(s, km, kf, k0): 67 | 68 | n = s / ds 69 | time = s / v # [s] 70 | 71 | if isinstance(time, type(np.array([]))): time = time[0] 72 | if isinstance(km, type(np.array([]))): km = km[0] 73 | if isinstance(kf, type(np.array([]))): kf = kf[0] 74 | 75 | tk = np.array([0.0, time / 2.0, time]) 76 | kk = np.array([k0, km, kf]) 77 | t = np.arange(0.0, time, time / n) 78 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 79 | kp = [fkp(ti) for ti in t] 80 | dt = time / n 81 | 82 | # plt.plot(t, kp) 83 | # plt.show() 84 | 85 | state = State() 86 | 87 | _ = [update(state, v, ikp, dt, L) for ikp in kp] 88 | 89 | return state.x, state.y, state.yaw 90 | -------------------------------------------------------------------------------- /PathPlanning/RRT/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/RRT/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRT/sobol/__init__.py: -------------------------------------------------------------------------------- 1 | from .sobol import i4_sobol as sobol_quasirand 2 | -------------------------------------------------------------------------------- /PathPlanning/RRTDubins/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/RRTDubins/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRTStar/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/RRTStar/__init__.py -------------------------------------------------------------------------------- /PathPlanning/SpiralSpanningTreeCPP/map/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/SpiralSpanningTreeCPP/map/test.png -------------------------------------------------------------------------------- /PathPlanning/SpiralSpanningTreeCPP/map/test_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/SpiralSpanningTreeCPP/map/test_2.png -------------------------------------------------------------------------------- /PathPlanning/SpiralSpanningTreeCPP/map/test_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/SpiralSpanningTreeCPP/map/test_3.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/StateLatticePlanner/__init__.py -------------------------------------------------------------------------------- /PathPlanning/VisibilityRoadMap/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/VisibilityRoadMap/__init__.py -------------------------------------------------------------------------------- /PathPlanning/VisibilityRoadMap/geometry.py: -------------------------------------------------------------------------------- 1 | class Geometry: 2 | 3 | class Point: 4 | def __init__(self, x, y): 5 | self.x = x 6 | self.y = y 7 | 8 | @staticmethod 9 | def is_seg_intersect(p1, q1, p2, q2): 10 | 11 | def on_segment(p, q, r): 12 | if ((q.x <= max(p.x, r.x)) and (q.x >= min(p.x, r.x)) and 13 | (q.y <= max(p.y, r.y)) and (q.y >= min(p.y, r.y))): 14 | return True 15 | return False 16 | 17 | def orientation(p, q, r): 18 | val = (float(q.y - p.y) * (r.x - q.x)) - ( 19 | float(q.x - p.x) * (r.y - q.y)) 20 | if val > 0: 21 | return 1 22 | if val < 0: 23 | return 2 24 | return 0 25 | 26 | # Find the 4 orientations required for 27 | # the general and special cases 28 | o1 = orientation(p1, q1, p2) 29 | o2 = orientation(p1, q1, q2) 30 | o3 = orientation(p2, q2, p1) 31 | o4 = orientation(p2, q2, q1) 32 | 33 | if (o1 != o2) and (o3 != o4): 34 | return True 35 | if (o1 == 0) and on_segment(p1, p2, q1): 36 | return True 37 | if (o2 == 0) and on_segment(p1, q2, q1): 38 | return True 39 | if (o3 == 0) and on_segment(p2, p1, q2): 40 | return True 41 | if (o4 == 0) and on_segment(p2, q1, q2): 42 | return True 43 | 44 | return False 45 | -------------------------------------------------------------------------------- /PathPlanning/VoronoiRoadMap/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/VoronoiRoadMap/__init__.py -------------------------------------------------------------------------------- /PathPlanning/WavefrontCPP/map/test.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/WavefrontCPP/map/test.png -------------------------------------------------------------------------------- /PathPlanning/WavefrontCPP/map/test_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/WavefrontCPP/map/test_2.png -------------------------------------------------------------------------------- /PathPlanning/WavefrontCPP/map/test_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/WavefrontCPP/map/test_3.png -------------------------------------------------------------------------------- /PathPlanning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathPlanning/__init__.py -------------------------------------------------------------------------------- /PathTracking/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathTracking/__init__.py -------------------------------------------------------------------------------- /PathTracking/lqr_steer_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/PathTracking/lqr_steer_control/__init__.py -------------------------------------------------------------------------------- /SECURITY.md: -------------------------------------------------------------------------------- 1 | # Security Policy 2 | 3 | ## Supported Versions 4 | 5 | In this project, we are only support latest code. 6 | 7 | | Version | Supported | 8 | | ------- | ------------------ | 9 | | latest | :white_check_mark: | 10 | 11 | ## Reporting a Vulnerability 12 | 13 | If you find any security related problem, let us know by creating an issue about it. 14 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/data/README.rst: -------------------------------------------------------------------------------- 1 | Acknowledgments and References 2 | ============================== 3 | 4 | Thanks to Luca Larlone for allowing inclusion of the `Intel dataset `_ in this repo. 5 | 6 | 1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492. 7 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Graph SLAM solver in Python. 8 | 9 | """ 10 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/edge/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/load.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Functions for loading graphs. 8 | 9 | """ 10 | 11 | import logging 12 | 13 | import numpy as np 14 | 15 | from .edge.edge_odometry import EdgeOdometry 16 | from .graph import Graph 17 | from .pose.se2 import PoseSE2 18 | from .util import upper_triangular_matrix_to_full_matrix 19 | from .vertex import Vertex 20 | 21 | _LOGGER = logging.getLogger(__name__) 22 | 23 | 24 | def load_g2o_se2(infile): 25 | """Load an :math:`SE(2)` graph from a .g2o file. 26 | 27 | Parameters 28 | ---------- 29 | infile : str 30 | The path to the .g2o file 31 | 32 | Returns 33 | ------- 34 | Graph 35 | The loaded graph 36 | 37 | """ 38 | edges = [] 39 | vertices = [] 40 | 41 | with open(infile) as f: 42 | for line in f.readlines(): 43 | if line.startswith("VERTEX_SE2"): 44 | numbers = line[10:].split() 45 | arr = np.array([float(number) for number in numbers[1:]], 46 | dtype=float) 47 | p = PoseSE2(arr[:2], arr[2]) 48 | v = Vertex(int(numbers[0]), p) 49 | vertices.append(v) 50 | continue 51 | 52 | if line.startswith("EDGE_SE2"): 53 | numbers = line[9:].split() 54 | arr = np.array([float(number) for number in numbers[2:]], 55 | dtype=float) 56 | vertex_ids = [int(numbers[0]), int(numbers[1])] 57 | estimate = PoseSE2(arr[:2], arr[2]) 58 | information = upper_triangular_matrix_to_full_matrix(arr[3:], 3) 59 | e = EdgeOdometry(vertex_ids, information, estimate) 60 | edges.append(e) 61 | continue 62 | 63 | if line.strip(): 64 | _LOGGER.warning("Line not supported -- '%s'", line.rstrip()) 65 | 66 | return Graph(edges, vertices) 67 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/pose/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/util.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Utility functions used throughout the package. 8 | 9 | """ 10 | 11 | import numpy as np 12 | 13 | 14 | TWO_PI = 2 * np.pi 15 | 16 | 17 | def neg_pi_to_pi(angle): 18 | r"""Normalize ``angle`` to be in :math:`[-\pi, \pi)`. 19 | 20 | Parameters 21 | ---------- 22 | angle : float 23 | An angle (in radians) 24 | 25 | Returns 26 | ------- 27 | float 28 | The angle normalized to :math:`[-\pi, \pi)` 29 | 30 | """ 31 | return (angle + np.pi) % (TWO_PI) - np.pi 32 | 33 | 34 | def solve_for_edge_dimensionality(n): 35 | r"""Solve for the dimensionality of an edge. 36 | 37 | In a .g2o file, an edge is specified as `` ``, where only the upper triangular portion of the matrix is provided. 38 | 39 | This solves the problem: 40 | 41 | .. math:: 42 | 43 | d + \frac{d (d + 1)}{2} = n 44 | 45 | Returns 46 | ------- 47 | int 48 | The dimensionality of the edge 49 | 50 | """ 51 | return int(round(np.sqrt(2 * n + 2.25) - 1.5)) 52 | 53 | 54 | def upper_triangular_matrix_to_full_matrix(arr, n): 55 | """Given an upper triangular matrix, return the full matrix. 56 | 57 | Parameters 58 | ---------- 59 | arr : np.ndarray 60 | The upper triangular portion of the matrix 61 | n : int 62 | The size of the matrix 63 | 64 | Returns 65 | ------- 66 | mat : np.ndarray 67 | The full matrix 68 | 69 | """ 70 | triu0 = np.triu_indices(n, 0) 71 | tril1 = np.tril_indices(n, -1) 72 | 73 | mat = np.zeros((n, n), dtype=float) 74 | mat[triu0] = arr 75 | mat[tril1] = mat.T[tril1] 76 | 77 | return mat 78 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/vertex.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """A ``Vertex`` class. 8 | 9 | """ 10 | 11 | import matplotlib.pyplot as plt 12 | 13 | 14 | # pylint: disable=too-few-public-methods 15 | class Vertex: 16 | """A class for representing a vertex in Graph SLAM. 17 | 18 | Parameters 19 | ---------- 20 | vertex_id : int 21 | The vertex's unique ID 22 | pose : graphslam.pose.se2.PoseSE2 23 | The pose associated with the vertex 24 | vertex_index : int, None 25 | The vertex's index in the graph's ``vertices`` list 26 | 27 | Attributes 28 | ---------- 29 | id : int 30 | The vertex's unique ID 31 | index : int, None 32 | The vertex's index in the graph's ``vertices`` list 33 | pose : graphslam.pose.se2.PoseSE2 34 | The pose associated with the vertex 35 | 36 | """ 37 | def __init__(self, vertex_id, pose, vertex_index=None): 38 | self.id = vertex_id 39 | self.pose = pose 40 | self.index = vertex_index 41 | 42 | def to_g2o(self): 43 | """Export the vertex to the .g2o format. 44 | 45 | Returns 46 | ------- 47 | str 48 | The vertex in .g2o format 49 | 50 | """ 51 | return "VERTEX_SE2 {} {} {} {}\n".format(self.id, self.pose[0], self.pose[1], self.pose[2]) 52 | 53 | def plot(self, color='r', marker='o', markersize=3): 54 | """Plot the vertex. 55 | 56 | Parameters 57 | ---------- 58 | color : str 59 | The color that will be used to plot the vertex 60 | marker : str 61 | The marker that will be used to plot the vertex 62 | markersize : int 63 | The size of the plotted vertex 64 | 65 | """ 66 | x, y = self.pose.position 67 | plt.plot(x, y, color=color, marker=marker, markersize=markersize) 68 | -------------------------------------------------------------------------------- /SLAM/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/SLAM/__init__.py -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/__init__.py -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | show_downloads: true 3 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | os: Visual Studio 2022 2 | 3 | environment: 4 | global: 5 | # SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the 6 | # /E:ON and /V:ON options are not enabled in the batch script intepreter 7 | # See: http://stackoverflow.com/a/13751649/163740 8 | CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd" 9 | 10 | matrix: 11 | - PYTHON_DIR: C:\Python310-x64 12 | 13 | branches: 14 | only: 15 | - master 16 | 17 | init: 18 | - "ECHO %PYTHON_DIR%" 19 | 20 | install: 21 | # If there is a newer build queued for the same PR, cancel this one. 22 | # The AppVeyor 'rollout builds' option is supposed to serve the same 23 | # purpose but it is problematic because it tends to cancel builds pushed 24 | # directly to master instead of just PR builds (or the converse). 25 | # credits: JuliaLang developers. 26 | - ps: if ($env:APPVEYOR_PULL_REQUEST_NUMBER -and $env:APPVEYOR_BUILD_NUMBER -ne ((Invoke-RestMethod ` 27 | https://ci.appveyor.com/api/projects/$env:APPVEYOR_ACCOUNT_NAME/$env:APPVEYOR_PROJECT_SLUG/history?recordsNumber=50).builds | ` 28 | Where-Object pullRequestId -eq $env:APPVEYOR_PULL_REQUEST_NUMBER)[0].buildNumber) { ` 29 | throw "There are newer queued builds for this pull request, failing early." } 30 | - ECHO "Filesystem root:" 31 | - ps: "ls \"C:/\"" 32 | 33 | # Prepend newly installed Python to the PATH of this build (this cannot be 34 | # done from inside the powershell script as it would require to restart 35 | # the parent CMD process). 36 | - SET PATH=%PYTHON_DIR%;%PYTHON_DIR%\\Scripts;%PATH% 37 | - SET PATH=%PYTHON%;%PYTHON%\Scripts;%PYTHON%\Library\bin;%PATH% 38 | - SET PATH=%PATH%;C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\bin 39 | - "python -m pip install --upgrade pip" 40 | - "python -m pip install -r requirements/requirements.txt" 41 | - "python -m pip install pytest-xdist" 42 | 43 | # Check that we have the expected version and architecture for Python 44 | - "python --version" 45 | - "python -c \"import struct; print(struct.calcsize('P') * 8)\"" 46 | 47 | build: off 48 | 49 | test_script: 50 | - "pytest tests -n auto -Werror --durations=0" 51 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = PythonRobotics 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | ## Python Robotics Documentation 2 | 3 | This folder contains documentation for the Python Robotics project. 4 | 5 | 6 | ### Build the Documentation 7 | 8 | #### Install Sphinx and Theme 9 | 10 | ``` 11 | pip install sphinx sphinx-autobuild sphinx-rtd-theme 12 | ``` 13 | 14 | #### Building the Docs 15 | 16 | In the `docs/` folder: 17 | ``` 18 | make html 19 | ``` 20 | 21 | if you want to building each time a file is changed: 22 | 23 | ``` 24 | sphinx-autobuild . _build/html 25 | ``` 26 | 27 | #### Check the generated doc 28 | 29 | Open the index.html file under docs/_build/ 30 | -------------------------------------------------------------------------------- /docs/_static/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/_static/.gitkeep -------------------------------------------------------------------------------- /docs/_static/custom.css: -------------------------------------------------------------------------------- 1 | /* 2 | * Necessary parts from 3 | * Sphinx stylesheet -- basic theme 4 | * absent from sphinx_rtd_theme 5 | * (see https://github.com/readthedocs/sphinx_rtd_theme/issues/301) 6 | * Ref https://github.com/PyAbel/PyAbel/commit/7e4dee81eac3f0a6955a85a4a42cf04a4e0d995c 7 | */ 8 | 9 | /* -- math display ---------------------------------------------------------- */ 10 | 11 | span.eqno { 12 | float: right; 13 | } 14 | 15 | span.eqno a.headerlink { 16 | position: absolute; 17 | z-index: 1; 18 | visibility: hidden; 19 | } 20 | 21 | div.math:hover a.headerlink { 22 | visibility: visible; 23 | } 24 | -------------------------------------------------------------------------------- /docs/_static/img/doc_ci.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/_static/img/doc_ci.png -------------------------------------------------------------------------------- /docs/_templates/layout.html: -------------------------------------------------------------------------------- 1 | {% extends "!layout.html" %} 2 | 3 | {% block sidebartitle %} 4 | {{ super() }} 5 | 7 | 8 | 14 | 17 | {% endblock %} 18 | -------------------------------------------------------------------------------- /docs/doc_requirements.txt: -------------------------------------------------------------------------------- 1 | sphinx == 4.3.2 # For sphinx documentation 2 | sphinx_rtd_theme == 1.0.0 3 | IPython == 7.31.1 # For sphinx documentation 4 | sphinxcontrib-napoleon == 0.7 # For auto doc 5 | -------------------------------------------------------------------------------- /docs/getting_started_main.rst: -------------------------------------------------------------------------------- 1 | .. _`getting started`: 2 | 3 | Getting Started 4 | =============== 5 | 6 | What is this? 7 | ------------- 8 | 9 | This is an Open Source Software (OSS) project: PythonRobotics, which is a Python code collection of robotics algorithms. 10 | 11 | The focus of the project is on autonomous navigation, and the goal is for beginners in robotics to understand the basic ideas behind each algorithm. 12 | 13 | In this project, the algorithms which are practical and widely used in both academia and industry are selected. 14 | 15 | Each sample code is written in Python3 and only depends on some standard modules for readability and ease of use. 16 | 17 | It includes intuitive animations to understand the behavior of the simulation. 18 | 19 | 20 | See this paper for more details: 21 | 22 | - PythonRobotics: a Python code collection of robotics algorithms: https://arxiv.org/abs/1808.10703 23 | 24 | .. _`Requirements`: 25 | 26 | Requirements 27 | ------------- 28 | 29 | - `Python 3.10.x`_ 30 | - `NumPy`_ 31 | - `SciPy`_ 32 | - `Matplotlib`_ 33 | - `cvxpy`_ 34 | 35 | For development: 36 | 37 | - pytest (for unit tests) 38 | - pytest-xdist (for parallel unit tests) 39 | - mypy (for type check) 40 | - sphinx (for document generation) 41 | - pycodestyle (for code style check) 42 | 43 | .. _`Python 3.10.x`: https://www.python.org/ 44 | .. _`NumPy`: https://numpy.org/ 45 | .. _`SciPy`: https://scipy.org/ 46 | .. _`Matplotlib`: https://matplotlib.org/ 47 | .. _`cvxpy`: https://www.cvxpy.org/ 48 | 49 | 50 | How to use 51 | ---------- 52 | 53 | 1. Clone this repo and go into dir. 54 | 55 | .. code-block:: 56 | 57 | >$ git clone https://github.com/AtsushiSakai/PythonRobotics.git 58 | 59 | >$ cd PythonRobotics 60 | 61 | 62 | 2. Install the required libraries. 63 | 64 | using conda : 65 | 66 | .. code-block:: 67 | 68 | >$ conda env create -f requirements/environment.yml 69 | 70 | using pip : 71 | 72 | .. code-block:: 73 | 74 | >$ pip install -r requirements/requirements.txt 75 | 76 | 77 | 3. Execute python script in each directory. 78 | 79 | 4. Add star to this repo if you like it 😃. 80 | 81 | -------------------------------------------------------------------------------- /docs/index_main.rst: -------------------------------------------------------------------------------- 1 | .. PythonRobotics documentation master file, created by 2 | sphinx-quickstart on Sat Sep 15 13:15:55 2018. 3 | You can adapt this file completely to your liking, but it should at least 4 | contain the root `toctree` directive. 5 | 6 | Welcome to PythonRobotics's documentation! 7 | ========================================== 8 | 9 | Python codes for robotics algorithm. The project is on `GitHub`_. 10 | 11 | This is a Python code collection of robotics algorithms. 12 | 13 | Features: 14 | 15 | 1. Easy to read for understanding each algorithm's basic idea. 16 | 17 | 2. Widely used and practical algorithms are selected. 18 | 19 | 3. Minimum dependency. 20 | 21 | See this paper for more details: 22 | 23 | - `[1808.10703] PythonRobotics: a Python code collection of robotics 24 | algorithms`_ (`BibTeX`_) 25 | 26 | 27 | .. _`[1808.10703] PythonRobotics: a Python code collection of robotics algorithms`: https://arxiv.org/abs/1808.10703 28 | .. _BibTeX: https://github.com/AtsushiSakai/PythonRoboticsPaper/blob/master/python_robotics.bib 29 | .. _GitHub: https://github.com/AtsushiSakai/PythonRobotics 30 | 31 | 32 | .. toctree:: 33 | :maxdepth: 2 34 | :caption: Contents 35 | 36 | getting_started 37 | modules/introduction 38 | modules/localization/localization 39 | modules/mapping/mapping 40 | modules/slam/slam 41 | modules/path_planning/path_planning 42 | modules/path_tracking/path_tracking 43 | modules/arm_navigation/arm_navigation 44 | modules/aerial_navigation/aerial_navigation 45 | modules/bipedal/bipedal 46 | modules/control/control 47 | modules/utils/utils 48 | modules/appendix/appendix 49 | how_to_contribute 50 | 51 | 52 | Indices and tables 53 | ================== 54 | 55 | * :ref:`genindex` 56 | * :ref:`modindex` 57 | * :ref:`search` 58 | -------------------------------------------------------------------------------- /docs/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | set SPHINXPROJ=PythonRobotics 13 | 14 | if "%1" == "" goto help 15 | 16 | %SPHINXBUILD% >NUL 2>NUL 17 | if errorlevel 9009 ( 18 | echo. 19 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 20 | echo.installed, then set the SPHINXBUILD environment variable to point 21 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 22 | echo.may add the Sphinx directory to PATH. 23 | echo. 24 | echo.If you don't have Sphinx installed, grab it from 25 | echo.http://sphinx-doc.org/ 26 | exit /b 1 27 | ) 28 | 29 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 30 | goto end 31 | 32 | :help 33 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 34 | 35 | :end 36 | popd 37 | -------------------------------------------------------------------------------- /docs/modules/aerial_navigation/aerial_navigation_main.rst: -------------------------------------------------------------------------------- 1 | .. _aerial_navigation: 2 | 3 | Aerial Navigation 4 | ================= 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | drone_3d_trajectory_following/drone_3d_trajectory_following 11 | rocket_powered_landing/rocket_powered_landing 12 | 13 | -------------------------------------------------------------------------------- /docs/modules/aerial_navigation/drone_3d_trajectory_following/drone_3d_trajectory_following_main.rst: -------------------------------------------------------------------------------- 1 | Drone 3d trajectory following 2 | ----------------------------- 3 | 4 | This is a 3d trajectory following simulation for a quadrotor. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif 7 | -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png -------------------------------------------------------------------------------- /docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/appendix/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png -------------------------------------------------------------------------------- /docs/modules/appendix/appendix_main.rst: -------------------------------------------------------------------------------- 1 | .. _appendix: 2 | 3 | Appendix 4 | ============== 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | Kalmanfilter_basics 11 | Kalmanfilter_basics_2 12 | 13 | -------------------------------------------------------------------------------- /docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png -------------------------------------------------------------------------------- /docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png -------------------------------------------------------------------------------- /docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png -------------------------------------------------------------------------------- /docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/arm_navigation/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png -------------------------------------------------------------------------------- /docs/modules/arm_navigation/arm_navigation_main.rst: -------------------------------------------------------------------------------- 1 | .. _arm_navigation: 2 | 3 | Arm Navigation 4 | ============== 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | planar_two_link_ik 11 | n_joint_arm_to_point_control 12 | obstacle_avoidance_arm_navigation 13 | -------------------------------------------------------------------------------- /docs/modules/arm_navigation/n_joint_arm_to_point_control_main.rst: -------------------------------------------------------------------------------- 1 | N joint arm to point control 2 | ---------------------------- 3 | 4 | N joint arm to a point control simulation. 5 | 6 | This is a interactive simulation. 7 | 8 | You can set the goal position of the end effector with left-click on the 9 | plotting area. 10 | 11 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif 12 | 13 | In this simulation N = 10, however, you can change it. 14 | -------------------------------------------------------------------------------- /docs/modules/arm_navigation/obstacle_avoidance_arm_navigation_main.rst: -------------------------------------------------------------------------------- 1 | Arm navigation with obstacle avoidance 2 | -------------------------------------- 3 | 4 | Arm navigation with obstacle avoidance simulation. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif -------------------------------------------------------------------------------- /docs/modules/bipedal/bipedal_main.rst: -------------------------------------------------------------------------------- 1 | .. _bipedal: 2 | 3 | Bipedal 4 | ================= 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | bipedal_planner/bipedal_planner 11 | 12 | -------------------------------------------------------------------------------- /docs/modules/bipedal/bipedal_planner/bipedal_planner_main.rst: -------------------------------------------------------------------------------- 1 | Bipedal Planner 2 | ----------------------------- 3 | 4 | Bipedal Walking with modifying designated footsteps 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Bipedal/bipedal_planner/animation.gif 7 | -------------------------------------------------------------------------------- /docs/modules/control/control_main.rst: -------------------------------------------------------------------------------- 1 | .. _control: 2 | 3 | Control 4 | ================= 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | inverted_pendulum_control/inverted_pendulum_control 11 | move_to_a_pose_control/move_to_a_pose_control 12 | 13 | -------------------------------------------------------------------------------- /docs/modules/control/inverted_pendulum_control/inverted-pendulum.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/control/inverted_pendulum_control/inverted-pendulum.png -------------------------------------------------------------------------------- /docs/modules/control/inverted_pendulum_control/inverted_pendulum_control_main.rst: -------------------------------------------------------------------------------- 1 | Inverted Pendulum Control 2 | ----------------------------- 3 | 4 | An inverted pendulum on a cart consists of a mass :math:`m` at the top of a pole of length :math:`l` pivoted on a 5 | horizontally moving base as shown in the adjacent. 6 | 7 | The objective of the control system is to balance the inverted pendulum by applying a force to the cart that the pendulum is attached to. 8 | 9 | Modeling 10 | ~~~~~~~~~~~~ 11 | 12 | .. image:: inverted-pendulum.png 13 | :align: center 14 | 15 | - :math:`M`: mass of the cart 16 | - :math:`m`: mass of the load on the top of the rod 17 | - :math:`l`: length of the rod 18 | - :math:`u`: force applied to the cart 19 | - :math:`x`: cart position coordinate 20 | - :math:`\theta`: pendulum angle from vertical 21 | 22 | Using Lagrange's equations: 23 | 24 | .. math:: 25 | & (M + m)\ddot{x} - ml\ddot{\theta}cos{\theta} + ml\dot{\theta^2}\sin{\theta} = u \\ 26 | & l\ddot{\theta} - g\sin{\theta} = \ddot{x}\cos{\theta} 27 | 28 | See this `link `__ for more details. 29 | 30 | So 31 | 32 | .. math:: 33 | & \ddot{x} = \frac{m(gcos{\theta} - \dot{\theta}^2l)sin{\theta} + u}{M + m - mcos^2{\theta}} \\ 34 | & \ddot{\theta} = \frac{g(M + m)sin{\theta} - \dot{\theta}^2lmsin{\theta}cos{\theta} + ucos{\theta}}{l(M + m - mcos^2{\theta})} 35 | 36 | 37 | Linearized model when :math:`\theta` small, :math:`cos{\theta} \approx 1`, :math:`sin{\theta} \approx \theta`, :math:`\dot{\theta}^2 \approx 0`. 38 | 39 | .. math:: 40 | & \ddot{x} = \frac{gm}{M}\theta + \frac{1}{M}u\\ 41 | & \ddot{\theta} = \frac{g(M + m)}{Ml}\theta + \frac{1}{Ml}u 42 | 43 | State space: 44 | 45 | .. math:: 46 | & \dot{x} = Ax + Bu \\ 47 | & y = Cx + Du 48 | 49 | where 50 | 51 | .. math:: 52 | & x = [x, \dot{x}, \theta,\dot{\theta}]\\ 53 | & A = \begin{bmatrix} 0 & 1 & 0 & 0\\0 & 0 & \frac{gm}{M} & 0\\0 & 0 & 0 & 1\\0 & 0 & \frac{g(M + m)}{Ml} & 0 \end{bmatrix}\\ 54 | & B = \begin{bmatrix} 0 \\ \frac{1}{M} \\ 0 \\ \frac{1}{Ml} \end{bmatrix} 55 | 56 | If control only \theta 57 | 58 | .. math:: 59 | & C = \begin{bmatrix} 0 & 0 & 1 & 0 \end{bmatrix}\\ 60 | & D = [0] 61 | 62 | If control x and \theta 63 | 64 | .. math:: 65 | & C = \begin{bmatrix} 1 & 0 & 0 & 0\\0 & 0 & 1 & 0 \end{bmatrix}\\ 66 | & D = \begin{bmatrix} 0 \\ 0 \end{bmatrix} 67 | 68 | LQR control 69 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 70 | 71 | The LQR controller minimize this cost function defined as: 72 | 73 | .. math:: J = x^T Q x + u^T R u 74 | 75 | the feedback control law that minimizes the value of the cost is: 76 | 77 | .. math:: u = -K x 78 | 79 | where: 80 | 81 | .. math:: K = (B^T P B + R)^{-1} B^T P A 82 | 83 | and :math:`P` is the unique positive definite solution to the discrete time 84 | `algebraic Riccati equation `__ (DARE): 85 | 86 | .. math:: P = A^T P A - A^T P B ( R + B^T P B )^{-1} B^T P A + Q 87 | 88 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation_lqr.gif 89 | 90 | MPC control 91 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 92 | The MPC controller minimize this cost function defined as: 93 | 94 | .. math:: J = x^T Q x + u^T R u 95 | 96 | subject to: 97 | 98 | - Linearized Inverted Pendulum model 99 | - Initial state 100 | 101 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Control/InvertedPendulumCart/animation.gif 102 | -------------------------------------------------------------------------------- /docs/modules/introduction_main.rst: -------------------------------------------------------------------------------- 1 | 2 | Introduction 3 | ============ 4 | 5 | TBD 6 | 7 | Definition Of Robotics 8 | ---------------------- 9 | 10 | TBD 11 | 12 | History Of Robotics 13 | ---------------------- 14 | 15 | TBD 16 | 17 | Application Of Robotics 18 | ------------------------ 19 | 20 | TBD 21 | 22 | Software for Robotics 23 | ---------------------- 24 | 25 | TBD 26 | 27 | Software for Robotics 28 | ---------------------- 29 | 30 | TBD 31 | 32 | Python for Robotics 33 | ---------------------- 34 | 35 | TBD 36 | 37 | Learning Robotics Algorithms 38 | ---------------------------- 39 | 40 | TBD 41 | 42 | 43 | -------------------------------------------------------------------------------- /docs/modules/localization/ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization_main.rst: -------------------------------------------------------------------------------- 1 | Ensamble Kalman Filter Localization 2 | ----------------------------------- 3 | 4 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/ensamble_kalman_filter/animation.gif 5 | 6 | This is a sensor fusion localization with Ensamble Kalman Filter(EnKF). 7 | 8 | -------------------------------------------------------------------------------- /docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/localization/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png -------------------------------------------------------------------------------- /docs/modules/localization/histogram_filter_localization/1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/localization/histogram_filter_localization/1.png -------------------------------------------------------------------------------- /docs/modules/localization/histogram_filter_localization/2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/localization/histogram_filter_localization/2.png -------------------------------------------------------------------------------- /docs/modules/localization/histogram_filter_localization/3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/localization/histogram_filter_localization/3.png -------------------------------------------------------------------------------- /docs/modules/localization/histogram_filter_localization/4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/localization/histogram_filter_localization/4.png -------------------------------------------------------------------------------- /docs/modules/localization/localization_main.rst: -------------------------------------------------------------------------------- 1 | .. _localization: 2 | 3 | Localization 4 | ============ 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | extended_kalman_filter_localization_files/extended_kalman_filter_localization 11 | ensamble_kalman_filter_localization_files/ensamble_kalman_filter_localization 12 | unscented_kalman_filter_localization/unscented_kalman_filter_localization 13 | histogram_filter_localization/histogram_filter_localization 14 | particle_filter_localization/particle_filter_localization 15 | 16 | 17 | -------------------------------------------------------------------------------- /docs/modules/localization/particle_filter_localization/particle_filter_localization_main.rst: -------------------------------------------------------------------------------- 1 | Particle filter localization 2 | ---------------------------- 3 | 4 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/particle_filter/animation.gif 5 | 6 | This is a sensor fusion localization with Particle Filter(PF). 7 | 8 | The blue line is true trajectory, the black line is dead reckoning 9 | trajectory, 10 | 11 | and the red line is estimated trajectory with PF. 12 | 13 | It is assumed that the robot can measure a distance from landmarks 14 | (RFID). 15 | 16 | This measurements are used for PF localization. 17 | 18 | How to calculate covariance matrix from particles 19 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 20 | 21 | The covariance matrix :math:`\Xi` from particle information is calculated by the following equation: 22 | 23 | .. math:: \Xi_{j,k}=\frac{1}{1-\sum^N_{i=1}(w^i)^2}\sum^N_{i=1}w^i(x^i_j-\mu_j)(x^i_k-\mu_k) 24 | 25 | - :math:`\Xi_{j,k}` is covariance matrix element at row :math:`i` and column :math:`k`. 26 | 27 | - :math:`w^i` is the weight of the :math:`i` th particle. 28 | 29 | - :math:`x^i_j` is the :math:`j` th state of the :math:`i` th particle. 30 | 31 | - :math:`\mu_j` is the :math:`j` th mean state of particles. 32 | 33 | References: 34 | ~~~~~~~~~~~ 35 | 36 | - `_PROBABILISTIC ROBOTICS: `_ 37 | - `Improving the particle filter in high dimensions using conjugate artificial process noise `_ 38 | -------------------------------------------------------------------------------- /docs/modules/localization/unscented_kalman_filter_localization/unscented_kalman_filter_localization_main.rst: -------------------------------------------------------------------------------- 1 | Unscented Kalman Filter localization 2 | ------------------------------------ 3 | 4 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/unscented_kalman_filter/animation.gif 5 | 6 | This is a sensor fusion localization with Unscented Kalman Filter(UKF). 7 | 8 | The lines and points are same meaning of the EKF simulation. 9 | 10 | References: 11 | ~~~~~~~~~~~ 12 | 13 | - `Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization `_ 14 | -------------------------------------------------------------------------------- /docs/modules/mapping/circle_fitting/circle_fitting_main.rst: -------------------------------------------------------------------------------- 1 | Object shape recognition using circle fitting 2 | --------------------------------------------- 3 | 4 | This is an object shape recognition using circle fitting. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/circle_fitting/animation.gif 7 | 8 | The blue circle is the true object shape. 9 | 10 | The red crosses are observations from a ranging sensor. 11 | 12 | The red circle is the estimated object shape using circle fitting. 13 | 14 | -------------------------------------------------------------------------------- /docs/modules/mapping/gaussian_grid_map/gaussian_grid_map_main.rst: -------------------------------------------------------------------------------- 1 | .. _gaussian_grid_map: 2 | 3 | Gaussian grid map 4 | ----------------- 5 | 6 | This is a 2D Gaussian grid mapping example. 7 | 8 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/gaussian_grid_map/animation.gif 9 | -------------------------------------------------------------------------------- /docs/modules/mapping/k_means_object_clustering/k_means_object_clustering_main.rst: -------------------------------------------------------------------------------- 1 | k-means object clustering 2 | ------------------------- 3 | 4 | This is a 2D object clustering with k-means algorithm. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/kmeans_clustering/animation.gif 7 | -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/grid_map_example.png -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_12_0.png -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_14_1.png -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_5_0.png -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_7_0.png -------------------------------------------------------------------------------- /docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/mapping/lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial_8_0.png -------------------------------------------------------------------------------- /docs/modules/mapping/mapping_main.rst: -------------------------------------------------------------------------------- 1 | .. _mapping: 2 | 3 | Mapping 4 | ======= 5 | .. toctree:: 6 | :maxdepth: 2 7 | :caption: Contents 8 | 9 | gaussian_grid_map/gaussian_grid_map 10 | ray_casting_grid_map/ray_casting_grid_map 11 | lidar_to_grid_map_tutorial/lidar_to_grid_map_tutorial 12 | k_means_object_clustering/k_means_object_clustering 13 | circle_fitting/circle_fitting 14 | rectangle_fitting/rectangle_fitting 15 | 16 | -------------------------------------------------------------------------------- /docs/modules/mapping/ray_casting_grid_map/ray_casting_grid_map_main.rst: -------------------------------------------------------------------------------- 1 | Ray casting grid map 2 | -------------------- 3 | 4 | This is a 2D ray casting grid mapping example. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif -------------------------------------------------------------------------------- /docs/modules/mapping/rectangle_fitting/rectangle_fitting_main.rst: -------------------------------------------------------------------------------- 1 | Object shape recognition using rectangle fitting 2 | ------------------------------------------------ 3 | 4 | This is an object shape recognition using rectangle fitting. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/rectangle_fitting/animation.gif 7 | 8 | -------------------------------------------------------------------------------- /docs/modules/path_planning/bezier_path/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bezier_path/Figure_1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bezier_path/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bezier_path/Figure_2.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bezier_path/bezier_path_main.rst: -------------------------------------------------------------------------------- 1 | Bezier path planning 2 | -------------------- 3 | 4 | A sample code of Bezier path planning. 5 | 6 | It is based on 4 control points Beizer path. 7 | 8 | .. image:: Figure_1.png 9 | 10 | If you change the offset distance from start and end point, 11 | 12 | You can get different Beizer course: 13 | 14 | .. image:: Figure_2.png 15 | 16 | Ref: 17 | 18 | - `Continuous Curvature Path Generation Based on Bezier Curves for 19 | Autonomous 20 | Vehicles `__ 21 | -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/approx_and_curvature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/approx_and_curvature.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/approximation1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/approximation1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/basis_functions.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/basis_functions.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/bspline_path_planning.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/bspline_path_planning.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/interp_and_curvature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/interp_and_curvature.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bspline_path/interpolation1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/bspline_path/interpolation1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/bugplanner/bugplanner_main.rst: -------------------------------------------------------------------------------- 1 | Bug planner 2 | ----------- 3 | 4 | This is a 2D planning with Bug algorithm. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BugPlanner/animation.gif 7 | 8 | - `ECE452 Bug Algorithms `_ 9 | -------------------------------------------------------------------------------- /docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_3.png -------------------------------------------------------------------------------- /docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_4.png -------------------------------------------------------------------------------- /docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/closed_loop_rrt_star_car/Figure_5.png -------------------------------------------------------------------------------- /docs/modules/path_planning/clothoid_path/clothoid_path_main.rst: -------------------------------------------------------------------------------- 1 | .. _clothoid-path-planning: 2 | 3 | Clothoid path planning 4 | -------------------------- 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation1.gif 7 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation2.gif 8 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClothoidPath/animation3.gif 9 | 10 | This is a clothoid path planning sample code. 11 | 12 | This can interpolate two 2D pose (x, y, yaw) with a clothoid path, 13 | which its curvature is linearly continuous. 14 | In other words, this is G1 Hermite interpolation with a single clothoid segment. 15 | 16 | This path planning algorithm as follows: 17 | 18 | Step1: Solve g function 19 | ~~~~~~~~~~~~~~~~~~~~~~~ 20 | 21 | Solve the g(A) function with a nonlinear optimization solver. 22 | 23 | .. math:: 24 | 25 | g(A):=Y(2A, \delta-A, \phi_{s}) 26 | 27 | Where 28 | 29 | * :math:`\delta`: the orientation difference between start and goal pose. 30 | * :math:`\phi_{s}`: the orientation of the start pose. 31 | * :math:`Y`: :math:`Y(a, b, c)=\int_{0}^{1} \sin \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau` 32 | 33 | 34 | Step2: Calculate path parameters 35 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 36 | 37 | We can calculate these path parameters using :math:`A`, 38 | 39 | :math:`L`: path length 40 | 41 | .. math:: 42 | 43 | L=\frac{R}{X\left(2 A, \delta-A, \phi_{s}\right)} 44 | 45 | where 46 | 47 | * :math:`R`: the distance between start and goal pose 48 | * :math:`X`: :math:`X(a, b, c)=\int_{0}^{1} \cos \left(\frac{a}{2} \tau^{2}+b \tau+c\right) d \tau` 49 | 50 | 51 | - :math:`\kappa`: curvature 52 | 53 | .. math:: 54 | 55 | \kappa=(\delta-A) / L 56 | 57 | 58 | - :math:`\kappa'`: curvature rate 59 | 60 | .. math:: 61 | 62 | \kappa^{\prime}=2 A / L^{2} 63 | 64 | 65 | Step3: Construct a path with Fresnel integral 66 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 67 | 68 | The final clothoid path can be calculated with the path parameters and Fresnel integrals. 69 | 70 | .. math:: 71 | \begin{aligned} 72 | &x(s)=x_{0}+\int_{0}^{s} \cos \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau \\ 73 | &y(s)=y_{0}+\int_{0}^{s} \sin \left(\frac{1}{2} \kappa^{\prime} \tau^{2}+\kappa \tau+\vartheta_{0}\right) \mathrm{d} \tau 74 | \end{aligned} 75 | 76 | 77 | References 78 | ~~~~~~~~~~ 79 | 80 | - `Fast and accurate G1 fitting of clothoid curves `__ 81 | -------------------------------------------------------------------------------- /docs/modules/path_planning/coverage_path/coverage_path_main.rst: -------------------------------------------------------------------------------- 1 | Coverage path planner 2 | --------------------- 3 | 4 | Grid based sweep 5 | ~~~~~~~~~~~~~~~~ 6 | 7 | This is a 2D grid based sweep coverage path planner simulation: 8 | 9 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/GridBasedSweepCPP/animation.gif 10 | 11 | Spiral Spanning Tree 12 | ~~~~~~~~~~~~~~~~~~~~ 13 | 14 | This is a 2D grid based spiral spanning tree coverage path planner simulation: 15 | 16 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation1.gif 17 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation2.gif 18 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/SpiralSpanningTreeCPP/animation3.gif 19 | 20 | - `Spiral-STC: An On-Line Coverage Algorithm of Grid Environments by a Mobile Robot `_ 21 | 22 | 23 | Wavefront path 24 | ~~~~~~~~~~~~~~ 25 | 26 | This is a 2D grid based wavefront coverage path planner simulation: 27 | 28 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation1.gif 29 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation2.gif 30 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/WavefrontCPP/animation3.gif 31 | 32 | - `Planning paths of complete coverage of an unstructured environment by a mobile robot `_ 33 | 34 | 35 | -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/cubic_spline_1d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/cubic_spline_1d.png -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/cubic_spline_2d_curvature.png -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/cubic_spline_2d_path.png -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/cubic_spline_2d_yaw.png -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/spline.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/spline.png -------------------------------------------------------------------------------- /docs/modules/path_planning/cubic_spline/spline_continuity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/cubic_spline/spline_continuity.png -------------------------------------------------------------------------------- /docs/modules/path_planning/dubins_path/RLR.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/dubins_path/RLR.jpg -------------------------------------------------------------------------------- /docs/modules/path_planning/dubins_path/RSR.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/dubins_path/RSR.jpg -------------------------------------------------------------------------------- /docs/modules/path_planning/dubins_path/dubins_path.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/dubins_path/dubins_path.jpg -------------------------------------------------------------------------------- /docs/modules/path_planning/dubins_path/dubins_path_main.rst: -------------------------------------------------------------------------------- 1 | Dubins path planning 2 | -------------------- 3 | 4 | A sample code for Dubins path planning. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DubinsPath/animation.gif?raw=True 7 | 8 | Dubins path 9 | ~~~~~~~~~~~~ 10 | Dubins path is a analytical path planning algorithm for a simple car model. 11 | 12 | It can generates a shortest path between two 2D poses (x, y, yaw) with maximum curvature constraint and tangent(yaw angle) constraint. 13 | 14 | Generated paths consist of 3 segments of maximum curvature curves or a straight line segment. 15 | 16 | Each segment type can is categorized by 3 type: 'Right turn (R)' , 'Left turn (L)', and 'Straight (S).' 17 | 18 | Possible path will be at least one of these six types: RSR, RSL, LSR, LSL, RLR, LRL. 19 | 20 | Dubins path planner can output each segment type and distance of each course segment. 21 | 22 | For example, a RSR Dubins path is: 23 | 24 | .. image:: RSR.jpg 25 | :width: 400px 26 | 27 | Each segment distance can be calculated by: 28 | 29 | :math:`\alpha = mod(-\theta)` 30 | 31 | :math:`\beta = mod(x_{e, yaw} - \theta)` 32 | 33 | :math:`p^2 = 2 + d ^ 2 - 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)` 34 | 35 | :math:`t = atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta)` 36 | 37 | :math:`d_1 = mod(-\alpha + t)` 38 | 39 | :math:`d_2 = p` 40 | 41 | :math:`d_3 = mod(\beta - t)` 42 | 43 | where :math:`\theta` is tangent and d is distance from :math:`x_s` to :math:`x_e` 44 | 45 | A RLR Dubins path is: 46 | 47 | .. image:: RLR.jpg 48 | :width: 200px 49 | 50 | Each segment distance can be calculated by: 51 | 52 | :math:`t = (6.0 - d^2 + 2\cos(\alpha-\beta) + 2d(\sin\alpha - \sin\beta)) / 8.0` 53 | 54 | :math:`d_2 = mod(2\pi - acos(t))` 55 | 56 | :math:`d_1 = mod(\alpha - atan2(\cos\beta - \cos\alpha, d + \sin\alpha - \sin\beta) + d_2 / 2.0)` 57 | 58 | :math:`d_3 = mod(\alpha - \beta - d_1 + d_2)` 59 | 60 | You can generate a path from these information and the maximum curvature information. 61 | 62 | A path type which has minimum course length among 6 types is selected, 63 | and then a path is constructed based on the selected type and its distances. 64 | 65 | API 66 | ~~~~~~~~~~~~~~~~~~~~ 67 | 68 | .. autofunction:: PathPlanning.DubinsPath.dubins_path_planner.plan_dubins_path 69 | 70 | 71 | Reference 72 | ~~~~~~~~~~~~~~~~~~~~ 73 | - `On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents `__ 74 | - `Dubins path - Wikipedia `__ 75 | - `15.3.1 Dubins Curves `__ 76 | - `A Comprehensive, Step-by-Step Tutorial to Computing Dubin’s Paths `__ 77 | -------------------------------------------------------------------------------- /docs/modules/path_planning/dynamic_window_approach/dynamic_window_approach_main.rst: -------------------------------------------------------------------------------- 1 | .. _dynamic_window_approach: 2 | 3 | Dynamic Window Approach 4 | ----------------------- 5 | 6 | This is a 2D navigation sample code with Dynamic Window Approach. 7 | 8 | - `The Dynamic Window Approach to Collision 9 | Avoidance `__ 10 | 11 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DynamicWindowApproach/animation.gif 12 | -------------------------------------------------------------------------------- /docs/modules/path_planning/eta3_spline/eta3_spline_main.rst: -------------------------------------------------------------------------------- 1 | .. _eta^3-spline-path-planning: 2 | 3 | Eta^3 Spline path planning 4 | -------------------------- 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Eta3SplinePath/animation.gif 7 | 8 | This is a path planning with Eta^3 spline. 9 | 10 | Ref: 11 | 12 | - `\\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile 13 | Robots `__ 14 | -------------------------------------------------------------------------------- /docs/modules/path_planning/frenet_frame_path/frenet_frame_path_main.rst: -------------------------------------------------------------------------------- 1 | Optimal Trajectory in a Frenet Frame 2 | ------------------------------------ 3 | 4 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/FrenetOptimalTrajectory/animation.gif 5 | 6 | This is optimal trajectory generation in a Frenet Frame. 7 | 8 | The cyan line is the target course and black crosses are obstacles. 9 | 10 | The red line is predicted path. 11 | 12 | Ref: 13 | 14 | - `Optimal Trajectory Generation for Dynamic Street Scenarios in a 15 | Frenet 16 | Frame `__ 17 | 18 | - `Optimal trajectory generation for dynamic street scenarios in a 19 | Frenet Frame `__ 20 | 21 | -------------------------------------------------------------------------------- /docs/modules/path_planning/grid_base_search/grid_base_search_main.rst: -------------------------------------------------------------------------------- 1 | Grid based search 2 | ----------------- 3 | 4 | Breadth First Search 5 | ~~~~~~~~~~~~~~~~~~~~ 6 | 7 | This is a 2D grid based path planning with Breadth first search algorithm. 8 | 9 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BreadthFirstSearch/animation.gif 10 | 11 | In the animation, cyan points are searched nodes. 12 | 13 | Depth First Search 14 | ~~~~~~~~~~~~~~~~~~~~ 15 | 16 | This is a 2D grid based path planning with Depth first search algorithm. 17 | 18 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DepthFirstSearch/animation.gif 19 | 20 | In the animation, cyan points are searched nodes. 21 | 22 | .. _dijkstra: 23 | 24 | Dijkstra algorithm 25 | ~~~~~~~~~~~~~~~~~~ 26 | 27 | This is a 2D grid based shortest path planning with Dijkstra's algorithm. 28 | 29 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/Dijkstra/animation.gif 30 | 31 | In the animation, cyan points are searched nodes. 32 | 33 | .. _a*-algorithm: 34 | 35 | A\* algorithm 36 | ~~~~~~~~~~~~~ 37 | 38 | This is a 2D grid based shortest path planning with A star algorithm. 39 | 40 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/AStar/animation.gif 41 | 42 | In the animation, cyan points are searched nodes. 43 | 44 | Its heuristic is 2D Euclid distance. 45 | 46 | Bidirectional A\* algorithm 47 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 48 | 49 | This is a 2D grid based shortest path planning with bidirectional A star algorithm. 50 | 51 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BidirectionalAStar/animation.gif 52 | 53 | In the animation, cyan points are searched nodes. 54 | 55 | .. _D*-algorithm: 56 | 57 | D\* algorithm 58 | ~~~~~~~~~~~~~ 59 | 60 | This is a 2D grid based shortest path planning with D star algorithm. 61 | 62 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStar/animation.gif 63 | 64 | The animation shows a robot finding its path avoiding an obstacle using the D* search algorithm. 65 | 66 | Ref: 67 | 68 | - `D* search Wikipedia `__ 69 | 70 | D\* lite algorithm 71 | ~~~~~~~~~~~~~~~~~~ 72 | 73 | This is a 2D grid based path planning and replanning with D star lite algorithm. 74 | 75 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/DStarLite/animation.gif 76 | 77 | Ref: 78 | 79 | - `Improved Fast Replanning for Robot Navigation in Unknown Terrain `_ 80 | 81 | 82 | Potential Field algorithm 83 | ~~~~~~~~~~~~~~~~~~~~~~~~~ 84 | 85 | This is a 2D grid based path planning with Potential Field algorithm. 86 | 87 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/PotentialFieldPlanning/animation.gif 88 | 89 | In the animation, the blue heat map shows potential value on each grid. 90 | 91 | Ref: 92 | 93 | - `Robotic Motion Planning:Potential 94 | Functions `__ 95 | 96 | -------------------------------------------------------------------------------- /docs/modules/path_planning/hybridastar/hybridastar_main.rst: -------------------------------------------------------------------------------- 1 | Hybrid a star 2 | --------------------- 3 | 4 | This is a simple vehicle model based hybrid A\* path planner. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/HybridAStar/animation.gif 7 | -------------------------------------------------------------------------------- /docs/modules/path_planning/lqr_path/lqr_path_main.rst: -------------------------------------------------------------------------------- 1 | LQR based path planning 2 | ----------------------- 3 | 4 | A sample code using LQR based path planning for double integrator model. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRPlanner/animation.gif?raw=true 7 | -------------------------------------------------------------------------------- /docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/model_predictive_trajectory_generator/lookup_table.png -------------------------------------------------------------------------------- /docs/modules/path_planning/model_predictive_trajectory_generator/model_predictive_trajectory_generator_main.rst: -------------------------------------------------------------------------------- 1 | Model Predictive Trajectory Generator 2 | ------------------------------------- 3 | 4 | This is a path optimization sample on model predictive trajectory 5 | generator. 6 | 7 | This algorithm is used for state lattice planner. 8 | 9 | Path optimization sample 10 | ~~~~~~~~~~~~~~~~~~~~~~~~ 11 | 12 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ModelPredictiveTrajectoryGenerator/kn05animation.gif 13 | 14 | Lookup table generation sample 15 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ 16 | 17 | .. image:: lookup_table.png 18 | 19 | Ref: 20 | 21 | - `Optimal rough terrain trajectory generation for wheeled mobile 22 | robots `__ 23 | -------------------------------------------------------------------------------- /docs/modules/path_planning/path_planning_main.rst: -------------------------------------------------------------------------------- 1 | .. _path_planning: 2 | 3 | Path Planning 4 | ============= 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | dynamic_window_approach/dynamic_window_approach 11 | bugplanner/bugplanner 12 | grid_base_search/grid_base_search 13 | model_predictive_trajectory_generator/model_predictive_trajectory_generator 14 | state_lattice_planner/state_lattice_planner 15 | prm_planner/prm_planner 16 | visibility_road_map_planner/visibility_road_map_planner 17 | vrm_planner/vrm_planner 18 | rrt/rrt 19 | cubic_spline/cubic_spline 20 | bspline_path/bspline_path 21 | clothoid_path/clothoid_path 22 | eta3_spline/eta3_spline 23 | bezier_path/bezier_path 24 | quintic_polynomials_planner/quintic_polynomials_planner 25 | dubins_path/dubins_path 26 | reeds_shepp_path/reeds_shepp_path 27 | lqr_path/lqr_path 28 | hybridastar/hybridastar 29 | frenet_frame_path/frenet_frame_path 30 | coverage_path/coverage_path 31 | -------------------------------------------------------------------------------- /docs/modules/path_planning/prm_planner/prm_planner_main.rst: -------------------------------------------------------------------------------- 1 | .. _probabilistic-road-map-(prm)-planning: 2 | 3 | Probabilistic Road-Map (PRM) planning 4 | ------------------------------------- 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ProbabilisticRoadMap/animation.gif 7 | 8 | This PRM planner uses Dijkstra method for graph search. 9 | 10 | In the animation, blue points are sampled points, 11 | 12 | Cyan crosses means searched points with Dijkstra method, 13 | 14 | The red line is the final path of PRM. 15 | 16 | Ref: 17 | 18 | - `Probabilistic roadmap - 19 | Wikipedia `__ 20 | -------------------------------------------------------------------------------- /docs/modules/path_planning/reeds_shepp_path/reeds_shepp_path_main.rst: -------------------------------------------------------------------------------- 1 | Reeds Shepp planning 2 | -------------------- 3 | 4 | A sample code with Reeds Shepp path planning. 5 | 6 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ReedsSheppPath/animation.gif?raw=true 7 | 8 | Ref: 9 | 10 | - `15.3.2 Reeds-Shepp 11 | Curves `__ 12 | 13 | - `optimal paths for a car that goes both forwards and 14 | backwards `__ 15 | 16 | - `ghliu/pyReedsShepp: Implementation of Reeds Shepp 17 | curve. `__ 18 | -------------------------------------------------------------------------------- /docs/modules/path_planning/rrt/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/rrt/figure_1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/rrt/rrt_main.rst: -------------------------------------------------------------------------------- 1 | .. _rapidly-exploring-random-trees-(rrt): 2 | 3 | Rapidly-Exploring Random Trees (RRT) 4 | ------------------------------------ 5 | 6 | Basic RRT 7 | ~~~~~~~~~ 8 | 9 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRT/animation.gif 10 | 11 | This is a simple path planning code with Rapidly-Exploring Random Trees 12 | (RRT) 13 | 14 | Black circles are obstacles, green line is a searched tree, red crosses 15 | are start and goal positions. 16 | 17 | .. include:: rrt_star.rst 18 | 19 | 20 | RRT with dubins path 21 | ~~~~~~~~~~~~~~~~~~~~ 22 | 23 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTDubins/animation.gif 24 | 25 | Path planning for a car robot with RRT and dubins path planner. 26 | 27 | .. _rrt*-with-dubins-path: 28 | 29 | RRT\* with dubins path 30 | ~~~~~~~~~~~~~~~~~~~~~~ 31 | 32 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarDubins/animation.gif 33 | 34 | Path planning for a car robot with RRT\* and dubins path planner. 35 | 36 | .. _rrt*-with-reeds-sheep-path: 37 | 38 | RRT\* with reeds-sheep path 39 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 40 | 41 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTStarReedsShepp/animation.gif 42 | 43 | Path planning for a car robot with RRT\* and reeds sheep path planner. 44 | 45 | .. _informed-rrt*: 46 | 47 | Informed RRT\* 48 | ~~~~~~~~~~~~~~ 49 | 50 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/InformedRRTStar/animation.gif 51 | 52 | This is a path planning code with Informed RRT*. 53 | 54 | The cyan ellipse is the heuristic sampling domain of Informed RRT*. 55 | 56 | Ref: 57 | 58 | - `Informed RRT\*: Optimal Sampling-based Path Planning Focused via 59 | Direct Sampling of an Admissible Ellipsoidal 60 | Heuristic `__ 61 | 62 | .. _batch-informed-rrt*: 63 | 64 | Batch Informed RRT\* 65 | ~~~~~~~~~~~~~~~~~~~~ 66 | 67 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/BatchInformedRRTStar/animation.gif 68 | 69 | This is a path planning code with Batch Informed RRT*. 70 | 71 | Ref: 72 | 73 | - `Batch Informed Trees (BIT*): Sampling-based Optimal Planning via the 74 | Heuristically Guided Search of Implicit Random Geometric 75 | Graphs `__ 76 | 77 | .. _closed-loop-rrt*: 78 | 79 | Closed Loop RRT\* 80 | ~~~~~~~~~~~~~~~~~ 81 | 82 | A vehicle model based path planning with closed loop RRT*. 83 | 84 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/ClosedLoopRRTStar/animation.gif 85 | 86 | In this code, pure-pursuit algorithm is used for steering control, 87 | 88 | PID is used for speed control. 89 | 90 | Ref: 91 | 92 | - `Motion Planning in Complex Environments using Closed-loop 93 | Prediction `__ 94 | 95 | - `Real-time Motion Planning with Applications to Autonomous Urban 96 | Driving `__ 97 | 98 | - `[1601.06326] Sampling-based Algorithms for Optimal Motion Planning 99 | Using Closed-loop Prediction `__ 100 | 101 | .. _lqr-rrt*: 102 | 103 | LQR-RRT\* 104 | ~~~~~~~~~ 105 | 106 | This is a path planning simulation with LQR-RRT*. 107 | 108 | A double integrator motion model is used for LQR local planner. 109 | 110 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/LQRRRTStar/animation.gif 111 | 112 | Ref: 113 | 114 | - `LQR-RRT\*: Optimal Sampling-Based Motion Planning with Automatically 115 | Derived Extension 116 | Heuristics `__ 117 | 118 | - `MahanFathi/LQR-RRTstar: LQR-RRT\* method is used for random motion planning of a simple pendulum in its phase plot `__ -------------------------------------------------------------------------------- /docs/modules/path_planning/rrt/rrt_star.rst: -------------------------------------------------------------------------------- 1 | RRT\* 2 | ~~~~~ 3 | 4 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTstar/animation.gif 5 | 6 | This is a path planning code with RRT\* 7 | 8 | Black circles are obstacles, green line is a searched tree, red crosses are start and goal positions. 9 | 10 | Simulation 11 | ^^^^^^^^^^ 12 | 13 | .. image:: rrt_star/rrt_star_1_0.png 14 | :width: 600px 15 | 16 | 17 | Ref 18 | ^^^ 19 | - `Sampling-based Algorithms for Optimal Motion Planning `__ 20 | - `Incremental Sampling-based Algorithms for Optimal Motion Planning `__ 21 | 22 | -------------------------------------------------------------------------------- /docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/rrt/rrt_star/rrt_star_1_0.png -------------------------------------------------------------------------------- /docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/rrt/rrt_star_reeds_shepp/figure_1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_2.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_3.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_4.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_5.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/Figure_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/state_lattice_planner/Figure_6.png -------------------------------------------------------------------------------- /docs/modules/path_planning/state_lattice_planner/state_lattice_planner_main.rst: -------------------------------------------------------------------------------- 1 | State Lattice Planning 2 | ---------------------- 3 | 4 | This script is a path planning code with state lattice planning. 5 | 6 | This code uses the model predictive trajectory generator to solve 7 | boundary problem. 8 | 9 | 10 | Uniform polar sampling 11 | ~~~~~~~~~~~~~~~~~~~~~~ 12 | 13 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/UniformPolarSampling.gif 14 | 15 | Biased polar sampling 16 | ~~~~~~~~~~~~~~~~~~~~~ 17 | 18 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/BiasedPolarSampling.gif 19 | 20 | Lane sampling 21 | ~~~~~~~~~~~~~ 22 | 23 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/StateLatticePlanner/LaneSampling.gif 24 | 25 | Ref: 26 | 27 | - `Optimal rough terrain trajectory generation for wheeled mobile 28 | robots `__ 29 | 30 | - `State Space Sampling of Feasible Motions for High-Performance Mobile 31 | Robot Navigation in Complex 32 | Environments `__ 33 | 34 | -------------------------------------------------------------------------------- /docs/modules/path_planning/visibility_road_map_planner/step0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/visibility_road_map_planner/step0.png -------------------------------------------------------------------------------- /docs/modules/path_planning/visibility_road_map_planner/step1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/visibility_road_map_planner/step1.png -------------------------------------------------------------------------------- /docs/modules/path_planning/visibility_road_map_planner/step2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/visibility_road_map_planner/step2.png -------------------------------------------------------------------------------- /docs/modules/path_planning/visibility_road_map_planner/step3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_planning/visibility_road_map_planner/step3.png -------------------------------------------------------------------------------- /docs/modules/path_planning/visibility_road_map_planner/visibility_road_map_planner_main.rst: -------------------------------------------------------------------------------- 1 | Visibility Road-Map planner 2 | --------------------------- 3 | 4 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/VisibilityRoadMap/animation.gif 5 | 6 | `[Code] `_ 7 | 8 | This visibility road-map planner uses Dijkstra method for graph search. 9 | 10 | In the animation, the black lines are polygon obstacles, 11 | 12 | red crosses are visibility nodes, and blue lines area collision free visibility graphs. 13 | 14 | The red line is the final path searched by dijkstra algorithm frm the visibility graphs. 15 | 16 | Algorithms 17 | ~~~~~~~~~~ 18 | 19 | In this chapter, how does the visibility road map planner search a path. 20 | 21 | We assume this planner can be provided these information in the below figure. 22 | 23 | - 1. Start point (Red point) 24 | - 2. Goal point (Blue point) 25 | - 3. Obstacle polygons (Black lines) 26 | 27 | .. image:: step0.png 28 | :width: 400px 29 | 30 | 31 | Step1: Generate visibility nodes based on polygon obstacles 32 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 33 | 34 | The nodes are generated by expanded these polygons vertexes like the below figure: 35 | 36 | .. image:: step1.png 37 | :width: 400px 38 | 39 | Each polygon vertex is expanded outward from the vector of adjacent vertices. 40 | 41 | The start and goal point are included as nodes as well. 42 | 43 | Step2: Generate visibility graphs connecting the nodes. 44 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 45 | 46 | When connecting the nodes, the arc between two nodes is checked to collided or not to each obstacles. 47 | 48 | If the arc is collided, the graph is removed. 49 | 50 | The blue lines are generated visibility graphs in the figure: 51 | 52 | .. image:: step2.png 53 | :width: 400px 54 | 55 | 56 | Step3: Search the shortest path in the graphs using Dijkstra algorithm 57 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 58 | 59 | The red line is searched path in the figure: 60 | 61 | .. image:: step3.png 62 | :width: 400px 63 | 64 | You can find the details of Dijkstra algorithm in :ref:`dijkstra`. 65 | 66 | References 67 | ^^^^^^^^^^ 68 | 69 | - `Visibility graph - Wikipedia `_ 70 | 71 | 72 | -------------------------------------------------------------------------------- /docs/modules/path_planning/vrm_planner/vrm_planner_main.rst: -------------------------------------------------------------------------------- 1 | Voronoi Road-Map planning 2 | ------------------------- 3 | 4 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/VoronoiRoadMap/animation.gif 5 | 6 | This Voronoi road-map planner uses Dijkstra method for graph search. 7 | 8 | In the animation, blue points are Voronoi points, 9 | 10 | Cyan crosses mean searched points with Dijkstra method, 11 | 12 | The red line is the final path of Vornoi Road-Map. 13 | 14 | Ref: 15 | 16 | - `Robotic Motion Planning `__ 17 | 18 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_1_0.png -------------------------------------------------------------------------------- /docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_2_0.png -------------------------------------------------------------------------------- /docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_3_0.png -------------------------------------------------------------------------------- /docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_4_0.png -------------------------------------------------------------------------------- /docs/modules/path_tracking/cgmres_nmpc/cgmres_nmpc_main.rst: -------------------------------------------------------------------------------- 1 | 2 | Nonlinear Model Predictive Control with C-GMRES 3 | ----------------------------------------------- 4 | 5 | .. image:: cgmres_nmpc_1_0.png 6 | :width: 600px 7 | 8 | .. image:: cgmres_nmpc_2_0.png 9 | :width: 600px 10 | 11 | .. image:: cgmres_nmpc_3_0.png 12 | :width: 600px 13 | 14 | .. image:: cgmres_nmpc_4_0.png 15 | :width: 600px 16 | 17 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif 18 | :alt: gif 19 | 20 | Mathematical Formulation 21 | ~~~~~~~~~~~~~~~~~~~~~~~~ 22 | 23 | Motion model is 24 | 25 | .. math:: \dot{x}=vcos\theta 26 | 27 | .. math:: \dot{y}=vsin\theta 28 | 29 | .. math:: \dot{\theta}=\frac{v}{WB}sin(u_{\delta}) 30 | 31 | \ (tan is not good for optimization) 32 | 33 | .. math:: \dot{v}=u_a 34 | 35 | Cost function is 36 | 37 | .. math:: J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta 38 | 39 | Input constraints are 40 | 41 | .. math:: |u_a| \leq u_{a,max} 42 | 43 | .. math:: |u_\delta| \leq u_{\delta,max} 44 | 45 | So, Hamiltonian　is 46 | 47 | .. math:: 48 | 49 | J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta\\ +\lambda_1vcos\theta+\lambda_2vsin\theta+\lambda_3\frac{v}{WB}sin(u_{\delta})+\lambda_4u_a\\ 50 | +\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\rho_2(u_\delta^2+d_\delta^2+u_{\delta,max}^2) 51 | 52 | Partial differential equations of the Hamiltonian are: 53 | 54 | :math:`\begin{equation*} \frac{\partial H}{\partial \bf{x}}=\\ \begin{bmatrix} \frac{\partial H}{\partial x}= 0&\\ \frac{\partial H}{\partial y}= 0&\\ \frac{\partial H}{\partial \theta}= -\lambda_1vsin\theta+\lambda_2vcos\theta&\\ \frac{\partial H}{\partial v}=-\lambda_1cos\theta+\lambda_2sin\theta+\lambda_3\frac{1}{WB}sin(u_{\delta})&\\ \end{bmatrix} \\ \end{equation*}` 55 | 56 | :math:`\begin{equation*} \frac{\partial H}{\partial \bf{u}}=\\ \begin{bmatrix} \frac{\partial H}{\partial u_a}= u_a+\lambda_4+2\rho_1u_a&\\ \frac{\partial H}{\partial u_\delta}= u_\delta+\lambda_3\frac{v}{WB}cos(u_{\delta})+2\rho_2u_\delta&\\ \frac{\partial H}{\partial d_a}= -\phi_a+2\rho_1d_a&\\ \frac{\partial H}{\partial d_\delta}=-\phi_\delta+2\rho_2d_\delta&\\ \end{bmatrix} \\ \end{equation*}` 57 | 58 | Ref 59 | ~~~ 60 | 61 | - `Shunichi09/nonlinear_control: Implementing the nonlinear model 62 | predictive control, sliding mode 63 | control `__ 64 | 65 | - `非線形モデル予測制御におけるCGMRES法をpythonで実装する - 66 | Qiita `__ 67 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/lqr_speed_and_steering_control/lqr_speed_and_steering_control_main.rst: -------------------------------------------------------------------------------- 1 | .. _linearquadratic-regulator-(lqr)-speed-and-steering-control: 2 | 3 | Linear–quadratic regulator (LQR) speed and steering control 4 | ----------------------------------------------------------- 5 | 6 | Path tracking simulation with LQR speed and steering control. 7 | 8 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif 9 | 10 | References: 11 | ~~~~~~~~~~~ 12 | 13 | - `Towards fully autonomous driving: Systems and algorithms `__ 14 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/lqr_steering_control/lqr_steering_control_main.rst: -------------------------------------------------------------------------------- 1 | .. _linearquadratic-regulator-(lqr)-steering-control: 2 | 3 | Linear–quadratic regulator (LQR) steering control 4 | ------------------------------------------------- 5 | 6 | Path tracking simulation with LQR steering control and PID speed 7 | control. 8 | 9 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif 10 | 11 | References: 12 | ~~~~~~~~~~~ 13 | - `ApolloAuto/apollo: An open autonomous driving platform `_ 14 | 15 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/path_tracking_main.rst: -------------------------------------------------------------------------------- 1 | .. _path_tracking: 2 | 3 | Path Tracking 4 | ============= 5 | 6 | .. toctree:: 7 | :maxdepth: 2 8 | :caption: Contents 9 | 10 | pure_pursuit_tracking/pure_pursuit_tracking 11 | stanley_control/stanley_control 12 | rear_wheel_feedback_control/rear_wheel_feedback_control 13 | lqr_steering_control/lqr_steering_control 14 | lqr_speed_and_steering_control/lqr_speed_and_steering_control 15 | model_predictive_speed_and_steering_control/model_predictive_speed_and_steering_control 16 | cgmres_nmpc/cgmres_nmpc 17 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/pure_pursuit_tracking/pure_pursuit_tracking_main.rst: -------------------------------------------------------------------------------- 1 | Pure pursuit tracking 2 | --------------------- 3 | 4 | Path tracking simulation with pure pursuit steering control and PID 5 | speed control. 6 | 7 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/pure_pursuit/animation.gif 8 | 9 | The red line is a target course, the green cross means the target point 10 | for pure pursuit control, the blue line is the tracking. 11 | 12 | References: 13 | ~~~~~~~~~~~ 14 | 15 | - `A Survey of Motion Planning and Control Techniques for Self-driving 16 | Urban Vehicles `_ 17 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/rear_wheel_feedback_control/rear_wheel_feedback_control_main.rst: -------------------------------------------------------------------------------- 1 | Rear wheel feedback control 2 | --------------------------- 3 | 4 | Path tracking simulation with rear wheel feedback steering control and 5 | PID speed control. 6 | 7 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif 8 | 9 | References: 10 | ~~~~~~~~~~~ 11 | - `A Survey of Motion Planning and Control Techniques for Self-driving 12 | Urban Vehicles `__ 13 | -------------------------------------------------------------------------------- /docs/modules/path_tracking/stanley_control/stanley_control_main.rst: -------------------------------------------------------------------------------- 1 | Stanley control 2 | --------------- 3 | 4 | Path tracking simulation with Stanley steering control and PID speed 5 | control. 6 | 7 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif 8 | 9 | References: 10 | ~~~~~~~~~~~ 11 | 12 | - `Stanley: The robot that won the DARPA grand 13 | challenge `_ 14 | 15 | - `Automatic Steering Methods for Autonomous Automobile Path 16 | Tracking `_ 17 | -------------------------------------------------------------------------------- /docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/FastSLAM1/FastSLAM1_12_0.png -------------------------------------------------------------------------------- /docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/FastSLAM1/FastSLAM1_12_1.png -------------------------------------------------------------------------------- /docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/FastSLAM1/FastSLAM1_1_0.png -------------------------------------------------------------------------------- /docs/modules/slam/FastSLAM2/FastSLAM2_main.rst: -------------------------------------------------------------------------------- 1 | FastSLAM 2.0 2 | ------------ 3 | 4 | This is a feature based SLAM example using FastSLAM 2.0. 5 | 6 | The animation has the same meanings as one of FastSLAM 1.0. 7 | 8 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif 9 | 10 | References 11 | ~~~~~~~~~~ 12 | 13 | - `PROBABILISTIC ROBOTICS `_ 14 | 15 | - `SLAM simulations by Tim Bailey `_ 16 | 17 | -------------------------------------------------------------------------------- /docs/modules/slam/ekf_slam/ekf_slam_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/ekf_slam/ekf_slam_1_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/Graph_SLAM_optimization.gif -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_13_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_15_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_16_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_4_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_8_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_SE2_example_files/graphSLAM_SE2_example_9_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_1.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_11_2.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_2_2.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_4_0.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/slam/graph_slam/graphSLAM_doc_files/graphSLAM_doc_9_1.png -------------------------------------------------------------------------------- /docs/modules/slam/graph_slam/graph_slam_main.rst: -------------------------------------------------------------------------------- 1 | Graph based SLAM 2 | ---------------- 3 | 4 | This is a graph based SLAM example. 5 | 6 | The blue line is ground truth. 7 | 8 | The black line is dead reckoning. 9 | 10 | The red line is the estimated trajectory with Graph based SLAM. 11 | 12 | The black stars are landmarks for graph edge generation. 13 | 14 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif 15 | 16 | .. include:: graphSLAM_doc.rst 17 | .. include:: graphSLAM_formulation.rst 18 | .. include:: graphSLAM_SE2_example.rst 19 | 20 | References: 21 | ~~~~~~~~~~~ 22 | 23 | - `A Tutorial on Graph-Based SLAM `_ 24 | 25 | -------------------------------------------------------------------------------- /docs/modules/slam/iterative_closest_point_matching/iterative_closest_point_matching_main.rst: -------------------------------------------------------------------------------- 1 | .. _iterative-closest-point-(icp)-matching: 2 | 3 | Iterative Closest Point (ICP) Matching 4 | -------------------------------------- 5 | 6 | This is a 2D ICP matching example with singular value decomposition. 7 | 8 | It can calculate a rotation matrix and a translation vector between 9 | points to points. 10 | 11 | .. image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif 12 | 13 | References 14 | ~~~~~~~~~~ 15 | 16 | - `Introduction to Mobile Robotics: Iterative Closest Point Algorithm `_ 17 | -------------------------------------------------------------------------------- /docs/modules/slam/slam_main.rst: -------------------------------------------------------------------------------- 1 | .. _slam: 2 | 3 | SLAM 4 | ==== 5 | 6 | Simultaneous Localization and Mapping(SLAM) examples 7 | 8 | .. toctree:: 9 | :maxdepth: 2 10 | :caption: Contents 11 | 12 | iterative_closest_point_matching/iterative_closest_point_matching 13 | ekf_slam/ekf_slam 14 | FastSLAM1/FastSLAM1 15 | FastSLAM2/FastSLAM2 16 | graph_slam/graph_slam 17 | -------------------------------------------------------------------------------- /docs/modules/utils/plot/curvature_plot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/docs/modules/utils/plot/curvature_plot.png -------------------------------------------------------------------------------- /docs/modules/utils/plot/plot_main.rst: -------------------------------------------------------------------------------- 1 | .. _plot_utils: 2 | 3 | Plotting Utilities 4 | ------------------- 5 | 6 | This module contains a number of utility functions for plotting data. 7 | 8 | .. _plot_curvature: 9 | 10 | plot_curvature 11 | ~~~~~~~~~~~~~~~ 12 | 13 | .. autofunction:: utils.plot.plot_curvature 14 | 15 | .. image:: curvature_plot.png 16 | 17 | -------------------------------------------------------------------------------- /docs/modules/utils/utils_main.rst: -------------------------------------------------------------------------------- 1 | .. _utils: 2 | 3 | Utilities 4 | ========== 5 | 6 | Common utilities for PythonRobotics. 7 | 8 | .. toctree:: 9 | :maxdepth: 2 10 | :caption: Contents 11 | 12 | plot/plot 13 | -------------------------------------------------------------------------------- /icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/icon.png -------------------------------------------------------------------------------- /mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | ignore_missing_imports = True -------------------------------------------------------------------------------- /requirements/environment.yml: -------------------------------------------------------------------------------- 1 | name: python_robotics 2 | channels: 3 | - conda-forge 4 | dependencies: 5 | - python=3.10 6 | - pip 7 | - scipy 8 | - numpy 9 | - cvxpy 10 | - matplotlib 11 | -------------------------------------------------------------------------------- /requirements/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy == 1.23.5 2 | scipy == 1.9.3 3 | matplotlib == 3.6.2 4 | cvxpy == 1.2.2 5 | pytest == 7.2.0 # For unit test 6 | pytest-xdist == 3.1.0 # For unit test 7 | mypy == 0.991 # For unit test 8 | flake8 == 5.0.4 # For unit test 9 | -------------------------------------------------------------------------------- /runtests.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | echo "Run test suites! " 3 | 4 | # === pytest based test runner === 5 | # -Werror: warning as error 6 | # --durations=0: show ranking of test durations 7 | # -l (--showlocals); show local variables when test failed 8 | pytest -n auto tests -l -Werror --durations=0 9 | -------------------------------------------------------------------------------- /tests/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/tests/__init__.py -------------------------------------------------------------------------------- /tests/conftest.py: -------------------------------------------------------------------------------- 1 | """Path hack to make tests work.""" 2 | import sys 3 | import os 4 | import pytest 5 | 6 | TEST_DIR = os.path.dirname(os.path.abspath(__file__)) 7 | sys.path.append(TEST_DIR) # to import this file from test code. 8 | ROOT_DIR = os.path.dirname(TEST_DIR) 9 | sys.path.append(ROOT_DIR) 10 | 11 | 12 | def run_this_test(file): 13 | pytest.main([os.path.abspath(file)]) 14 | -------------------------------------------------------------------------------- /tests/test_LQR_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.LQRPlanner import lqr_planner as m 3 | 4 | 5 | def test_1(): 6 | m.SHOW_ANIMATION = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_a_star.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.AStar import a_star as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_a_star_searching_two_side.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.AStar import a_star_searching_from_two_side as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main(800) 8 | 9 | 10 | def test2(): 11 | m.show_animation = False 12 | m.main(5000) # increase obstacle number, block path 13 | 14 | 15 | if __name__ == '__main__': 16 | conftest.run_this_test(__file__) 17 | -------------------------------------------------------------------------------- /tests/test_a_star_variants.py: -------------------------------------------------------------------------------- 1 | import PathPlanning.AStar.a_star_variants as a_star 2 | import conftest 3 | 4 | 5 | def test_1(): 6 | # A* with beam search 7 | a_star.show_animation = False 8 | 9 | a_star.use_beam_search = True 10 | a_star.main() 11 | reset_all() 12 | 13 | # A* with iterative deepening 14 | a_star.use_iterative_deepening = True 15 | a_star.main() 16 | reset_all() 17 | 18 | # A* with dynamic weighting 19 | a_star.use_dynamic_weighting = True 20 | a_star.main() 21 | reset_all() 22 | 23 | # theta* 24 | a_star.use_theta_star = True 25 | a_star.main() 26 | reset_all() 27 | 28 | # A* with jump point 29 | a_star.use_jump_point = True 30 | a_star.main() 31 | reset_all() 32 | 33 | 34 | def reset_all(): 35 | a_star.show_animation = False 36 | a_star.use_beam_search = False 37 | a_star.use_iterative_deepening = False 38 | a_star.use_dynamic_weighting = False 39 | a_star.use_theta_star = False 40 | a_star.use_jump_point = False 41 | 42 | 43 | if __name__ == '__main__': 44 | conftest.run_this_test(__file__) 45 | -------------------------------------------------------------------------------- /tests/test_batch_informed_rrt_star.py: -------------------------------------------------------------------------------- 1 | import random 2 | 3 | import conftest 4 | from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m 5 | 6 | 7 | def test_1(): 8 | m.show_animation = False 9 | random.seed(12345) 10 | m.main(maxIter=10) 11 | 12 | 13 | if __name__ == '__main__': 14 | conftest.run_this_test(__file__) 15 | -------------------------------------------------------------------------------- /tests/test_bezier_path.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.BezierPath import bezier_path as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | def test_2(): 11 | m.show_animation = False 12 | m.main2() 13 | 14 | 15 | if __name__ == '__main__': 16 | conftest.run_this_test(__file__) 17 | -------------------------------------------------------------------------------- /tests/test_bipedal_planner.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Bipedal.bipedal_planner import bipedal_planner as m 3 | 4 | 5 | def test_1(): 6 | bipedal_planner = m.BipedalPlanner() 7 | 8 | footsteps = [[0.0, 0.2, 0.0], 9 | [0.3, 0.2, 0.0], 10 | [0.3, 0.2, 0.2], 11 | [0.3, 0.2, 0.2], 12 | [0.0, 0.2, 0.2]] 13 | bipedal_planner.set_ref_footsteps(footsteps) 14 | bipedal_planner.walk(plot=False) 15 | 16 | 17 | if __name__ == '__main__': 18 | conftest.run_this_test(__file__) 19 | -------------------------------------------------------------------------------- /tests/test_breadth_first_search.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.BreadthFirstSearch import breadth_first_search as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_bspline_path.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | import numpy as np 3 | import pytest 4 | from PathPlanning.BSplinePath import bspline_path 5 | 6 | 7 | def test_list_input(): 8 | way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0] 9 | way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0] 10 | n_course_point = 50 # sampling number 11 | 12 | rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( 13 | way_point_x, way_point_y, n_course_point, s=0.5) 14 | 15 | assert len(rax) == len(ray) == len(heading) == len(curvature) 16 | 17 | rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( 18 | way_point_x, way_point_y, n_course_point) 19 | 20 | assert len(rix) == len(riy) == len(heading) == len(curvature) 21 | 22 | 23 | def test_array_input(): 24 | way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) 25 | way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) 26 | n_course_point = 50 # sampling number 27 | 28 | rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( 29 | way_point_x, way_point_y, n_course_point, s=0.5) 30 | 31 | assert len(rax) == len(ray) == len(heading) == len(curvature) 32 | 33 | rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( 34 | way_point_x, way_point_y, n_course_point) 35 | 36 | assert len(rix) == len(riy) == len(heading) == len(curvature) 37 | 38 | 39 | def test_degree_change(): 40 | way_point_x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) 41 | way_point_y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) 42 | n_course_point = 50 # sampling number 43 | 44 | rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( 45 | way_point_x, way_point_y, n_course_point, s=0.5, degree=4) 46 | 47 | assert len(rax) == len(ray) == len(heading) == len(curvature) 48 | 49 | rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( 50 | way_point_x, way_point_y, n_course_point, degree=4) 51 | 52 | assert len(rix) == len(riy) == len(heading) == len(curvature) 53 | 54 | rax, ray, heading, curvature = bspline_path.approximate_b_spline_path( 55 | way_point_x, way_point_y, n_course_point, s=0.5, degree=2) 56 | 57 | assert len(rax) == len(ray) == len(heading) == len(curvature) 58 | 59 | rix, riy, heading, curvature = bspline_path.interpolate_b_spline_path( 60 | way_point_x, way_point_y, n_course_point, degree=2) 61 | 62 | assert len(rix) == len(riy) == len(heading) == len(curvature) 63 | 64 | with pytest.raises(ValueError): 65 | bspline_path.approximate_b_spline_path( 66 | way_point_x, way_point_y, n_course_point, s=0.5, degree=1) 67 | 68 | with pytest.raises(ValueError): 69 | bspline_path.interpolate_b_spline_path( 70 | way_point_x, way_point_y, n_course_point, degree=1) 71 | 72 | 73 | if __name__ == '__main__': 74 | conftest.run_this_test(__file__) 75 | -------------------------------------------------------------------------------- /tests/test_bug.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.BugPlanning import bug as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main(bug_0=True, bug_1=True, bug_2=True) 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_cgmres_nmpc.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathTracking.cgmres_nmpc import cgmres_nmpc as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_circle_fitting.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Mapping.circle_fitting import circle_fitting as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_closed_loop_rrt_star_car.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m 3 | import random 4 | 5 | 6 | def test_1(): 7 | random.seed(12345) 8 | m.show_animation = False 9 | m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5) 10 | 11 | 12 | if __name__ == '__main__': 13 | conftest.run_this_test(__file__) 14 | -------------------------------------------------------------------------------- /tests/test_clothoidal_paths.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.ClothoidPath import clothoid_path_planner as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_cubature_kalman_filter.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Localization.cubature_kalman_filter import cubature_kalman_filter as m 3 | 4 | 5 | def test1(): 6 | m.show_final = False 7 | m.show_animation = False 8 | m.show_ellipse = False 9 | m.main() 10 | 11 | 12 | if __name__ == '__main__': 13 | conftest.run_this_test(__file__) 14 | -------------------------------------------------------------------------------- /tests/test_d_star_lite.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.DStarLite import d_star_lite as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_depth_first_search.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.DepthFirstSearch import depth_first_search as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_dijkstra.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.Dijkstra import dijkstra as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_drone_3d_trajectory_following.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from AerialNavigation.drone_3d_trajectory_following \ 3 | import drone_3d_trajectory_following as m 4 | 5 | 6 | def test1(): 7 | m.show_animation = False 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_dstar.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.DStar import dstar as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_dubins_path_planning.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import conftest 4 | from PathPlanning.DubinsPath import dubins_path_planner 5 | 6 | np.random.seed(12345) 7 | 8 | 9 | def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, 10 | end_y, end_yaw): 11 | assert (abs(px[0] - start_x) <= 0.01) 12 | assert (abs(py[0] - start_y) <= 0.01) 13 | assert (abs(pyaw[0] - start_yaw) <= 0.01) 14 | assert (abs(px[-1] - end_x) <= 0.01) 15 | assert (abs(py[-1] - end_y) <= 0.01) 16 | assert (abs(pyaw[-1] - end_yaw) <= 0.01) 17 | 18 | 19 | def check_path_length(px, py, lengths): 20 | path_len = sum( 21 | [np.hypot(dx, dy) for (dx, dy) in zip(np.diff(px), np.diff(py))]) 22 | assert (abs(path_len - sum(lengths)) <= 0.1) 23 | 24 | 25 | def test_1(): 26 | start_x = 1.0 # [m] 27 | start_y = 1.0 # [m] 28 | start_yaw = np.deg2rad(45.0) # [rad] 29 | 30 | end_x = -3.0 # [m] 31 | end_y = -3.0 # [m] 32 | end_yaw = np.deg2rad(-45.0) # [rad] 33 | 34 | curvature = 1.0 35 | 36 | px, py, pyaw, mode, lengths = dubins_path_planner.plan_dubins_path( 37 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 38 | 39 | check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, 40 | end_y, end_yaw) 41 | check_path_length(px, py, lengths) 42 | 43 | 44 | def test_2(): 45 | dubins_path_planner.show_animation = False 46 | dubins_path_planner.main() 47 | 48 | 49 | def test_3(): 50 | N_TEST = 10 51 | 52 | for i in range(N_TEST): 53 | start_x = (np.random.rand() - 0.5) * 10.0 # [m] 54 | start_y = (np.random.rand() - 0.5) * 10.0 # [m] 55 | start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 56 | 57 | end_x = (np.random.rand() - 0.5) * 10.0 # [m] 58 | end_y = (np.random.rand() - 0.5) * 10.0 # [m] 59 | end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 60 | 61 | curvature = 1.0 / (np.random.rand() * 5.0) 62 | 63 | px, py, pyaw, mode, lengths = \ 64 | dubins_path_planner.plan_dubins_path( 65 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 66 | 67 | check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, 68 | end_y, end_yaw) 69 | check_path_length(px, py, lengths) 70 | 71 | 72 | def test_path_plannings_types(): 73 | dubins_path_planner.show_animation = False 74 | start_x = 1.0 # [m] 75 | start_y = 1.0 # [m] 76 | start_yaw = np.deg2rad(45.0) # [rad] 77 | 78 | end_x = -3.0 # [m] 79 | end_y = -3.0 # [m] 80 | end_yaw = np.deg2rad(-45.0) # [rad] 81 | 82 | curvature = 1.0 83 | 84 | _, _, _, mode, _ = dubins_path_planner.plan_dubins_path( 85 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature, 86 | selected_types=["RSL"]) 87 | 88 | assert mode == ["R", "S", "L"] 89 | 90 | 91 | if __name__ == '__main__': 92 | conftest.run_this_test(__file__) 93 | -------------------------------------------------------------------------------- /tests/test_dynamic_movement_primitives.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | import numpy as np 3 | from PathPlanning.DynamicMovementPrimitives import \ 4 | dynamic_movement_primitives 5 | 6 | 7 | def test_1(): 8 | # test that trajectory can be learned from user-passed data 9 | T = 5 10 | t = np.arange(0, T, 0.01) 11 | sin_t = np.sin(t) 12 | train_data = np.array([t, sin_t]).T 13 | 14 | DMP_controller = dynamic_movement_primitives.DMP(train_data, T) 15 | DMP_controller.recreate_trajectory(train_data[0], train_data[-1], 4) 16 | 17 | 18 | def test_2(): 19 | # test that length of trajectory is equal to desired number of timesteps 20 | T = 5 21 | t = np.arange(0, T, 0.01) 22 | sin_t = np.sin(t) 23 | train_data = np.array([t, sin_t]).T 24 | 25 | DMP_controller = dynamic_movement_primitives.DMP(train_data, T) 26 | t, path = DMP_controller.recreate_trajectory(train_data[0], 27 | train_data[-1], 4) 28 | 29 | assert(path.shape[0] == DMP_controller.timesteps) 30 | 31 | 32 | def test_3(): 33 | # check that learned trajectory is close to initial 34 | T = 3*np.pi/2 35 | A_noise = 0.02 36 | t = np.arange(0, T, 0.01) 37 | noisy_sin_t = np.sin(t) + A_noise*np.random.rand(len(t)) 38 | train_data = np.array([t, noisy_sin_t]).T 39 | 40 | DMP_controller = dynamic_movement_primitives.DMP(train_data, T) 41 | t, pos = DMP_controller.recreate_trajectory(train_data[0], 42 | train_data[-1], T) 43 | 44 | diff = abs(pos[:, 1] - noisy_sin_t) 45 | assert(max(diff) < 5*A_noise) 46 | 47 | 48 | if __name__ == '__main__': 49 | conftest.run_this_test(__file__) 50 | -------------------------------------------------------------------------------- /tests/test_dynamic_window_approach.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | import numpy as np 3 | 4 | from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m 5 | 6 | 7 | def test_main1(): 8 | m.show_animation = False 9 | m.main(gx=1.0, gy=1.0) 10 | 11 | 12 | def test_main2(): 13 | m.show_animation = False 14 | m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle) 15 | 16 | 17 | def test_stuck_main(): 18 | m.show_animation = False 19 | # adjust cost 20 | m.config.to_goal_cost_gain = 0.2 21 | m.config.obstacle_cost_gain = 2.0 22 | # obstacles and goals for stuck condition 23 | m.config.ob = -1 * np.array([[-1.0, -1.0], 24 | [0.0, 2.0], 25 | [2.0, 6.0], 26 | [2.0, 8.0], 27 | [3.0, 9.27], 28 | [3.79, 9.39], 29 | [7.25, 8.97], 30 | [7.0, 2.0], 31 | [3.0, 4.0], 32 | [6.0, 5.0], 33 | [3.5, 5.8], 34 | [6.0, 9.0], 35 | [8.8, 9.0], 36 | [5.0, 9.0], 37 | [7.5, 3.0], 38 | [9.0, 8.0], 39 | [5.8, 4.4], 40 | [12.0, 12.0], 41 | [3.0, 2.0], 42 | [13.0, 13.0] 43 | ]) 44 | m.main(gx=-5.0, gy=-7.0) 45 | 46 | 47 | if __name__ == '__main__': 48 | conftest.run_this_test(__file__) 49 | -------------------------------------------------------------------------------- /tests/test_ekf_slam.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from SLAM.EKFSLAM import ekf_slam as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_eta3_spline_path.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.Eta3SplinePath import eta3_spline_path as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_extended_kalman_filter.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Localization.extended_kalman_filter import extended_kalman_filter as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_fast_slam1.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from SLAM.FastSLAM1 import fast_slam1 as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.SIM_TIME = 3.0 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_fast_slam2.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from SLAM.FastSLAM2 import fast_slam2 as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.SIM_TIME = 3.0 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_flow_field.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | import PathPlanning.FlowField.flowfield as flow_field 3 | 4 | 5 | def test(): 6 | flow_field.show_animation = False 7 | flow_field.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_frenet_optimal_trajectory.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.SIM_LOOP = 5 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_gaussian_grid_map.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Mapping.gaussian_grid_map import gaussian_grid_map as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_graph_based_slam.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from SLAM.GraphBasedSLAM import graph_based_slam as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.SIM_TIME = 20.0 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_greedy_best_first_search.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.GreedyBestFirstSearch import greedy_best_first_search as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_grid_based_sweep_coverage_path_planner.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.GridBasedSweepCPP \ 3 | import grid_based_sweep_coverage_path_planner 4 | 5 | grid_based_sweep_coverage_path_planner.do_animation = False 6 | RIGHT = grid_based_sweep_coverage_path_planner. \ 7 | SweepSearcher.MovingDirection.RIGHT 8 | LEFT = grid_based_sweep_coverage_path_planner. \ 9 | SweepSearcher.MovingDirection.LEFT 10 | UP = grid_based_sweep_coverage_path_planner. \ 11 | SweepSearcher.SweepDirection.UP 12 | DOWN = grid_based_sweep_coverage_path_planner. \ 13 | SweepSearcher.SweepDirection.DOWN 14 | 15 | 16 | def test_planning1(): 17 | ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0, 0.0] 18 | oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0, 0.0] 19 | resolution = 5.0 20 | 21 | px, py = grid_based_sweep_coverage_path_planner.planning( 22 | ox, oy, resolution, 23 | moving_direction=RIGHT, 24 | sweeping_direction=DOWN, 25 | ) 26 | assert len(px) >= 5 27 | 28 | px, py = grid_based_sweep_coverage_path_planner.planning( 29 | ox, oy, resolution, 30 | moving_direction=LEFT, 31 | sweeping_direction=DOWN, 32 | ) 33 | assert len(px) >= 5 34 | 35 | px, py = grid_based_sweep_coverage_path_planner.planning( 36 | ox, oy, resolution, 37 | moving_direction=RIGHT, 38 | sweeping_direction=UP, 39 | ) 40 | assert len(px) >= 5 41 | 42 | px, py = grid_based_sweep_coverage_path_planner.planning( 43 | ox, oy, resolution, 44 | moving_direction=RIGHT, 45 | sweeping_direction=UP, 46 | ) 47 | assert len(px) >= 5 48 | 49 | 50 | def test_planning2(): 51 | ox = [0.0, 50.0, 50.0, 0.0, 0.0] 52 | oy = [0.0, 0.0, 30.0, 30.0, 0.0] 53 | resolution = 1.3 54 | 55 | px, py = grid_based_sweep_coverage_path_planner.planning( 56 | ox, oy, resolution, 57 | moving_direction=RIGHT, 58 | sweeping_direction=DOWN, 59 | ) 60 | assert len(px) >= 5 61 | 62 | px, py = grid_based_sweep_coverage_path_planner.planning( 63 | ox, oy, resolution, 64 | moving_direction=LEFT, 65 | sweeping_direction=DOWN, 66 | ) 67 | assert len(px) >= 5 68 | 69 | px, py = grid_based_sweep_coverage_path_planner.planning( 70 | ox, oy, resolution, 71 | moving_direction=RIGHT, 72 | sweeping_direction=UP, 73 | ) 74 | assert len(px) >= 5 75 | 76 | px, py = grid_based_sweep_coverage_path_planner.planning( 77 | ox, oy, resolution, 78 | moving_direction=RIGHT, 79 | sweeping_direction=DOWN, 80 | ) 81 | assert len(px) >= 5 82 | 83 | 84 | def test_planning3(): 85 | ox = [0.0, 20.0, 50.0, 200.0, 130.0, 40.0, 0.0] 86 | oy = [0.0, -80.0, 0.0, 30.0, 60.0, 80.0, 0.0] 87 | resolution = 5.1 88 | px, py = grid_based_sweep_coverage_path_planner.planning( 89 | ox, oy, resolution, 90 | moving_direction=RIGHT, 91 | sweeping_direction=DOWN, 92 | ) 93 | assert len(px) >= 5 94 | 95 | px, py = grid_based_sweep_coverage_path_planner.planning( 96 | ox, oy, resolution, 97 | moving_direction=LEFT, 98 | sweeping_direction=DOWN, 99 | ) 100 | assert len(px) >= 5 101 | 102 | px, py = grid_based_sweep_coverage_path_planner.planning( 103 | ox, oy, resolution, 104 | moving_direction=RIGHT, 105 | sweeping_direction=UP, 106 | ) 107 | assert len(px) >= 5 108 | 109 | px, py = grid_based_sweep_coverage_path_planner.planning( 110 | ox, oy, resolution, 111 | moving_direction=RIGHT, 112 | sweeping_direction=DOWN, 113 | ) 114 | assert len(px) >= 5 115 | 116 | 117 | if __name__ == '__main__': 118 | conftest.run_this_test(__file__) 119 | -------------------------------------------------------------------------------- /tests/test_grid_map_lib.py: -------------------------------------------------------------------------------- 1 | from Mapping.grid_map_lib.grid_map_lib import GridMap 2 | import numpy as np 3 | 4 | 5 | def test_position_set(): 6 | grid_map = GridMap(100, 120, 0.5, 10.0, -0.5) 7 | 8 | grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0) 9 | grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0) 10 | grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0) 11 | grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0) 12 | grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0) 13 | grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0) 14 | 15 | 16 | def test_polygon_set(): 17 | ox = [0.0, 4.35, 20.0, 50.0, 100.0, 130.0, 40.0] 18 | oy = [0.0, -4.15, -20.0, 0.0, 30.0, 60.0, 80.0] 19 | 20 | grid_map = GridMap(600, 290, 0.7, 60.0, 30.5) 21 | 22 | grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False) 23 | grid_map.set_value_from_polygon(np.array(ox), np.array(oy), 24 | 1.0, inside=False) 25 | 26 | 27 | if __name__ == '__main__': 28 | conftest.run_this_test(__file__) 29 | -------------------------------------------------------------------------------- /tests/test_histogram_filter.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Localization.histogram_filter import histogram_filter as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.SIM_TIME = 1.0 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_hybrid_a_star.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.HybridAStar import hybrid_a_star as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_informed_rrt_star.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from PathPlanning.InformedRRTStar import informed_rrt_star as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_inverted_pendulum_lqr_control.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Control.inverted_pendulum import inverted_pendulum_lqr_control as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_inverted_pendulum_mpc_control.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | 3 | from Control.inverted_pendulum import inverted_pendulum_mpc_control as m 4 | 5 | 6 | def test1(): 7 | m.show_animation = False 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_iterative_closest_point.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from SLAM.iterative_closest_point import iterative_closest_point as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | def test_2(): 11 | m.show_animation = False 12 | m.main_3d_points() 13 | 14 | 15 | if __name__ == '__main__': 16 | conftest.run_this_test(__file__) 17 | -------------------------------------------------------------------------------- /tests/test_kmeans_clustering.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | from Mapping.kmeans_clustering import kmeans_clustering as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_lqr_rrt_star.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.LQRRRTStar import lqr_rrt_star as m 3 | import random 4 | 5 | random.seed(12345) 6 | 7 | 8 | def test1(): 9 | m.show_animation = False 10 | m.main(maxIter=5) 11 | 12 | 13 | if __name__ == '__main__': 14 | conftest.run_this_test(__file__) 15 | -------------------------------------------------------------------------------- /tests/test_lqr_speed_steer_control.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathTracking.lqr_speed_steer_control import lqr_speed_steer_control as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_lqr_steer_control.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathTracking.lqr_steer_control import lqr_steer_control as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_model_predictive_speed_and_steer_control.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | 3 | from PathTracking.model_predictive_speed_and_steer_control \ 4 | import model_predictive_speed_and_steer_control as m 5 | 6 | 7 | def test_1(): 8 | m.show_animation = False 9 | m.main() 10 | 11 | 12 | def test_2(): 13 | m.show_animation = False 14 | m.main2() 15 | 16 | 17 | if __name__ == '__main__': 18 | conftest.run_this_test(__file__) 19 | -------------------------------------------------------------------------------- /tests/test_move_to_pose.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Control.move_to_pose import move_to_pose as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | def test_2(): 11 | """ 12 | This unit test tests the move_to_pose.py program for a MAX_LINEAR_SPEED and 13 | MAX_ANGULAR_SPEED 14 | """ 15 | m.show_animation = False 16 | m.MAX_LINEAR_SPEED = 11 17 | m.MAX_ANGULAR_SPEED = 5 18 | m.main() 19 | 20 | 21 | if __name__ == '__main__': 22 | conftest.run_this_test(__file__) 23 | -------------------------------------------------------------------------------- /tests/test_move_to_pose_robot.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Control.move_to_pose import move_to_pose as m 3 | 4 | 5 | def test_1(): 6 | """ 7 | This unit test tests the move_to_pose_robot.py program 8 | """ 9 | m.show_animation = False 10 | m.main() 11 | 12 | 13 | if __name__ == '__main__': 14 | conftest.run_this_test(__file__) 15 | -------------------------------------------------------------------------------- /tests/test_mypy_type_check.py: -------------------------------------------------------------------------------- 1 | import os 2 | import subprocess 3 | 4 | import conftest 5 | 6 | SUBPACKAGE_LIST = [ 7 | "AerialNavigation", 8 | "ArmNavigation", 9 | "Bipedal", 10 | "Control", 11 | "Localization", 12 | "Mapping", 13 | "PathPlanning", 14 | "PathTracking", 15 | "SLAM", 16 | ] 17 | 18 | 19 | def run_mypy(dir_name, project_path, config_path): 20 | res = subprocess.run( 21 | ['mypy', 22 | '--config-file', 23 | config_path, 24 | '-p', 25 | dir_name], 26 | cwd=project_path, 27 | stdout=subprocess.PIPE, 28 | encoding='utf-8') 29 | return res.returncode, res.stdout 30 | 31 | 32 | def test(): 33 | project_dir_path = os.path.dirname( 34 | os.path.dirname(os.path.abspath(__file__))) 35 | print(f"{project_dir_path=}") 36 | 37 | config_path = os.path.join(project_dir_path, "mypy.ini") 38 | print(f"{config_path=}") 39 | 40 | for sub_package_name in SUBPACKAGE_LIST: 41 | rc, errors = run_mypy(sub_package_name, project_dir_path, config_path) 42 | if errors: 43 | print(errors) 44 | else: 45 | print("No lint errors found.") 46 | assert rc == 0 47 | 48 | 49 | if __name__ == '__main__': 50 | conftest.run_this_test(__file__) 51 | -------------------------------------------------------------------------------- /tests/test_n_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from ArmNavigation.n_joint_arm_to_point_control\ 3 | import n_joint_arm_to_point_control as m 4 | import random 5 | 6 | random.seed(12345) 7 | 8 | 9 | def test1(): 10 | m.show_animation = False 11 | m.animation() 12 | 13 | 14 | if __name__ == '__main__': 15 | conftest.run_this_test(__file__) 16 | -------------------------------------------------------------------------------- /tests/test_particle_filter.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Localization.particle_filter import particle_filter as m 3 | 4 | 5 | def test_1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_potential_field_planning.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.PotentialFieldPlanning import potential_field_planning as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_probabilistic_road_map.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | import numpy as np 3 | from PathPlanning.ProbabilisticRoadMap import probabilistic_road_map 4 | 5 | 6 | def test1(): 7 | probabilistic_road_map.show_animation = False 8 | probabilistic_road_map.main(rng=np.random.default_rng(1233)) 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_pure_pursuit.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathTracking.pure_pursuit import pure_pursuit as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_quintic_polynomials_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.QuinticPolynomialsPlanner import quintic_polynomials_planner as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_raycasting_grid_map.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Mapping.raycasting_grid_map import raycasting_grid_map as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_rear_wheel_feedback.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathTracking.rear_wheel_feedback import rear_wheel_feedback as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_rectangle_fitting.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Mapping.rectangle_fitting import rectangle_fitting as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_reeds_shepp_path_planning.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import conftest # Add root path to sys.path 4 | from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m 5 | 6 | 7 | def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, 8 | end_y, end_yaw): 9 | assert (abs(px[0] - start_x) <= 0.01) 10 | assert (abs(py[0] - start_y) <= 0.01) 11 | assert (abs(pyaw[0] - start_yaw) <= 0.01) 12 | assert (abs(px[-1] - end_x) <= 0.01) 13 | assert (abs(py[-1] - end_y) <= 0.01) 14 | assert (abs(pyaw[-1] - end_yaw) <= 0.01) 15 | 16 | 17 | def check_path_length(px, py, lengths): 18 | sum_len = sum(abs(length) for length in lengths) 19 | dpx = np.diff(px) 20 | dpy = np.diff(py) 21 | actual_len = sum( 22 | np.hypot(dx, dy) for (dx, dy) in zip(dpx, dpy)) 23 | diff_len = sum_len - actual_len 24 | assert (diff_len <= 0.01) 25 | 26 | 27 | def test1(): 28 | m.show_animation = False 29 | m.main() 30 | 31 | 32 | def test2(): 33 | N_TEST = 10 34 | np.random.seed(1234) 35 | 36 | for i in range(N_TEST): 37 | start_x = (np.random.rand() - 0.5) * 10.0 # [m] 38 | start_y = (np.random.rand() - 0.5) * 10.0 # [m] 39 | start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 40 | 41 | end_x = (np.random.rand() - 0.5) * 10.0 # [m] 42 | end_y = (np.random.rand() - 0.5) * 10.0 # [m] 43 | end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 44 | 45 | curvature = 1.0 / (np.random.rand() * 5.0) 46 | 47 | px, py, pyaw, mode, lengths = m.reeds_shepp_path_planning( 48 | start_x, start_y, start_yaw, 49 | end_x, end_y, end_yaw, curvature) 50 | 51 | check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, 52 | end_y, end_yaw) 53 | check_path_length(px, py, lengths) 54 | 55 | 56 | if __name__ == '__main__': 57 | conftest.run_this_test(__file__) 58 | -------------------------------------------------------------------------------- /tests/test_rocket_powered_landing.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | import numpy as np 3 | from numpy.testing import suppress_warnings 4 | 5 | from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m 6 | 7 | 8 | def test1(): 9 | m.show_animation = False 10 | with suppress_warnings() as sup: 11 | sup.filter(UserWarning, 12 | "You are solving a parameterized problem that is not DPP" 13 | ) 14 | sup.filter(UserWarning, 15 | "Solution may be inaccurate") 16 | m.main(rng=np.random.default_rng(1234)) 17 | 18 | 19 | if __name__ == '__main__': 20 | conftest.run_this_test(__file__) 21 | -------------------------------------------------------------------------------- /tests/test_rrt.py: -------------------------------------------------------------------------------- 1 | import conftest 2 | import random 3 | 4 | from PathPlanning.RRT import rrt as m 5 | from PathPlanning.RRT import rrt_with_pathsmoothing as m1 6 | 7 | random.seed(12345) 8 | 9 | 10 | def test1(): 11 | m.show_animation = False 12 | m.main(gx=1.0, gy=1.0) 13 | 14 | 15 | def test2(): 16 | m1.show_animation = False 17 | m1.main() 18 | 19 | 20 | if __name__ == '__main__': 21 | conftest.run_this_test(__file__) 22 | -------------------------------------------------------------------------------- /tests/test_rrt_dubins.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.RRTDubins import rrt_dubins as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_rrt_star.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.RRTStar import rrt_star as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | def test_no_obstacle(): 11 | obstacle_list = [] 12 | 13 | # Set Initial parameters 14 | rrt_star = m.RRTStar(start=[0, 0], 15 | goal=[6, 10], 16 | rand_area=[-2, 15], 17 | obstacle_list=obstacle_list) 18 | path = rrt_star.planning(animation=False) 19 | assert path is not None 20 | 21 | 22 | def test_no_obstacle_and_robot_radius(): 23 | obstacle_list = [] 24 | 25 | # Set Initial parameters 26 | rrt_star = m.RRTStar(start=[0, 0], 27 | goal=[6, 10], 28 | rand_area=[-2, 15], 29 | obstacle_list=obstacle_list, 30 | robot_radius=0.8) 31 | path = rrt_star.planning(animation=False) 32 | assert path is not None 33 | 34 | 35 | if __name__ == '__main__': 36 | conftest.run_this_test(__file__) 37 | -------------------------------------------------------------------------------- /tests/test_rrt_star_dubins.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.RRTStarDubins import rrt_star_dubins as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_rrt_star_reeds_shepp.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.RRTStarReedsShepp import rrt_star_reeds_shepp as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main(max_iter=5) 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_rrt_star_seven_joint_arm.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from ArmNavigation.rrt_star_seven_joint_arm_control \ 3 | import rrt_star_seven_joint_arm_control as m 4 | 5 | 6 | def test1(): 7 | m.show_animation = False 8 | m.main() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_rrt_with_sobol_sampler.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.RRT import rrt_with_sobol_sampler as m 3 | import random 4 | 5 | random.seed(12345) 6 | 7 | 8 | def test1(): 9 | m.show_animation = False 10 | m.main(gx=1.0, gy=1.0) 11 | 12 | 13 | if __name__ == '__main__': 14 | conftest.run_this_test(__file__) 15 | -------------------------------------------------------------------------------- /tests/test_spiral_spanning_tree_coverage_path_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | import os 3 | import matplotlib.pyplot as plt 4 | from PathPlanning.SpiralSpanningTreeCPP \ 5 | import spiral_spanning_tree_coverage_path_planner 6 | 7 | spiral_spanning_tree_coverage_path_planner.do_animation = True 8 | 9 | 10 | def spiral_stc_cpp(img, start): 11 | num_free = 0 12 | for i in range(img.shape[0]): 13 | for j in range(img.shape[1]): 14 | num_free += img[i][j] 15 | 16 | STC_planner = spiral_spanning_tree_coverage_path_planner.\ 17 | SpiralSpanningTreeCoveragePlanner(img) 18 | 19 | edge, route, path = STC_planner.plan(start) 20 | 21 | covered_nodes = set() 22 | for p, q in edge: 23 | covered_nodes.add(p) 24 | covered_nodes.add(q) 25 | 26 | # assert complete coverage 27 | assert len(covered_nodes) == num_free / 4 28 | 29 | 30 | def test_spiral_stc_cpp_1(): 31 | img_dir = os.path.dirname( 32 | os.path.abspath(__file__)) + \ 33 | "/../PathPlanning/SpiralSpanningTreeCPP" 34 | img = plt.imread(os.path.join(img_dir, 'map', 'test.png')) 35 | start = (0, 0) 36 | spiral_stc_cpp(img, start) 37 | 38 | 39 | def test_spiral_stc_cpp_2(): 40 | img_dir = os.path.dirname( 41 | os.path.abspath(__file__)) + \ 42 | "/../PathPlanning/SpiralSpanningTreeCPP" 43 | img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png')) 44 | start = (10, 0) 45 | spiral_stc_cpp(img, start) 46 | 47 | 48 | def test_spiral_stc_cpp_3(): 49 | img_dir = os.path.dirname( 50 | os.path.abspath(__file__)) + \ 51 | "/../PathPlanning/SpiralSpanningTreeCPP" 52 | img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png')) 53 | start = (0, 0) 54 | spiral_stc_cpp(img, start) 55 | 56 | 57 | if __name__ == '__main__': 58 | conftest.run_this_test(__file__) 59 | -------------------------------------------------------------------------------- /tests/test_stanley_controller.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathTracking.stanley_controller import stanley_controller as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_state_lattice_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.StateLatticePlanner import state_lattice_planner as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_two_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from ArmNavigation.two_joint_arm_to_point_control \ 3 | import two_joint_arm_to_point_control as m 4 | 5 | 6 | def test1(): 7 | m.show_animation = False 8 | m.animation() 9 | 10 | 11 | if __name__ == '__main__': 12 | conftest.run_this_test(__file__) 13 | -------------------------------------------------------------------------------- /tests/test_unscented_kalman_filter.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from Localization.unscented_kalman_filter import unscented_kalman_filter as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_utils.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from utils import angle 3 | from numpy.testing import assert_allclose 4 | import numpy as np 5 | 6 | 7 | def test_rot_mat_2d(): 8 | assert_allclose(angle.rot_mat_2d(0.0), 9 | np.array([[1., 0.], 10 | [0., 1.]])) 11 | 12 | 13 | def test_angle_mod(): 14 | assert_allclose(angle.angle_mod(-4.0), 2.28318531) 15 | assert(isinstance(angle.angle_mod(-4.0), float)) 16 | assert_allclose(angle.angle_mod([-4.0]), [2.28318531]) 17 | assert(isinstance(angle.angle_mod([-4.0]), np.ndarray)) 18 | 19 | assert_allclose(angle.angle_mod([-150.0, 190.0, 350], degree=True), 20 | [-150., -170., -10.]) 21 | 22 | assert_allclose(angle.angle_mod(-60.0, zero_2_2pi=True, degree=True), 23 | [300.]) 24 | 25 | 26 | if __name__ == '__main__': 27 | conftest.run_this_test(__file__) 28 | -------------------------------------------------------------------------------- /tests/test_visibility_road_map_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.VoronoiRoadMap import voronoi_road_map as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_voronoi_road_map_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | from PathPlanning.VisibilityRoadMap import visibility_road_map as m 3 | 4 | 5 | def test1(): 6 | m.show_animation = False 7 | m.main() 8 | 9 | 10 | if __name__ == '__main__': 11 | conftest.run_this_test(__file__) 12 | -------------------------------------------------------------------------------- /tests/test_wavefront_coverage_path_planner.py: -------------------------------------------------------------------------------- 1 | import conftest # Add root path to sys.path 2 | import os 3 | import matplotlib.pyplot as plt 4 | from PathPlanning.WavefrontCPP import wavefront_coverage_path_planner 5 | 6 | wavefront_coverage_path_planner.do_animation = False 7 | 8 | 9 | def wavefront_cpp(img, start, goal): 10 | num_free = 0 11 | for i in range(img.shape[0]): 12 | for j in range(img.shape[1]): 13 | num_free += 1 - img[i][j] 14 | 15 | DT = wavefront_coverage_path_planner.transform( 16 | img, goal, transform_type='distance') 17 | DT_path = wavefront_coverage_path_planner.wavefront(DT, start, goal) 18 | assert len(DT_path) == num_free # assert complete coverage 19 | 20 | PT = wavefront_coverage_path_planner.transform( 21 | img, goal, transform_type='path', alpha=0.01) 22 | PT_path = wavefront_coverage_path_planner.wavefront(PT, start, goal) 23 | assert len(PT_path) == num_free # assert complete coverage 24 | 25 | 26 | def test_wavefront_CPP_1(): 27 | img_dir = os.path.dirname( 28 | os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP" 29 | img = plt.imread(os.path.join(img_dir, 'map', 'test.png')) 30 | img = 1 - img 31 | 32 | start = (43, 0) 33 | goal = (0, 0) 34 | 35 | wavefront_cpp(img, start, goal) 36 | 37 | 38 | def test_wavefront_CPP_2(): 39 | img_dir = os.path.dirname( 40 | os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP" 41 | img = plt.imread(os.path.join(img_dir, 'map', 'test_2.png')) 42 | img = 1 - img 43 | 44 | start = (10, 0) 45 | goal = (10, 40) 46 | 47 | wavefront_cpp(img, start, goal) 48 | 49 | 50 | def test_wavefront_CPP_3(): 51 | img_dir = os.path.dirname( 52 | os.path.abspath(__file__)) + "/../PathPlanning/WavefrontCPP" 53 | img = plt.imread(os.path.join(img_dir, 'map', 'test_3.png')) 54 | img = 1 - img 55 | 56 | start = (0, 0) 57 | goal = (30, 30) 58 | 59 | wavefront_cpp(img, start, goal) 60 | 61 | 62 | if __name__ == '__main__': 63 | conftest.run_this_test(__file__) 64 | -------------------------------------------------------------------------------- /utils/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Alexbeast-CN/PythonRobotics/6653ed1fd16dc19c02bcbc9225533dc71106697d/utils/__init__.py -------------------------------------------------------------------------------- /utils/angle.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from scipy.spatial.transform import Rotation as Rot 3 | 4 | 5 | def rot_mat_2d(angle): 6 | """ 7 | Create 2D rotation matrix from an angle 8 | 9 | Parameters 10 | ---------- 11 | angle : 12 | 13 | Returns 14 | ------- 15 | A 2D rotation matrix 16 | 17 | Examples 18 | -------- 19 | >>> angle_mod(-4.0) 20 | 21 | 22 | """ 23 | return Rot.from_euler('z', angle).as_matrix()[0:2, 0:2] 24 | 25 | 26 | def angle_mod(x, zero_2_2pi=False, degree=False): 27 | """ 28 | Angle modulo operation 29 | Default angle modulo range is [-pi, pi) 30 | 31 | Parameters 32 | ---------- 33 | x : float or array_like 34 | A angle or an array of angles. This array is flattened for 35 | the calculation. When an angle is provided, a float angle is returned. 36 | zero_2_2pi : bool, optional 37 | Change angle modulo range to [0, 2pi) 38 | Default is False. 39 | degree : bool, optional 40 | If True, then the given angles are assumed to be in degrees. 41 | Default is False. 42 | 43 | Returns 44 | ------- 45 | ret : float or ndarray 46 | an angle or an array of modulated angle. 47 | 48 | Examples 49 | -------- 50 | >>> angle_mod(-4.0) 51 | 2.28318531 52 | 53 | >>> angle_mod([-4.0]) 54 | np.array(2.28318531) 55 | 56 | >>> angle_mod([-150.0, 190.0, 350], degree=True) 57 | array([-150., -170., -10.]) 58 | 59 | >>> angle_mod(-60.0, zero_2_2pi=True, degree=True) 60 | array([300.]) 61 | 62 | """ 63 | if isinstance(x, float): 64 | is_float = True 65 | else: 66 | is_float = False 67 | 68 | x = np.asarray(x).flatten() 69 | if degree: 70 | x = np.deg2rad(x) 71 | 72 | if zero_2_2pi: 73 | mod_angle = x % (2 * np.pi) 74 | else: 75 | mod_angle = (x + np.pi) % (2 * np.pi) - np.pi 76 | 77 | if degree: 78 | mod_angle = np.rad2deg(mod_angle) 79 | 80 | if is_float: 81 | return mod_angle.item() 82 | else: 83 | return mod_angle 84 | -------------------------------------------------------------------------------- /utils/plot.py: -------------------------------------------------------------------------------- 1 | """ 2 | Matplotlib based plotting utilities 3 | """ 4 | import math 5 | import matplotlib.pyplot as plt 6 | import numpy as np 7 | 8 | 9 | def plot_arrow(x, y, yaw, arrow_length=1.0, 10 | origin_point_plot_style="xr", 11 | head_width=0.1, fc="r", ec="k", **kwargs): 12 | """ 13 | Plot an arrow or arrows based on 2D state (x, y, yaw) 14 | 15 | All optional settings of matplotlib.pyplot.arrow can be used. 16 | - matplotlib.pyplot.arrow: 17 | https://matplotlib.org/stable/api/_as_gen/matplotlib.pyplot.arrow.html 18 | 19 | Parameters 20 | ---------- 21 | x : a float or array_like 22 | a value or a list of arrow origin x position. 23 | y : a float or array_like 24 | a value or a list of arrow origin y position. 25 | yaw : a float or array_like 26 | a value or a list of arrow yaw angle (orientation). 27 | arrow_length : a float (optional) 28 | arrow length. default is 1.0 29 | origin_point_plot_style : str (optional) 30 | origin point plot style. If None, not plotting. 31 | head_width : a float (optional) 32 | arrow head width. default is 0.1 33 | fc : string (optional) 34 | face color 35 | ec : string (optional) 36 | edge color 37 | """ 38 | if not isinstance(x, float): 39 | for (i_x, i_y, i_yaw) in zip(x, y, yaw): 40 | plot_arrow(i_x, i_y, i_yaw, head_width=head_width, 41 | fc=fc, ec=ec, **kwargs) 42 | else: 43 | plt.arrow(x, y, 44 | arrow_length * math.cos(yaw), 45 | arrow_length * math.sin(yaw), 46 | head_width=head_width, 47 | fc=fc, ec=ec, 48 | **kwargs) 49 | if origin_point_plot_style is not None: 50 | plt.plot(x, y, origin_point_plot_style) 51 | 52 | 53 | def plot_curvature(x_list, y_list, heading_list, curvature, 54 | k=0.01, c="-c", label="Curvature"): 55 | """ 56 | Plot curvature on 2D path. This plot is a line from the original path, 57 | the lateral distance from the original path shows curvature magnitude. 58 | Left turning shows right side plot, right turning shows left side plot. 59 | For straight path, the curvature plot will be on the path, because 60 | curvature is 0 on the straight path. 61 | 62 | Parameters 63 | ---------- 64 | x_list : array_like 65 | x position list of the path 66 | y_list : array_like 67 | y position list of the path 68 | heading_list : array_like 69 | heading list of the path 70 | curvature : array_like 71 | curvature list of the path 72 | k : float 73 | curvature scale factor to calculate distance from the original path 74 | c : string 75 | color of the plot 76 | label : string 77 | label of the plot 78 | """ 79 | cx = [x + d * k * np.cos(yaw - np.pi / 2.0) for x, y, yaw, d in 80 | zip(x_list, y_list, heading_list, curvature)] 81 | cy = [y + d * k * np.sin(yaw - np.pi / 2.0) for x, y, yaw, d in 82 | zip(x_list, y_list, heading_list, curvature)] 83 | 84 | plt.plot(cx, cy, c, label=label) 85 | for ix, iy, icx, icy in zip(x_list, y_list, cx, cy): 86 | plt.plot([ix, icx], [iy, icy], c) 87 | --------------------------------------------------------------------------------