├── SLAM ├── __init__.py ├── EKFSLAM │ └── animation.png ├── FastSLAM1 │ └── animation.png ├── PoseOptimizationSLAM2D │ ├── README.md │ └── data_downloader.py ├── PoseOptimizationSLAM3D │ ├── data_downloader.py │ └── README.md └── iterative_closest_point │ └── iterative_closest_point.py ├── tests ├── .gitkeep ├── test_ekf_slam.py ├── test_eta3_spline_path.py ├── test_dijkstra.py ├── test_reeds_shepp_path_planning.py ├── test_circle_fitting.py ├── test_move_to_pose.py ├── test_pure_pursuit.py ├── test_gaussian_grid_map.py ├── test_kmeans_clustering.py ├── test_particle_filter.py ├── test_voronoi_path_planner.py ├── test_raycasting_grid_map.py ├── test_iterative_closest_point.py ├── test_extended_kalman_filter.py ├── test_potential_field_planning.py ├── test_probabilistic_road_map.py ├── test_unscented_kalman_filter.py ├── test_quintic_polynomials_planner.py ├── test_two_joint_arm_to_point_control.py ├── test_LQR_planner.py ├── test_bezier_path.py ├── test_lqr_steer_control.py ├── test_stanley_controller.py ├── test_rear_wheel_feedback.py ├── test_lqr_speed_steer_control.py ├── test_n_joint_arm_to_point_control.py ├── test_cgmres_nmpc.py ├── test_drone_3d_trajectory_following.py ├── test_a_star.py ├── test_rocket_powered_landing.py ├── test_fast_slam1.py ├── test_fast_slam2.py ├── test_batch_informed_rrt_star.py ├── test_dynamic_window_approach.py ├── test_histogram_filter.py ├── test_rrt_star.py ├── test_rrt_star_dubins.py ├── test_graph_based_slam.py ├── test_model_predictive_speed_and_steer_control.py ├── test_lqr_rrt_star.py ├── test_frenet_optimal_trajectory.py ├── test_informed_rrt_star.py ├── test_rrt_dubins.py ├── test_rrt_star_reeds_shepp.py ├── test_rrt.py ├── test_closed_loop_rrt_star_car.py ├── test_state_lattice_planner.py ├── test_dubins_path_planning.py └── test_grid_map_lib.py ├── Bipedal └── __init__.py ├── docs ├── _static │ └── .gitkeep ├── modules │ ├── introduction.rst │ ├── rrt_star_files │ │ └── rrt_star_1_0.png │ ├── FastSLAM1_files │ │ ├── FastSLAM1_12_0.png │ │ ├── FastSLAM1_12_1.png │ │ └── FastSLAM1_1_0.png │ ├── cgmres_nmpc_files │ │ ├── cgmres_nmpc_1_0.png │ │ ├── cgmres_nmpc_2_0.png │ │ ├── cgmres_nmpc_3_0.png │ │ └── cgmres_nmpc_4_0.png │ ├── 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 │ ├── 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 │ ├── appendix.rst │ ├── Kalmanfilter_basics_2_files │ │ └── Kalmanfilter_basics_2_5_0.png │ ├── extended_kalman_filter_localization_files │ │ └── extended_kalman_filter_localization_1_0.png │ ├── aerial_navigation.rst │ ├── rrt_star.rst │ ├── arm_navigation.rst │ ├── mapping.rst │ ├── localization.rst │ ├── slam.rst │ ├── cgmres_nmpc.rst │ ├── path_tracking.rst │ ├── extended_kalman_filter_localization.rst │ └── Model_predictive_speed_and_steering_control.rst ├── Makefile ├── README.md ├── make.bat ├── getting_started.rst ├── index.rst ├── jupyternotebook2rst.py └── conf.py ├── ArmNavigation ├── __init__.py ├── n_joint_arm_to_point_control │ ├── NLinkArm.py │ └── n_joint_arm_to_point_control.py └── two_joint_arm_to_point_control │ └── two_joint_arm_to_point_control.py ├── PathPlanning ├── RRT │ ├── __init__.py │ ├── figure_1.png │ ├── .vscode │ │ └── settings.json │ └── rrt_with_pathsmoothing.py ├── __init__.py ├── RRTStar │ ├── __init__.py │ └── Figure_1.png ├── DubinsPath │ └── __init__.py ├── RRTDubins │ └── __init__.py ├── ClosedLoopRRTStar │ ├── __init__.py │ ├── Figure_1.png │ ├── Figure_3.png │ ├── Figure_4.png │ ├── Figure_5.png │ └── unicycle_model.py ├── StateLatticePlanner │ ├── __init__.py │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ ├── Figure_4.png │ ├── Figure_5.png │ └── Figure_6.png ├── ModelPredictiveTrajectoryGenerator │ ├── __init__.py │ ├── lookuptable.png │ ├── motion_model.py │ ├── lookuptable_generator.py │ └── model_predictive_trajectory_generator.py ├── BSplinePath │ ├── Figure_1.png │ └── bspline_path.py ├── BezierPath │ ├── Figure_1.png │ └── Figure_2.png ├── CubicSpline │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ └── cubic_spline_planner.py ├── RRTStarReedsShepp │ └── figure_1.png ├── HybridAStar │ └── car.py ├── LQRPlanner │ └── LQRplanner.py ├── PotentialFieldPlanning │ └── potential_field_planning.py └── FrenetOptimalTrajectory │ └── cubic_spline_planner.py ├── PathTracking ├── lqr_steer_control │ └── __init__.py ├── cgmres_nmpc │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ └── Figure_4.png ├── rear_wheel_feedback │ ├── Figure_2.png │ └── rear_wheel_feedback.py ├── move_to_pose │ └── move_to_pose.py ├── pure_pursuit │ └── pure_pursuit.py └── stanley_controller │ └── stanley_controller.py ├── _config.yml ├── requirements.txt ├── icon.png ├── .lgtm.yml ├── Localization ├── bayes_filter.png └── extended_kalman_filter │ ├── ekf.png │ └── extended_kalman_filter.py ├── .github └── FUNDING.yml ├── Mapping ├── lidar_to_grid_map │ └── grid_map_example.png ├── gaussian_grid_map │ └── gaussian_grid_map.py ├── raycasting_grid_map │ └── raycasting_grid_map.py ├── circle_fitting │ └── circle_fitting.py ├── rectangle_fitting │ └── simulator.py └── kmeans_clustering │ └── kmeans_clustering.py ├── AerialNavigation ├── rocket_powered_landing │ └── figure.png └── drone_3d_trajectory_following │ ├── TrajectoryGenerator.py │ ├── Quadrotor.py │ └── drone_3d_trajectory_following.py ├── environment.yml ├── runtests.sh ├── CONTRIBUTING.md ├── Introduction └── introduction.ipynb ├── .travis.yml ├── LICENSE ├── .gitignore └── appveyor.yml /SLAM/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /tests/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /Bipedal/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /docs/_static/.gitkeep: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /ArmNavigation/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/RRT/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/RRTStar/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/DubinsPath/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/RRTDubins/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathTracking/lqr_steer_control/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | show_downloads: true 3 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pandas 3 | scipy 4 | matplotlib 5 | cvxpy 6 | -------------------------------------------------------------------------------- /icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/icon.png -------------------------------------------------------------------------------- /.lgtm.yml: -------------------------------------------------------------------------------- 1 | extraction: 2 | python: 3 | python_setup: 4 | version: 3 5 | -------------------------------------------------------------------------------- /docs/modules/introduction.rst: -------------------------------------------------------------------------------- 1 | 2 | Introduction 3 | ============ 4 | 5 | Ref 6 | --- 7 | -------------------------------------------------------------------------------- /SLAM/EKFSLAM/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/SLAM/EKFSLAM/animation.png -------------------------------------------------------------------------------- /Localization/bayes_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/Localization/bayes_filter.png -------------------------------------------------------------------------------- /PathPlanning/RRT/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/RRT/figure_1.png -------------------------------------------------------------------------------- /SLAM/FastSLAM1/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/SLAM/FastSLAM1/animation.png -------------------------------------------------------------------------------- /PathPlanning/RRT/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "D:\\Anaconda\\envs\\python_robotics\\python.exe" 3 | } -------------------------------------------------------------------------------- /PathPlanning/RRTStar/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/RRTStar/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/BSplinePath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/BezierPath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/BezierPath/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/CubicSpline/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/CubicSpline/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/CubicSpline/Figure_3.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathTracking/cgmres_nmpc/Figure_1.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathTracking/cgmres_nmpc/Figure_2.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathTracking/cgmres_nmpc/Figure_3.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathTracking/cgmres_nmpc/Figure_4.png -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | patreon: myenigma 3 | custom: https://www.paypal.me/myenigmapay/ 4 | -------------------------------------------------------------------------------- /Localization/extended_kalman_filter/ekf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/Localization/extended_kalman_filter/ekf.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/ClosedLoopRRTStar/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/ClosedLoopRRTStar/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/ClosedLoopRRTStar/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/ClosedLoopRRTStar/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/RRTStarReedsShepp/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/RRTStarReedsShepp/figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/StateLatticePlanner/Figure_6.png -------------------------------------------------------------------------------- /PathTracking/rear_wheel_feedback/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathTracking/rear_wheel_feedback/Figure_2.png -------------------------------------------------------------------------------- /docs/modules/rrt_star_files/rrt_star_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/rrt_star_files/rrt_star_1_0.png -------------------------------------------------------------------------------- /Mapping/lidar_to_grid_map/grid_map_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/Mapping/lidar_to_grid_map/grid_map_example.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/FastSLAM1_files/FastSLAM1_12_0.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_12_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/FastSLAM1_files/FastSLAM1_12_1.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/FastSLAM1_files/FastSLAM1_1_0.png -------------------------------------------------------------------------------- /AerialNavigation/rocket_powered_landing/figure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/AerialNavigation/rocket_powered_landing/figure.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: python_robotics 2 | dependencies: 3 | - python 4 | - matplotlib 5 | - scipy 6 | - numpy==1.15 7 | - pandas 8 | - pip: 9 | - cvxpy 10 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_5_0.png -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_7_0.png -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png -------------------------------------------------------------------------------- /docs/modules/appendix.rst: -------------------------------------------------------------------------------- 1 | .. _appendix: 2 | 3 | Appendix 4 | ============== 5 | 6 | .. include:: Kalmanfilter_basics.rst 7 | 8 | .. include:: Kalmanfilter_basics_2.rst 9 | 10 | -------------------------------------------------------------------------------- /SLAM/PoseOptimizationSLAM2D/README.md: -------------------------------------------------------------------------------- 1 | # How to use 2 | 3 | 1. Download data 4 | 5 | >$ python data_downloader.py 6 | 7 | 2. run SLAM 8 | 9 | >$ python pose_optimization_slam.py 10 | -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png -------------------------------------------------------------------------------- /runtests.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | echo "Run test suites! " 3 | #python -m unittest discover tests 4 | #python -Wignore -m unittest discover tests #ignore warning 5 | coverage run -m unittest discover tests # generate coverage file 6 | -------------------------------------------------------------------------------- /docs/modules/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ddl-hust/PythonRobotis/HEAD/docs/modules/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png -------------------------------------------------------------------------------- /tests/test_ekf_slam.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from SLAM.EKFSLAM import ekf_slam as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_eta3_spline_path.py: -------------------------------------------------------------------------------- 1 | 2 | from unittest import TestCase 3 | from PathPlanning.Eta3SplinePath import eta3_spline_path as m 4 | 5 | 6 | class Test(TestCase): 7 | 8 | def test1(self): 9 | m.show_animation = False 10 | m.main() 11 | -------------------------------------------------------------------------------- /tests/test_dijkstra.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from PathPlanning.Dijkstra import dijkstra as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_reeds_shepp_path_planning.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m 3 | 4 | 5 | class Test(TestCase): 6 | 7 | def test1(self): 8 | m.show_animation = False 9 | m.main() 10 | -------------------------------------------------------------------------------- /tests/test_circle_fitting.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Mapping.circle_fitting import circle_fitting as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_move_to_pose.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from PathTracking.move_to_pose import move_to_pose as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_pure_pursuit.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathTracking.pure_pursuit import pure_pursuit as m 3 | 4 | print("pure_pursuit test") 5 | 6 | 7 | class Test(TestCase): 8 | 9 | def test1(self): 10 | m.show_animation = False 11 | m.main() 12 | -------------------------------------------------------------------------------- /tests/test_gaussian_grid_map.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Mapping.gaussian_grid_map import gaussian_grid_map as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_kmeans_clustering.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Mapping.kmeans_clustering import kmeans_clustering as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_particle_filter.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Localization.particle_filter import particle_filter as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_voronoi_path_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.VoronoiRoadMap import voronoi_road_map as m 3 | 4 | print(__file__) 5 | 6 | 7 | class Test(TestCase): 8 | 9 | def test1(self): 10 | m.show_animation = False 11 | m.main() 12 | -------------------------------------------------------------------------------- /tests/test_raycasting_grid_map.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Mapping.raycasting_grid_map import raycasting_grid_map as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_iterative_closest_point.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from SLAM.iterative_closest_point import iterative_closest_point as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_extended_kalman_filter.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Localization.extended_kalman_filter import extended_kalman_filter as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_potential_field_planning.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from PathPlanning.PotentialFieldPlanning import potential_field_planning as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_probabilistic_road_map.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.ProbabilisticRoadMap import probabilistic_road_map 3 | 4 | 5 | class Test(TestCase): 6 | 7 | def test1(self): 8 | probabilistic_road_map.show_animation = False 9 | probabilistic_road_map.main() 10 | -------------------------------------------------------------------------------- /tests/test_unscented_kalman_filter.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from Localization.unscented_kalman_filter import unscented_kalman_filter as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_quintic_polynomials_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from PathPlanning.QuinticPolynomialsPlanner import quintic_polynomials_planner as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.main() 13 | -------------------------------------------------------------------------------- /tests/test_two_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | from ArmNavigation.two_joint_arm_to_point_control import two_joint_arm_to_point_control as m 4 | 5 | print(__file__) 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | m.show_animation = False 12 | m.animation() 13 | -------------------------------------------------------------------------------- /tests/test_LQR_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathPlanning/LQRPlanner") 5 | 6 | from PathPlanning.LQRPlanner import LQRplanner as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_bezier_path.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathPlanning/BezierPath/") 5 | 6 | from PathPlanning.BezierPath import bezier_path as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | m.main2() 17 | -------------------------------------------------------------------------------- /tests/test_lqr_steer_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathTracking/lqr_steer_control/") 5 | 6 | from PathTracking.lqr_steer_control import lqr_steer_control as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_stanley_controller.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathTracking/stanley_controller/") 5 | 6 | from PathTracking.stanley_controller import stanley_controller as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_rear_wheel_feedback.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathTracking/rear_wheel_feedback/") 5 | 6 | from PathTracking.rear_wheel_feedback import rear_wheel_feedback as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_lqr_speed_steer_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./PathTracking/lqr_speed_steer_control/") 5 | 6 | from PathTracking.lqr_speed_steer_control import lqr_speed_steer_control as m 7 | 8 | print(__file__) 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_n_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./ArmNavigation/n_joint_arm_to_point_control/") 5 | 6 | from ArmNavigation.n_joint_arm_to_point_control import n_joint_arm_to_point_control as m 7 | print(__file__) 8 | 9 | 10 | class Test(TestCase): 11 | 12 | def test1(self): 13 | m.show_animation = False 14 | m.animation() 15 | -------------------------------------------------------------------------------- /tests/test_cgmres_nmpc.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | if 'cvxpy' in sys.modules: # pragma: no cover 5 | sys.path.append("./PathTracking/cgmres_nmpc/") 6 | 7 | from PathTracking.cgmres_nmpc import cgmres_nmpc as m 8 | 9 | print(__file__) 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /tests/test_drone_3d_trajectory_following.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | sys.path.append("./AerialNavigation/drone_3d_trajectory_following/") 5 | 6 | from AerialNavigation.drone_3d_trajectory_following import drone_3d_trajectory_following as m 7 | print(__file__) 8 | 9 | 10 | class Test(TestCase): 11 | 12 | def test1(self): 13 | m.show_animation = False 14 | m.main() 15 | -------------------------------------------------------------------------------- /tests/test_a_star.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(__file__) + "/../") 5 | try: 6 | from PathPlanning.AStar import a_star as m 7 | except: 8 | raise 9 | 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | 17 | 18 | if __name__ == '__main__': # pragma: no cover 19 | test = Test() 20 | test.test1() 21 | -------------------------------------------------------------------------------- /tests/test_rocket_powered_landing.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | 5 | if 'cvxpy' in sys.modules: # pragma: no cover 6 | sys.path.append("./AerialNavigation/rocket_powered_landing/") 7 | 8 | from AerialNavigation.rocket_powered_landing import rocket_powered_landing as m 9 | print(__file__) 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | -------------------------------------------------------------------------------- /docs/modules/aerial_navigation.rst: -------------------------------------------------------------------------------- 1 | .. _aerial_navigation: 2 | 3 | Aerial Navigation 4 | ================= 5 | 6 | Drone 3d trajectory following 7 | ----------------------------- 8 | 9 | This is a 3d trajectory following simulation for a quadrotor. 10 | 11 | |3| 12 | 13 | rocket powered landing 14 | ----------------------------- 15 | 16 | .. include:: rocket_powered_landing.rst 17 | 18 | .. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/AerialNavigation/drone_3d_trajectory_following/animation.gif 19 | -------------------------------------------------------------------------------- /tests/test_fast_slam1.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 5 | try: 6 | from SLAM.FastSLAM1 import fast_slam1 as m 7 | except: 8 | raise 9 | 10 | 11 | print(__file__) 12 | 13 | 14 | class Test(TestCase): 15 | 16 | def test1(self): 17 | m.show_animation = False 18 | m.SIM_TIME = 3.0 19 | m.main() 20 | 21 | 22 | if __name__ == '__main__': # pragma: no cover 23 | test = Test() 24 | test.test1() 25 | -------------------------------------------------------------------------------- /tests/test_fast_slam2.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 5 | try: 6 | from SLAM.FastSLAM2 import fast_slam2 as m 7 | except: 8 | raise 9 | 10 | 11 | print(__file__) 12 | 13 | 14 | class Test(TestCase): 15 | 16 | def test1(self): 17 | m.show_animation = False 18 | m.SIM_TIME = 3.0 19 | m.main() 20 | 21 | 22 | if __name__ == '__main__': # pragma: no cover 23 | test = Test() 24 | test.test1() 25 | -------------------------------------------------------------------------------- /tests/test_batch_informed_rrt_star.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(__file__) + "/../") 5 | try: 6 | from PathPlanning.BatchInformedRRTStar import batch_informed_rrtstar as m 7 | except: 8 | raise 9 | 10 | print(__file__) 11 | 12 | 13 | class Test(TestCase): 14 | 15 | def test1(self): 16 | m.show_animation = False 17 | m.main(maxIter=10) 18 | 19 | 20 | if __name__ == '__main__': # pragma: no cover 21 | test = Test() 22 | test.test1() 23 | -------------------------------------------------------------------------------- /tests/test_dynamic_window_approach.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(__file__) + "/../") 6 | try: 7 | from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m 8 | except: 9 | raise 10 | 11 | print(__file__) 12 | 13 | 14 | class Test(TestCase): 15 | 16 | def test1(self): 17 | m.show_animation = False 18 | m.main(gx=1.0, gy=1.0) 19 | 20 | 21 | if __name__ == '__main__': # pragma: no cover 22 | test = Test() 23 | test.test1() 24 | -------------------------------------------------------------------------------- /tests/test_histogram_filter.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 6 | try: 7 | from Localization.histogram_filter import histogram_filter as m 8 | except: 9 | raise 10 | 11 | 12 | print(__file__) 13 | 14 | 15 | class Test(TestCase): 16 | 17 | def test1(self): 18 | m.show_animation = False 19 | m.SIM_TIME = 1.0 20 | m.main() 21 | 22 | 23 | if __name__ == '__main__': 24 | test = Test() 25 | test.test1() 26 | -------------------------------------------------------------------------------- /tests/test_rrt_star.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 6 | "/../PathPlanning/RRTStar/") 7 | 8 | try: 9 | import rrt_star as m 10 | except ImportError: 11 | raise 12 | 13 | 14 | print(__file__) 15 | 16 | 17 | class Test(TestCase): 18 | 19 | def test1(self): 20 | m.show_animation = False 21 | m.main() 22 | 23 | 24 | if __name__ == '__main__': # pragma: no cover 25 | test = Test() 26 | test.test1() 27 | -------------------------------------------------------------------------------- /tests/test_rrt_star_dubins.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/RRTStarDubins/") 7 | 8 | try: 9 | import rrt_star_dubins as m 10 | except: 11 | raise 12 | 13 | print(__file__) 14 | 15 | 16 | class Test(TestCase): 17 | 18 | def test1(self): 19 | m.show_animation = False 20 | m.main() 21 | 22 | 23 | if __name__ == '__main__': # pragma: no cover 24 | test = Test() 25 | test.test1() 26 | -------------------------------------------------------------------------------- /tests/test_graph_based_slam.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 5 | try: 6 | from SLAM.GraphBasedSLAM import graph_based_slam as m 7 | except: 8 | raise 9 | 10 | 11 | print(__file__) 12 | 13 | 14 | class Test(TestCase): 15 | 16 | def test1(self): 17 | m.show_animation = False 18 | m.SIM_TIME = 20.0 19 | m.main() 20 | 21 | 22 | if __name__ == '__main__': # pragma: no cover 23 | test = Test() 24 | test.test1() 25 | -------------------------------------------------------------------------------- /tests/test_model_predictive_speed_and_steer_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | if 'cvxpy' in sys.modules: # pragma: no cover 5 | sys.path.append("./PathTracking/model_predictive_speed_and_steer_control/") 6 | 7 | from PathTracking.model_predictive_speed_and_steer_control import model_predictive_speed_and_steer_control as m 8 | 9 | print(__file__) 10 | 11 | class Test(TestCase): 12 | 13 | def test1(self): 14 | m.show_animation = False 15 | m.main() 16 | m.main2() 17 | -------------------------------------------------------------------------------- /docs/modules/rrt_star.rst: -------------------------------------------------------------------------------- 1 | 2 | Simulation 3 | ^^^^^^^^^^ 4 | 5 | .. code-block:: ipython3 6 | 7 | from IPython.display import Image 8 | Image(filename="Figure_1.png",width=600) 9 | 10 | 11 | 12 | 13 | .. image:: rrt_star_files/rrt_star_1_0.png 14 | :width: 600px 15 | 16 | 17 | 18 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathPlanning/RRTstar/animation.gif 19 | :alt: gif 20 | 21 | gif 22 | 23 | Ref 24 | ^^^ 25 | 26 | - `Sampling-based Algorithms for Optimal Motion 27 | Planning `__ 28 | -------------------------------------------------------------------------------- /tests/test_lqr_rrt_star.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/LQRRRTStar/") 7 | try: 8 | from PathPlanning.LQRRRTStar import lqr_rrt_star as m 9 | except: 10 | raise 11 | 12 | print(__file__) 13 | 14 | 15 | class Test(TestCase): 16 | 17 | def test1(self): 18 | m.show_animation = False 19 | m.main(maxIter=5) 20 | 21 | 22 | if __name__ == '__main__': # pragma: no cover 23 | test = Test() 24 | test.test1() 25 | -------------------------------------------------------------------------------- /tests/test_frenet_optimal_trajectory.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | import os 5 | sys.path.append("./PathPlanning/FrenetOptimalTrajectory/") 6 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 7 | try: 8 | from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m 9 | except: 10 | raise 11 | 12 | 13 | print(__file__) 14 | 15 | 16 | class Test(TestCase): 17 | 18 | def test1(self): 19 | m.show_animation = False 20 | m.SIM_LOOP = 5 21 | m.main() 22 | 23 | 24 | if __name__ == '__main__': # pragma: no cover 25 | test = Test() 26 | test.test1() 27 | -------------------------------------------------------------------------------- /tests/test_informed_rrt_star.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/InformedRRTStar/") 7 | try: 8 | from PathPlanning.InformedRRTStar import informed_rrt_star as m 9 | except: 10 | raise 11 | 12 | print(__file__) 13 | 14 | 15 | class Test(TestCase): 16 | 17 | def test1(self): 18 | m.show_animation = False 19 | m.main() 20 | 21 | 22 | if __name__ == '__main__': # pragma: no cover 23 | test = Test() 24 | test.test1() 25 | -------------------------------------------------------------------------------- /tests/test_rrt_dubins.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 5 | "/../PathPlanning/RRTDubins/") 6 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 7 | "/../PathPlanning/DubinsPath/") 8 | 9 | try: 10 | import rrt_dubins as m 11 | except: 12 | raise 13 | 14 | 15 | print(__file__) 16 | 17 | 18 | class Test(TestCase): 19 | 20 | def test1(self): 21 | m.show_animation = False 22 | m.main() 23 | 24 | 25 | if __name__ == '__main__': # pragma: no cover 26 | test = Test() 27 | test.test1() 28 | -------------------------------------------------------------------------------- /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) -------------------------------------------------------------------------------- /tests/test_rrt_star_reeds_shepp.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/RRTStarReedsShepp/") 7 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 8 | + "/../PathPlanning/ReedsSheppPath/") 9 | 10 | try: 11 | import rrt_star_reeds_shepp as m 12 | except: 13 | raise 14 | 15 | 16 | print(__file__) 17 | 18 | 19 | class Test(TestCase): 20 | 21 | def test1(self): 22 | m.show_animation = False 23 | m.main(max_iter=5) 24 | 25 | 26 | if __name__ == '__main__': # pragma: no cover 27 | test = Test() 28 | test.test1() 29 | -------------------------------------------------------------------------------- /tests/test_rrt.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 6 | try: 7 | from PathPlanning.RRT import rrt as m 8 | from PathPlanning.RRT import rrt_with_pathsmoothing as m1 9 | except ImportError: 10 | raise 11 | 12 | 13 | print(__file__) 14 | 15 | 16 | class Test(TestCase): 17 | 18 | def test1(self): 19 | m.show_animation = False 20 | m.main(gx=1.0, gy=1.0) 21 | 22 | def test2(self): 23 | m1.show_animation = False 24 | m1.main() 25 | 26 | 27 | if __name__ == '__main__': # pragma: no cover 28 | test = Test() 29 | test.test1() 30 | test.test2() 31 | -------------------------------------------------------------------------------- /SLAM/PoseOptimizationSLAM3D/data_downloader.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Data down loader for pose optimization 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | 9 | import subprocess 10 | 11 | 12 | def main(): 13 | print("start!!") 14 | 15 | cmd = "wget https://www.dropbox.com/s/zu23p8d522qccor/parking-garage.g2o?dl=0 -O parking-garage.g2o -nc" 16 | subprocess.call(cmd, shell=True) 17 | cmd = "wget http://www.furo.org/irie/datasets/torus3d_guess.g2o -nc" 18 | subprocess.call(cmd, shell=True) 19 | cmd = "wget http://www.furo.org/irie/datasets/sphere2200_guess.g2o -nc" 20 | subprocess.call(cmd, shell=True) 21 | 22 | print("done!!") 23 | 24 | 25 | if __name__ == '__main__': 26 | main() 27 | -------------------------------------------------------------------------------- /tests/test_closed_loop_rrt_star_car.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../") 6 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 7 | "/../PathPlanning/ClosedLoopRRTStar/") 8 | try: 9 | from PathPlanning.ClosedLoopRRTStar import closed_loop_rrt_star_car as m 10 | except: 11 | raise 12 | 13 | 14 | print(__file__) 15 | 16 | 17 | class Test(TestCase): 18 | 19 | def test1(self): 20 | m.show_animation = False 21 | m.main(gx=1.0, gy=0.0, gyaw=0.0, max_iter=5) 22 | 23 | 24 | if __name__ == '__main__': # pragma: no cover 25 | test = Test() 26 | test.test1() 27 | -------------------------------------------------------------------------------- /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 | ### Jupyter notebook integration 28 | 29 | When you want to generate rst files from each jupyter notebooks, 30 | 31 | you can use 32 | 33 | ``` 34 | cd path/to/docs 35 | python jupyternotebook2rst.py 36 | ``` 37 | 38 | -------------------------------------------------------------------------------- /SLAM/PoseOptimizationSLAM2D/data_downloader.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Data down loader for pose optimization 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | 9 | 10 | import subprocess 11 | def main(): 12 | print("start!!") 13 | 14 | cmd = "wget https://www.dropbox.com/s/vcz8cag7bo0zlaj/input_INTEL_g2o.g2o?dl=0 -O intel.g2o -nc" 15 | subprocess.call(cmd, shell=True) 16 | 17 | cmd = "wget https://www.dropbox.com/s/d8fcn1jg1mebx8f/input_MITb_g2o.g2o?dl=0 -O mit_killian.g2o -nc" 18 | 19 | subprocess.call(cmd, shell=True) 20 | cmd = "wget https://www.dropbox.com/s/gmdzo74b3tzvbrw/input_M3500_g2o.g2o?dl=0 -O manhattan3500.g2o -nc" 21 | subprocess.call(cmd, shell=True) 22 | 23 | print("done!!") 24 | 25 | 26 | if __name__ == '__main__': 27 | main() 28 | 29 | -------------------------------------------------------------------------------- /tests/test_state_lattice_planner.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | 5 | import os 6 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 7 | "/../PathPlanning/ModelPredictiveTrajectoryGenerator/") 8 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + 9 | "/../PathPlanning/StateLatticePlanner/") 10 | 11 | try: 12 | import state_lattice_planner as m 13 | import model_predictive_trajectory_generator as m2 14 | except: 15 | raise 16 | 17 | print(__file__) 18 | 19 | 20 | class Test(TestCase): 21 | 22 | def test1(self): 23 | m.show_animation = False 24 | m2.show_animation = False 25 | m.main() 26 | 27 | 28 | if __name__ == '__main__': # pragma: no cover 29 | test = Test() 30 | test.test1() 31 | -------------------------------------------------------------------------------- /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.6.x or higher. 22 | 23 | We will not accept a PR for Python 2.x. 24 | -------------------------------------------------------------------------------- /Introduction/introduction.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Introduction" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [] 14 | }, 15 | { 16 | "cell_type": "markdown", 17 | "metadata": {}, 18 | "source": [ 19 | "## Ref" 20 | ] 21 | } 22 | ], 23 | "metadata": { 24 | "kernelspec": { 25 | "display_name": "Python 3", 26 | "language": "python", 27 | "name": "python3" 28 | }, 29 | "language_info": { 30 | "codemirror_mode": { 31 | "name": "ipython", 32 | "version": 3 33 | }, 34 | "file_extension": ".py", 35 | "mimetype": "text/x-python", 36 | "name": "python", 37 | "nbconvert_exporter": "python", 38 | "pygments_lexer": "ipython3", 39 | "version": "3.6.7" 40 | } 41 | }, 42 | "nbformat": 4, 43 | "nbformat_minor": 2 44 | } 45 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | 3 | matrix: 4 | include: 5 | - os: linux 6 | 7 | python: 8 | - 3.6 9 | 10 | before_install: 11 | - sudo apt-get update 12 | - wget https://repo.continuum.io/miniconda/Miniconda3-latest-Linux-x86_64.sh -O miniconda.sh 13 | - chmod +x miniconda.sh 14 | - bash miniconda.sh -b -p $HOME/miniconda 15 | - export PATH="$HOME/miniconda/bin:$PATH" 16 | - hash -r 17 | - conda config --set always_yes yes --set changeps1 no 18 | - conda update -q conda 19 | # Useful for debugging any issues with conda 20 | - conda info -a 21 | - conda install python==3.6.8 22 | 23 | install: 24 | - conda install numpy==1.15 25 | - conda install scipy 26 | - conda install matplotlib 27 | - conda install pandas 28 | - pip install cvxpy 29 | - conda install coveralls 30 | 31 | script: 32 | - python --version 33 | - ./runtests.sh 34 | 35 | after_success: 36 | - coveralls 37 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /SLAM/PoseOptimizationSLAM3D/README.md: -------------------------------------------------------------------------------- 1 | # PoseOptimizationSLAM3D 2 | 3D (x, y, z, qw, qx, qy, qz) pose optimization SLAM 3 | 4 | ## How to use 5 | 1. Download data 6 | 7 | ~~~ 8 | python data_downloader.py 9 | ~~~ 10 | 11 | 2. run SLAM 12 | 13 | ~~~ 14 | python pose_optimization_slam_3d.py 15 | ~~~ 16 | 17 | ## Reference 18 | - [A Compact and Portable Implementation of Graph\-based SLAM](https://www.researchgate.net/publication/321287640_A_Compact_and_Portable_Implementation_of_Graph-based_SLAM) 19 | 20 | - [GitHub \- furo\-org/p2o: Single header 2D/3D graph\-based SLAM library](https://github.com/furo-org/p2o) 21 | 22 | - [GitHub \- AtsushiSakai/PythonRobotics 23 | /SLAM/PoseOptimizationSLAM](https://github.com/AtsushiSakai/PythonRobotics/tree/master/SLAM/PoseOptimizationSLAM) 24 | 25 | - [リー代数による3次元ポーズ調整\(Pose Adjustment\)\[PythonRobotics\]\[SLAM\] \- Qiita](https://qiita.com/saitosasaki/items/a0540cba994f08bf5e16#comment-3e6588e6b096cc2567d8) 26 | -------------------------------------------------------------------------------- /docs/modules/arm_navigation.rst: -------------------------------------------------------------------------------- 1 | .. _arm_navigation: 2 | 3 | Arm Navigation 4 | ============== 5 | 6 | .. include:: Planar_Two_Link_IK.rst 7 | 8 | 9 | N joint arm to point control 10 | ---------------------------- 11 | 12 | N joint arm to a point control simulation. 13 | 14 | This is a interactive simulation. 15 | 16 | You can set the goal position of the end effector with left-click on the 17 | ploting area. 18 | 19 | |n_joints_arm| 20 | 21 | In this simulation N = 10, however, you can change it. 22 | 23 | Arm navigation with obstacle avoidance 24 | -------------------------------------- 25 | 26 | Arm navigation with obstacle avoidance simulation. 27 | 28 | |obstacle| 29 | 30 | .. |n_joints_arm| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/n_joint_arm_to_point_control/animation.gif 31 | .. |obstacle| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/ArmNavigation/arm_obstacle_navigation/animation.gif 32 | -------------------------------------------------------------------------------- /tests/test_dubins_path_planning.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import numpy as np 4 | 5 | from PathPlanning.DubinsPath import dubins_path_planning 6 | 7 | 8 | class Test(TestCase): 9 | 10 | def test1(self): 11 | start_x = 1.0 # [m] 12 | start_y = 1.0 # [m] 13 | start_yaw = np.deg2rad(45.0) # [rad] 14 | 15 | end_x = -3.0 # [m] 16 | end_y = -3.0 # [m] 17 | end_yaw = np.deg2rad(-45.0) # [rad] 18 | 19 | curvature = 1.0 20 | 21 | px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( 22 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 23 | 24 | assert (abs(px[-1] - end_x) <= 0.5) 25 | assert (abs(py[-1] - end_y) <= 0.5) 26 | assert(abs(pyaw[-1] - end_yaw) <= 0.1) 27 | 28 | def test2(self): 29 | dubins_path_planning.show_animation = False 30 | dubins_path_planning.main() 31 | 32 | def test3(self): 33 | dubins_path_planning.show_animation = False 34 | dubins_path_planning.test() 35 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Atsushi Sakai 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /.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 | -------------------------------------------------------------------------------- /tests/test_grid_map_lib.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | import unittest 4 | 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) + "/../Mapping/grid_map_lib") 6 | try: 7 | from grid_map_lib import GridMap 8 | except ImportError: 9 | raise 10 | 11 | 12 | class MyTestCase(unittest.TestCase): 13 | 14 | def test_position_set(self): 15 | grid_map = GridMap(100, 120, 0.5, 10.0, -0.5) 16 | 17 | grid_map.set_value_from_xy_pos(10.1, -1.1, 1.0) 18 | grid_map.set_value_from_xy_pos(10.1, -0.1, 1.0) 19 | grid_map.set_value_from_xy_pos(10.1, 1.1, 1.0) 20 | grid_map.set_value_from_xy_pos(11.1, 0.1, 1.0) 21 | grid_map.set_value_from_xy_pos(10.1, 0.1, 1.0) 22 | grid_map.set_value_from_xy_pos(9.1, 0.1, 1.0) 23 | 24 | self.assertEqual(True, True) 25 | 26 | def test_polygon_set(self): 27 | ox = [0.0, 20.0, 50.0, 100.0, 130.0, 40.0] 28 | oy = [0.0, -20.0, 0.0, 30.0, 60.0, 80.0] 29 | 30 | grid_map = GridMap(600, 290, 0.7, 60.0, 30.5) 31 | 32 | grid_map.set_value_from_polygon(ox, oy, 1.0, inside=False) 33 | 34 | self.assertEqual(True, True) 35 | 36 | 37 | if __name__ == '__main__': 38 | unittest.main() 39 | -------------------------------------------------------------------------------- /docs/modules/mapping.rst: -------------------------------------------------------------------------------- 1 | .. _mapping: 2 | 3 | Mapping 4 | ======= 5 | 6 | Gaussian grid map 7 | ----------------- 8 | 9 | This is a 2D Gaussian grid mapping example. 10 | 11 | |2| 12 | 13 | Ray casting grid map 14 | -------------------- 15 | 16 | This is a 2D ray casting grid mapping example. 17 | 18 | |3| 19 | 20 | k-means object clustering 21 | ------------------------- 22 | 23 | This is a 2D object clustering with k-means algorithm. 24 | 25 | |4| 26 | 27 | Object shape recognition using circle fitting 28 | --------------------------------------------- 29 | 30 | This is an object shape recognition using circle fitting. 31 | 32 | |5| 33 | 34 | The blue circle is the true object shape. 35 | 36 | The red crosses are observations from a ranging sensor. 37 | 38 | The red circle is the estimated object shape using circle fitting. 39 | 40 | .. |2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/gaussian_grid_map/animation.gif 41 | .. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/raycasting_grid_map/animation.gif 42 | .. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/kmeans_clustering/animation.gif 43 | .. |5| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Mapping/circle_fitting/animation.gif 44 | -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/bspline_path.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path Plannting with B-Spline 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import scipy.interpolate as si 12 | 13 | # parameter 14 | N = 3 # B Spline order 15 | 16 | 17 | def bspline_planning(x, y, sn): 18 | t = range(len(x)) 19 | x_tup = si.splrep(t, x, k=N) 20 | y_tup = si.splrep(t, y, k=N) 21 | 22 | x_list = list(x_tup) 23 | xl = x.tolist() 24 | x_list[1] = xl + [0.0, 0.0, 0.0, 0.0] 25 | 26 | y_list = list(y_tup) 27 | yl = y.tolist() 28 | y_list[1] = yl + [0.0, 0.0, 0.0, 0.0] 29 | 30 | ipl_t = np.linspace(0.0, len(x) - 1, sn) 31 | rx = si.splev(ipl_t, x_list) 32 | ry = si.splev(ipl_t, y_list) 33 | 34 | return rx, ry 35 | 36 | 37 | def main(): 38 | print(__file__ + " start!!") 39 | # way points 40 | x = np.array([-1.0, 3.0, 4.0, 2.0, 1.0]) 41 | y = np.array([0.0, -3.0, 1.0, 1.0, 3.0]) 42 | sn = 100 # sampling number 43 | 44 | rx, ry = bspline_planning(x, y, sn) 45 | 46 | # show results 47 | plt.plot(x, y, '-og', label="Waypoints") 48 | plt.plot(rx, ry, '-r', label="B-Spline path") 49 | plt.grid(True) 50 | plt.legend() 51 | plt.axis("equal") 52 | plt.show() 53 | 54 | 55 | if __name__ == '__main__': 56 | main() 57 | -------------------------------------------------------------------------------- /docs/getting_started.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 | 25 | Requirements 26 | ------------- 27 | 28 | - Python 3.6.x 29 | - numpy 30 | - scipy 31 | - matplotlib 32 | - pandas 33 | - `cvxpy`_ 34 | 35 | .. _cvxpy: http://www.cvxpy.org/en/latest/ 36 | 37 | 38 | How to use 39 | ---------- 40 | 41 | 1. Install the required libraries. You can use environment.yml with 42 | conda command. 43 | 44 | 2. Clone this repo. 45 | 46 | 3. Execute python script in each directory. 47 | 48 | 4. Add star to this repo if you like it 😃. 49 | 50 | -------------------------------------------------------------------------------- /docs/index.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, especially for autonomous navigation. 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: Guide 35 | 36 | getting_started 37 | modules/introduction 38 | modules/localization 39 | modules/mapping 40 | modules/slam 41 | modules/path_planning 42 | modules/path_tracking 43 | modules/arm_navigation 44 | modules/aerial_navigation 45 | modules/appendix 46 | 47 | 48 | Indices and tables 49 | ================== 50 | 51 | * :ref:`genindex` 52 | * :ref:`modindex` 53 | * :ref:`search` 54 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | environment: 2 | global: 3 | # SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the 4 | # /E:ON and /V:ON options are not enabled in the batch script intepreter 5 | # See: http://stackoverflow.com/a/13751649/163740 6 | CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd" 7 | 8 | matrix: 9 | - MINICONDA: C:\Miniconda36-x64 10 | 11 | 12 | 13 | init: 14 | - "ECHO %MINICONDA% %PYTHON_VERSION% %PYTHON_ARCH%" 15 | 16 | install: 17 | # If there is a newer build queued for the same PR, cancel this one. 18 | # The AppVeyor 'rollout builds' option is supposed to serve the same 19 | # purpose but it is problematic because it tends to cancel builds pushed 20 | # directly to master instead of just PR builds (or the converse). 21 | # credits: JuliaLang developers. 22 | - ps: if ($env:APPVEYOR_PULL_REQUEST_NUMBER -and $env:APPVEYOR_BUILD_NUMBER -ne ((Invoke-RestMethod ` 23 | https://ci.appveyor.com/api/projects/$env:APPVEYOR_ACCOUNT_NAME/$env:APPVEYOR_PROJECT_SLUG/history?recordsNumber=50).builds | ` 24 | Where-Object pullRequestId -eq $env:APPVEYOR_PULL_REQUEST_NUMBER)[0].buildNumber) { ` 25 | throw "There are newer queued builds for this pull request, failing early." } 26 | - ECHO "Filesystem root:" 27 | - ps: "ls \"C:/\"" 28 | 29 | - ECHO "Installed SDKs:" 30 | - ps: "ls \"C:/Program Files/Microsoft SDKs/Windows\"" 31 | 32 | # Prepend newly installed Python to the PATH of this build (this cannot be 33 | # done from inside the powershell script as it would require to restart 34 | # the parent CMD process). 35 | - "SET PATH=%MINICONDA%;%MINICONDA%\\Scripts;%PATH%" 36 | - conda config --set always_yes yes --set changeps1 no 37 | - conda update -q conda 38 | - conda info -a 39 | - conda env create -f C:\\projects\pythonrobotics\environment.yml 40 | - activate python_robotics 41 | 42 | # Check that we have the expected version and architecture for Python 43 | - "python --version" 44 | - "python -c \"import struct; print(struct.calcsize('P') * 8)\"" 45 | 46 | build: off 47 | 48 | test_script: 49 | - "python -Wignore -m unittest discover tests" 50 | -------------------------------------------------------------------------------- /docs/jupyternotebook2rst.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Jupyter notebook converter to rst file 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | import subprocess 9 | import os.path 10 | import os 11 | import glob 12 | 13 | 14 | NOTEBOOK_DIR = "../" 15 | 16 | 17 | def get_notebook_path_list(ndir): 18 | path = glob.glob(ndir + "**/*.ipynb", recursive=True) 19 | return path 20 | 21 | 22 | def convert_rst(rstpath): 23 | 24 | with open(rstpath, "r") as bfile: 25 | filedata = bfile.read() 26 | 27 | # convert from code directive to code-block 28 | # because showing code in Sphinx 29 | before = ".. code:: ipython3" 30 | after = ".. code-block:: ipython3" 31 | filedata = filedata.replace(before, after) 32 | 33 | with open(rstpath, "w") as afile: 34 | afile.write(filedata) 35 | 36 | 37 | def generate_rst(npath): 38 | print("====Start generating rst======") 39 | 40 | # generate dir 41 | dirpath = os.path.dirname(npath) 42 | # print(dirpath) 43 | 44 | rstpath = os.path.abspath("./modules/" + npath[3:-5] + "rst") 45 | # print(rstpath) 46 | 47 | basename = os.path.basename(rstpath) 48 | 49 | cmd = "jupyter nbconvert --to rst " 50 | cmd += npath 51 | print(cmd) 52 | subprocess.call(cmd, shell=True) 53 | 54 | rstpath = dirpath + "/" + basename 55 | convert_rst(rstpath) 56 | 57 | # clean up old files 58 | cmd = "rm -rf " 59 | cmd += "./modules/" 60 | cmd += basename[:-4] 61 | cmd += "*" 62 | # print(cmd) 63 | subprocess.call(cmd, shell=True) 64 | 65 | # move files to module dir 66 | cmd = "mv " 67 | cmd += dirpath 68 | cmd += "/*.rst ./modules/" 69 | print(cmd) 70 | subprocess.call(cmd, shell=True) 71 | 72 | cmd = "mv " 73 | cmd += dirpath 74 | cmd += "/*_files ./modules/" 75 | print(cmd) 76 | subprocess.call(cmd, shell=True) 77 | 78 | 79 | def main(): 80 | print("start!!") 81 | 82 | notebook_path_list = get_notebook_path_list(NOTEBOOK_DIR) 83 | # print(notebook_path_list) 84 | 85 | for npath in notebook_path_list: 86 | if "template" not in npath: 87 | generate_rst(npath) 88 | 89 | print("done!!") 90 | 91 | 92 | if __name__ == '__main__': 93 | main() 94 | -------------------------------------------------------------------------------- /docs/modules/localization.rst: -------------------------------------------------------------------------------- 1 | .. _localization: 2 | 3 | Localization 4 | ============ 5 | 6 | .. include:: extended_kalman_filter_localization.rst 7 | 8 | 9 | Unscented Kalman Filter localization 10 | ------------------------------------ 11 | 12 | |2| 13 | 14 | This is a sensor fusion localization with Unscented Kalman Filter(UKF). 15 | 16 | The lines and points are same meaning of the EKF simulation. 17 | 18 | Ref: 19 | 20 | - `Discriminatively Trained Unscented Kalman Filter for Mobile Robot 21 | Localization`_ 22 | 23 | Particle filter localization 24 | ---------------------------- 25 | 26 | |3| 27 | 28 | This is a sensor fusion localization with Particle Filter(PF). 29 | 30 | The blue line is true trajectory, the black line is dead reckoning 31 | trajectory, 32 | 33 | and the red line is estimated trajectory with PF. 34 | 35 | It is assumed that the robot can measure a distance from landmarks 36 | (RFID). 37 | 38 | This measurements are used for PF localization. 39 | 40 | Ref: 41 | 42 | - `PROBABILISTIC ROBOTICS`_ 43 | 44 | Histogram filter localization 45 | ----------------------------- 46 | 47 | |4| 48 | 49 | This is a 2D localization example with Histogram filter. 50 | 51 | The red cross is true position, black points are RFID positions. 52 | 53 | The blue grid shows a position probability of histogram filter. 54 | 55 | In this simulation, x,y are unknown, yaw is known. 56 | 57 | The filter integrates speed input and range observations from RFID for 58 | localization. 59 | 60 | Initial position is not needed. 61 | 62 | Ref: 63 | 64 | - `PROBABILISTIC ROBOTICS`_ 65 | 66 | .. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ 67 | .. _Discriminatively Trained Unscented Kalman Filter for Mobile Robot Localization: https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization 68 | 69 | .. |2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/unscented_kalman_filter/animation.gif 70 | .. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/particle_filter/animation.gif 71 | .. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/histogram_filter/animation.gif 72 | -------------------------------------------------------------------------------- /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) -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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.sqrt((iox - x)**2 + (ioy - y)**2) 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 | draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) 77 | plt.plot(ox, oy, "xr") 78 | plt.plot(0.0, 0.0, "ob") 79 | plt.pause(1.0) 80 | 81 | 82 | if __name__ == '__main__': 83 | main() 84 | -------------------------------------------------------------------------------- /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 | 55 | for i in range(self.n_links + 1): 56 | if i is not self.n_links: 57 | plt.plot([self.points[i][0], self.points[i + 1][0]], 58 | [self.points[i][1], self.points[i + 1][1]], 'r-') 59 | plt.plot(self.points[i][0], self.points[i][1], 'ko') 60 | 61 | plt.plot(self.goal[0], self.goal[1], 'gx') 62 | 63 | plt.plot([self.end_effector[0], self.goal[0]], [ 64 | self.end_effector[1], self.goal[1]], 'g--') 65 | 66 | plt.xlim([-self.lim, self.lim]) 67 | plt.ylim([-self.lim, self.lim]) 68 | plt.draw() 69 | plt.pause(0.0001) 70 | 71 | def click(self, event): 72 | self.goal = np.array([event.xdata, event.ydata]).T 73 | self.plot() 74 | -------------------------------------------------------------------------------- /docs/modules/slam.rst: -------------------------------------------------------------------------------- 1 | .. _slam: 2 | 3 | SLAM 4 | ==== 5 | 6 | Simultaneous Localization and Mapping(SLAM) examples 7 | 8 | .. _iterative-closest-point-(icp)-matching: 9 | 10 | Iterative Closest Point (ICP) Matching 11 | -------------------------------------- 12 | 13 | This is a 2D ICP matching example with singular value decomposition. 14 | 15 | It can calculate a rotation matrix and a translation vector between 16 | points to points. 17 | 18 | |3| 19 | 20 | Ref: 21 | 22 | - `Introduction to Mobile Robotics: Iterative Closest Point Algorithm`_ 23 | 24 | EKF SLAM 25 | -------- 26 | 27 | This is an Extended Kalman Filter based SLAM example. 28 | 29 | The blue line is ground truth, the black line is dead reckoning, the red 30 | line is the estimated trajectory with EKF SLAM. 31 | 32 | The green crosses are estimated landmarks. 33 | 34 | |4| 35 | 36 | Ref: 37 | 38 | - `PROBABILISTIC ROBOTICS`_ 39 | 40 | .. include:: FastSLAM1.rst 41 | 42 | |5| 43 | 44 | Ref: 45 | 46 | - `PROBABILISTIC ROBOTICS`_ 47 | 48 | - `SLAM simulations by Tim Bailey`_ 49 | 50 | FastSLAM 2.0 51 | ------------ 52 | 53 | This is a feature based SLAM example using FastSLAM 2.0. 54 | 55 | The animation has the same meanings as one of FastSLAM 1.0. 56 | 57 | |6| 58 | 59 | Ref: 60 | 61 | - `PROBABILISTIC ROBOTICS`_ 62 | 63 | - `SLAM simulations by Tim Bailey`_ 64 | 65 | Graph based SLAM 66 | ---------------- 67 | 68 | This is a graph based SLAM example. 69 | 70 | The blue line is ground truth. 71 | 72 | The black line is dead reckoning. 73 | 74 | The red line is the estimated trajectory with Graph based SLAM. 75 | 76 | The black stars are landmarks for graph edge generation. 77 | 78 | |7| 79 | 80 | Ref: 81 | 82 | - `A Tutorial on Graph-Based SLAM`_ 83 | 84 | .. _`Introduction to Mobile Robotics: Iterative Closest Point Algorithm`: https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf 85 | .. _PROBABILISTIC ROBOTICS: http://www.probabilistic-robotics.org/ 86 | .. _SLAM simulations by Tim Bailey: http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm 87 | .. _A Tutorial on Graph-Based SLAM: http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf 88 | 89 | .. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/iterative_closest_point/animation.gif 90 | .. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/EKFSLAM/animation.gif 91 | .. |5| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM1/animation.gif 92 | .. |6| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/FastSLAM2/animation.gif 93 | .. |7| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/SLAM/GraphBasedSLAM/animation.gif 94 | -------------------------------------------------------------------------------- /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 | 12 | class Quadrotor(): 13 | def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): 14 | self.p1 = np.array([size / 2, 0, 0, 1]).T 15 | self.p2 = np.array([-size / 2, 0, 0, 1]).T 16 | self.p3 = np.array([0, size / 2, 0, 1]).T 17 | self.p4 = np.array([0, -size / 2, 0, 1]).T 18 | 19 | self.x_data = [] 20 | self.y_data = [] 21 | self.z_data = [] 22 | self.show_animation = show_animation 23 | 24 | if self.show_animation: 25 | plt.ion() 26 | fig = plt.figure() 27 | self.ax = fig.add_subplot(111, projection='3d') 28 | 29 | self.update_pose(x, y, z, roll, pitch, yaw) 30 | 31 | def update_pose(self, x, y, z, roll, pitch, yaw): 32 | self.x = x 33 | self.y = y 34 | self.z = z 35 | self.roll = roll 36 | self.pitch = pitch 37 | self.yaw = yaw 38 | self.x_data.append(x) 39 | self.y_data.append(y) 40 | self.z_data.append(z) 41 | 42 | if self.show_animation: 43 | self.plot() 44 | 45 | def transformation_matrix(self): 46 | x = self.x 47 | y = self.y 48 | z = self.z 49 | roll = self.roll 50 | pitch = self.pitch 51 | yaw = self.yaw 52 | return np.array( 53 | [[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], 54 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) 55 | * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], 56 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] 57 | ]) 58 | 59 | def plot(self): # pragma: no cover 60 | T = self.transformation_matrix() 61 | 62 | p1_t = np.matmul(T, self.p1) 63 | p2_t = np.matmul(T, self.p2) 64 | p3_t = np.matmul(T, self.p3) 65 | p4_t = np.matmul(T, self.p4) 66 | 67 | plt.cla() 68 | 69 | self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]], 70 | [p1_t[1], p2_t[1], p3_t[1], p4_t[1]], 71 | [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.') 72 | 73 | self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], 74 | [p1_t[2], p2_t[2]], 'r-') 75 | self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], 76 | [p3_t[2], p4_t[2]], 'r-') 77 | 78 | self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:') 79 | 80 | plt.xlim(-5, 5) 81 | plt.ylim(-5, 5) 82 | self.ax.set_zlim(0, 10) 83 | 84 | plt.pause(0.001) -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/car.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Car model for Hybrid A* path planning 4 | 5 | author: Zheng Zh (@Zhengzh) 6 | 7 | """ 8 | 9 | 10 | import matplotlib.pyplot as plt 11 | from math import sqrt, cos, sin, tan, pi 12 | 13 | WB = 3. # rear to front wheel 14 | W = 2. # width of car 15 | LF = 3.3 # distance from rear to vehicle front end 16 | LB = 1.0 # distance from rear to vehicle back end 17 | MAX_STEER = 0.6 # [rad] maximum steering angle 18 | 19 | WBUBBLE_DIST = (LF - LB) / 2.0 20 | WBUBBLE_R = sqrt(((LF + LB) / 2.0)**2 + 1) 21 | 22 | # vehicle rectangle verticles 23 | VRX = [LF, LF, -LB, -LB, LF] 24 | VRY = [W / 2, -W / 2, -W / 2, W / 2, W / 2] 25 | 26 | 27 | def check_car_collision(xlist, ylist, yawlist, ox, oy, kdtree): 28 | for x, y, yaw in zip(xlist, ylist, yawlist): 29 | cx = x + WBUBBLE_DIST * cos(yaw) 30 | cy = y + WBUBBLE_DIST * sin(yaw) 31 | 32 | ids = kdtree.search_in_distance([cx, cy], WBUBBLE_R) 33 | 34 | if not ids: 35 | continue 36 | 37 | if not rectangle_check(x, y, yaw, 38 | [ox[i] for i in ids], [oy[i] for i in ids]): 39 | return False # collision 40 | 41 | return True # no collision 42 | 43 | 44 | def rectangle_check(x, y, yaw, ox, oy): 45 | # transform obstacles to base link frame 46 | c, s = cos(-yaw), sin(-yaw) 47 | for iox, ioy in zip(ox, oy): 48 | tx = iox - x 49 | ty = ioy - y 50 | rx = c * tx - s * ty 51 | ry = s * tx + c * ty 52 | 53 | if not (rx > LF or rx < -LB or ry > W / 2.0 or ry < -W / 2.0): 54 | return False # no collision 55 | 56 | return True # collision 57 | 58 | 59 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 60 | """Plot arrow.""" 61 | if not isinstance(x, float): 62 | for (ix, iy, iyaw) in zip(x, y, yaw): 63 | plot_arrow(ix, iy, iyaw) 64 | else: 65 | plt.arrow(x, y, length * cos(yaw), length * sin(yaw), 66 | fc=fc, ec=ec, head_width=width, head_length=width, alpha=0.4) 67 | # plt.plot(x, y) 68 | 69 | 70 | def plot_car(x, y, yaw): 71 | car_color = '-k' 72 | c, s = cos(yaw), sin(yaw) 73 | 74 | car_outline_x, car_outline_y = [], [] 75 | for rx, ry in zip(VRX, VRY): 76 | tx = c * rx - s * ry + x 77 | ty = s * rx + c * ry + y 78 | car_outline_x.append(tx) 79 | car_outline_y.append(ty) 80 | 81 | arrow_x, arrow_y, arrow_yaw = c * 1.5 + x, s * 1.5 + y, yaw 82 | plot_arrow(arrow_x, arrow_y, arrow_yaw) 83 | 84 | plt.plot(car_outline_x, car_outline_y, car_color) 85 | 86 | 87 | def pi_2_pi(angle): 88 | return (angle + pi) % (2 * pi) - pi 89 | 90 | 91 | def move(x, y, yaw, distance, steer, L=WB): 92 | x += distance * cos(yaw) 93 | y += distance * sin(yaw) 94 | yaw += pi_2_pi(distance * tan(steer) / L) # distance/2 95 | 96 | return x, y, yaw 97 | 98 | 99 | if __name__ == '__main__': 100 | x, y, yaw = 0., 0., 1. 101 | plt.axis('equal') 102 | plot_car(x, y, yaw) 103 | plt.show() -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc.rst: -------------------------------------------------------------------------------- 1 | 2 | Nonlinear Model Predictive Control with C-GMRES 3 | ----------------------------------------------- 4 | 5 | .. code-block:: ipython3 6 | 7 | from IPython.display import Image 8 | Image(filename="Figure_4.png",width=600) 9 | 10 | 11 | 12 | 13 | .. image:: cgmres_nmpc_files/cgmres_nmpc_1_0.png 14 | :width: 600px 15 | 16 | 17 | 18 | .. code-block:: ipython3 19 | 20 | Image(filename="Figure_1.png",width=600) 21 | 22 | 23 | 24 | 25 | .. image:: cgmres_nmpc_files/cgmres_nmpc_2_0.png 26 | :width: 600px 27 | 28 | 29 | 30 | .. code-block:: ipython3 31 | 32 | Image(filename="Figure_2.png",width=600) 33 | 34 | 35 | 36 | 37 | .. image:: cgmres_nmpc_files/cgmres_nmpc_3_0.png 38 | :width: 600px 39 | 40 | 41 | 42 | .. code-block:: ipython3 43 | 44 | Image(filename="Figure_3.png",width=600) 45 | 46 | 47 | 48 | 49 | .. image:: cgmres_nmpc_files/cgmres_nmpc_4_0.png 50 | :width: 600px 51 | 52 | 53 | 54 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/cgmres_nmpc/animation.gif 55 | :alt: gif 56 | 57 | gif 58 | 59 | Mathematical Formulation 60 | ~~~~~~~~~~~~~~~~~~~~~~~~ 61 | 62 | Motion model is 63 | 64 | .. math:: \dot{x}=vcos\theta 65 | 66 | .. math:: \dot{y}=vsin\theta 67 | 68 | .. math:: \dot{\theta}=\frac{v}{WB}sin(u_{\delta}) 69 | 70 | \ (tan is not good for optimization) 71 | 72 | .. math:: \dot{v}=u_a 73 | 74 | Cost function is 75 | 76 | .. math:: J=\frac{1}{2}(u_a^2+u_{\delta}^2)-\phi_a d_a-\phi_\delta d_\delta 77 | 78 | Input constraints are 79 | 80 | .. math:: |u_a| \leq u_{a,max} 81 | 82 | .. math:: |u_\delta| \leq u_{\delta,max} 83 | 84 | So, Hamiltonian　is 85 | 86 | .. math:: 87 | 88 | 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\\ 89 | +\rho_1(u_a^2+d_a^2+u_{a,max}^2)+\rho_2(u_\delta^2+d_\delta^2+u_{\delta,max}^2) 90 | 91 | Partial differential equations of the Hamiltonian are: 92 | 93 | :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*}` 94 | 95 | :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*}` 96 | 97 | Ref 98 | ~~~ 99 | 100 | - `Shunichi09/nonlinear_control: Implementing the nonlinear model 101 | predictive control, sliding mode 102 | control `__ 103 | 104 | - `非線形モデル予測制御におけるCGMRES法をpythonで実装する - 105 | Qiita `__ 106 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable_generator.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Lookup Table generation for model predictive trajectory generator 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | from matplotlib import pyplot as plt 9 | import numpy as np 10 | import math 11 | import model_predictive_trajectory_generator as planner 12 | import motion_model 13 | import pandas as pd 14 | 15 | 16 | def calc_states_list(): 17 | maxyaw = np.deg2rad(-30.0) 18 | 19 | x = np.arange(10.0, 30.0, 5.0) 20 | y = np.arange(0.0, 20.0, 2.0) 21 | yaw = np.arange(-maxyaw, maxyaw, maxyaw) 22 | 23 | states = [] 24 | for iyaw in yaw: 25 | for iy in y: 26 | for ix in x: 27 | states.append([ix, iy, iyaw]) 28 | print("nstate:", len(states)) 29 | 30 | return states 31 | 32 | 33 | def search_nearest_one_from_lookuptable(tx, ty, tyaw, lookuptable): 34 | mind = float("inf") 35 | minid = -1 36 | 37 | for (i, table) in enumerate(lookuptable): 38 | 39 | dx = tx - table[0] 40 | dy = ty - table[1] 41 | dyaw = tyaw - table[2] 42 | d = math.sqrt(dx ** 2 + dy ** 2 + dyaw ** 2) 43 | if d <= mind: 44 | minid = i 45 | mind = d 46 | 47 | # print(minid) 48 | 49 | return lookuptable[minid] 50 | 51 | 52 | def save_lookup_table(fname, table): 53 | mt = np.array(table) 54 | print(mt) 55 | # save csv 56 | df = pd.DataFrame() 57 | df["x"] = mt[:, 0] 58 | df["y"] = mt[:, 1] 59 | df["yaw"] = mt[:, 2] 60 | df["s"] = mt[:, 3] 61 | df["km"] = mt[:, 4] 62 | df["kf"] = mt[:, 5] 63 | df.to_csv(fname, index=None) 64 | 65 | print("lookup table file is saved as " + fname) 66 | 67 | 68 | def generate_lookup_table(): 69 | states = calc_states_list() 70 | k0 = 0.0 71 | 72 | # x, y, yaw, s, km, kf 73 | lookuptable = [[1.0, 0.0, 0.0, 1.0, 0.0, 0.0]] 74 | 75 | for state in states: 76 | bestp = search_nearest_one_from_lookuptable( 77 | state[0], state[1], state[2], lookuptable) 78 | 79 | target = motion_model.State(x=state[0], y=state[1], yaw=state[2]) 80 | init_p = np.array( 81 | [math.sqrt(state[0] ** 2 + state[1] ** 2), bestp[4], bestp[5]]).reshape(3, 1) 82 | 83 | x, y, yaw, p = planner.optimize_trajectory(target, k0, init_p) 84 | 85 | if x is not None: 86 | print("find good path") 87 | lookuptable.append( 88 | [x[-1], y[-1], yaw[-1], float(p[0]), float(p[1]), float(p[2])]) 89 | 90 | print("finish lookup table generation") 91 | 92 | save_lookup_table("lookuptable.csv", lookuptable) 93 | 94 | for table in lookuptable: 95 | xc, yc, yawc = motion_model.generate_trajectory( 96 | table[3], table[4], table[5], k0) 97 | plt.plot(xc, yc, "-r") 98 | xc, yc, yawc = motion_model.generate_trajectory( 99 | table[3], -table[4], -table[5], k0) 100 | plt.plot(xc, yc, "-r") 101 | 102 | plt.grid(True) 103 | plt.axis("equal") 104 | plt.show() 105 | 106 | print("Done") 107 | 108 | 109 | def main(): 110 | generate_lookup_table() 111 | 112 | 113 | if __name__ == '__main__': 114 | main() 115 | -------------------------------------------------------------------------------- /docs/modules/path_tracking.rst: -------------------------------------------------------------------------------- 1 | .. _path_tracking: 2 | 3 | Path tracking 4 | ============= 5 | 6 | move to a pose control 7 | ---------------------- 8 | 9 | This is a simulation of moving to a pose control 10 | 11 | |2| 12 | 13 | Ref: 14 | 15 | - `P. I. Corke, "Robotics, Vision and Control" \| SpringerLink 16 | p102 `__ 17 | 18 | Pure pursuit tracking 19 | --------------------- 20 | 21 | Path tracking simulation with pure pursuit steering control and PID 22 | speed control. 23 | 24 | |3| 25 | 26 | The red line is a target course, the green cross means the target point 27 | for pure pursuit control, the blue line is the tracking. 28 | 29 | Ref: 30 | 31 | - `A Survey of Motion Planning and Control Techniques for Self-driving 32 | Urban Vehicles `__ 33 | 34 | Stanley control 35 | --------------- 36 | 37 | Path tracking simulation with Stanley steering control and PID speed 38 | control. 39 | 40 | |4| 41 | 42 | Ref: 43 | 44 | - `Stanley: The robot that won the DARPA grand 45 | challenge `__ 46 | 47 | - `Automatic Steering Methods for Autonomous Automobile Path 48 | Tracking `__ 49 | 50 | Rear wheel feedback control 51 | --------------------------- 52 | 53 | Path tracking simulation with rear wheel feedback steering control and 54 | PID speed control. 55 | 56 | |5| 57 | 58 | Ref: 59 | 60 | - `A Survey of Motion Planning and Control Techniques for Self-driving 61 | Urban Vehicles `__ 62 | 63 | .. _linearquadratic-regulator-(lqr)-steering-control: 64 | 65 | Linear–quadratic regulator (LQR) steering control 66 | ------------------------------------------------- 67 | 68 | Path tracking simulation with LQR steering control and PID speed 69 | control. 70 | 71 | |6| 72 | 73 | Ref: 74 | 75 | - `ApolloAuto/apollo: An open autonomous driving 76 | platform `__ 77 | 78 | .. _linearquadratic-regulator-(lqr)-speed-and-steering-control: 79 | 80 | Linear–quadratic regulator (LQR) speed and steering control 81 | ----------------------------------------------------------- 82 | 83 | Path tracking simulation with LQR speed and steering control. 84 | 85 | |7| 86 | 87 | Ref: 88 | 89 | - `Towards fully autonomous driving: Systems and algorithms - IEEE 90 | Conference 91 | Publication `__ 92 | 93 | .. include:: Model_predictive_speed_and_steering_control.rst 94 | 95 | Ref: 96 | 97 | - `notebook `__ 98 | 99 | 100 | .. include:: cgmres_nmpc.rst 101 | 102 | .. |2| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/move_to_pose/animation.gif 103 | .. |3| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/pure_pursuit/animation.gif 104 | .. |4| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/stanley_controller/animation.gif 105 | .. |5| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/rear_wheel_feedback/animation.gif 106 | .. |6| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_steer_control/animation.gif 107 | .. |7| image:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/lqr_speed_steer_control/animation.gif 108 | -------------------------------------------------------------------------------- /ArmNavigation/two_joint_arm_to_point_control/two_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse kinematics of a two-joint arm 3 | Left-click the plot to set the goal position of the end effector 4 | 5 | Author: Daniel Ingram (daniel-s-ingram) 6 | Atsushi Sakai (@Atsushi_twi) 7 | 8 | Ref: P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 p102 9 | - [Robotics, Vision and Control \| SpringerLink](https://link.springer.com/book/10.1007/978-3-642-20144-8) 10 | 11 | """ 12 | 13 | import matplotlib.pyplot as plt 14 | import numpy as np 15 | 16 | 17 | # Similation parameters 18 | Kp = 15 19 | dt = 0.01 20 | 21 | # Link lengths 22 | l1 = l2 = 1 23 | 24 | # Set initial goal position to the initial end-effector position 25 | x = 2 26 | y = 0 27 | 28 | show_animation = True 29 | 30 | if show_animation: 31 | plt.ion() 32 | 33 | 34 | def two_joint_arm(GOAL_TH=0.0, theta1=0.0, theta2=0.0): 35 | """ 36 | Computes the inverse kinematics for a planar 2DOF arm 37 | """ 38 | while True: 39 | try: 40 | theta2_goal = np.arccos( 41 | (x**2 + y**2 - l1**2 - l2**2) / (2 * l1 * l2)) 42 | theta1_goal = np.math.atan2(y, x) - np.math.atan2(l2 * 43 | np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) 44 | 45 | if theta1_goal < 0: 46 | theta2_goal = -theta2_goal 47 | theta1_goal = np.math.atan2( 48 | y, x) - np.math.atan2(l2 * np.sin(theta2_goal), (l1 + l2 * np.cos(theta2_goal))) 49 | 50 | theta1 = theta1 + Kp * ang_diff(theta1_goal, theta1) * dt 51 | theta2 = theta2 + Kp * ang_diff(theta2_goal, theta2) * dt 52 | except ValueError as e: 53 | print("Unreachable goal") 54 | 55 | wrist = plot_arm(theta1, theta2, x, y) 56 | 57 | # check goal 58 | d2goal = np.math.sqrt((wrist[0] - x)**2 + (wrist[1] - y)**2) 59 | 60 | if abs(d2goal) < GOAL_TH and x is not None: 61 | return theta1, theta2 62 | 63 | 64 | def plot_arm(theta1, theta2, x, y): # pragma: no cover 65 | shoulder = np.array([0, 0]) 66 | elbow = shoulder + np.array([l1 * np.cos(theta1), l1 * np.sin(theta1)]) 67 | wrist = elbow + \ 68 | np.array([l2 * np.cos(theta1 + theta2), l2 * np.sin(theta1 + theta2)]) 69 | 70 | if show_animation: 71 | plt.cla() 72 | 73 | plt.plot([shoulder[0], elbow[0]], [shoulder[1], elbow[1]], 'k-') 74 | plt.plot([elbow[0], wrist[0]], [elbow[1], wrist[1]], 'k-') 75 | 76 | plt.plot(shoulder[0], shoulder[1], 'ro') 77 | plt.plot(elbow[0], elbow[1], 'ro') 78 | plt.plot(wrist[0], wrist[1], 'ro') 79 | 80 | plt.plot([wrist[0], x], [wrist[1], y], 'g--') 81 | plt.plot(x, y, 'g*') 82 | 83 | plt.xlim(-2, 2) 84 | plt.ylim(-2, 2) 85 | 86 | plt.show() 87 | plt.pause(dt) 88 | 89 | return wrist 90 | 91 | 92 | def ang_diff(theta1, theta2): 93 | # Returns the difference between two angles in the range -pi to +pi 94 | return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi 95 | 96 | 97 | def click(event): # pragma: no cover 98 | global x, y 99 | x = event.xdata 100 | y = event.ydata 101 | 102 | 103 | def animation(): 104 | from random import random 105 | global x, y 106 | theta1 = theta2 = 0.0 107 | for i in range(5): 108 | x = 2.0 * random() - 1.0 109 | y = 2.0 * random() - 1.0 110 | theta1, theta2 = two_joint_arm( 111 | GOAL_TH=0.01, theta1=theta1, theta2=theta2) 112 | 113 | 114 | def main(): # pragma: no cover 115 | fig = plt.figure() 116 | fig.canvas.mpl_connect("button_press_event", click) 117 | two_joint_arm() 118 | 119 | 120 | if __name__ == "__main__": 121 | # animation() 122 | main() 123 | -------------------------------------------------------------------------------- /Mapping/raycasting_grid_map/raycasting_grid_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Ray casting 2D grid map example 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 | 13 | EXTEND_AREA = 10.0 14 | 15 | show_animation = True 16 | 17 | 18 | def calc_grid_map_config(ox, oy, xyreso): 19 | minx = round(min(ox) - EXTEND_AREA / 2.0) 20 | miny = round(min(oy) - EXTEND_AREA / 2.0) 21 | maxx = round(max(ox) + EXTEND_AREA / 2.0) 22 | maxy = round(max(oy) + EXTEND_AREA / 2.0) 23 | xw = int(round((maxx - minx) / xyreso)) 24 | yw = int(round((maxy - miny) / xyreso)) 25 | 26 | return minx, miny, maxx, maxy, xw, yw 27 | 28 | 29 | class precastDB: 30 | 31 | def __init__(self): 32 | self.px = 0.0 33 | self.py = 0.0 34 | self.d = 0.0 35 | self.angle = 0.0 36 | self.ix = 0 37 | self.iy = 0 38 | 39 | def __str__(self): 40 | return str(self.px) + "," + str(self.py) + "," + str(self.d) + "," + str(self.angle) 41 | 42 | 43 | def atan_zero_to_twopi(y, x): 44 | angle = math.atan2(y, x) 45 | if angle < 0.0: 46 | angle += math.pi * 2.0 47 | 48 | return angle 49 | 50 | 51 | def precasting(minx, miny, xw, yw, xyreso, yawreso): 52 | 53 | precast = [[] for i in range(int(round((math.pi * 2.0) / yawreso)) + 1)] 54 | 55 | for ix in range(xw): 56 | for iy in range(yw): 57 | px = ix * xyreso + minx 58 | py = iy * xyreso + miny 59 | 60 | d = math.sqrt(px**2 + py**2) 61 | angle = atan_zero_to_twopi(py, px) 62 | angleid = int(math.floor(angle / yawreso)) 63 | 64 | pc = precastDB() 65 | 66 | pc.px = px 67 | pc.py = py 68 | pc.d = d 69 | pc.ix = ix 70 | pc.iy = iy 71 | pc.angle = angle 72 | 73 | precast[angleid].append(pc) 74 | 75 | return precast 76 | 77 | 78 | def generate_ray_casting_grid_map(ox, oy, xyreso, yawreso): 79 | 80 | minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) 81 | 82 | pmap = [[0.0 for i in range(yw)] for i in range(xw)] 83 | 84 | precast = precasting(minx, miny, xw, yw, xyreso, yawreso) 85 | 86 | for (x, y) in zip(ox, oy): 87 | 88 | d = math.sqrt(x**2 + y**2) 89 | angle = atan_zero_to_twopi(y, x) 90 | angleid = int(math.floor(angle / yawreso)) 91 | 92 | gridlist = precast[angleid] 93 | 94 | ix = int(round((x - minx) / xyreso)) 95 | iy = int(round((y - miny) / xyreso)) 96 | 97 | for grid in gridlist: 98 | if grid.d > d: 99 | pmap[grid.ix][grid.iy] = 0.5 100 | 101 | pmap[ix][iy] = 1.0 102 | 103 | return pmap, minx, maxx, miny, maxy, xyreso 104 | 105 | 106 | def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): 107 | x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), 108 | slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] 109 | plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) 110 | plt.axis("equal") 111 | 112 | 113 | def main(): 114 | print(__file__ + " start!!") 115 | 116 | xyreso = 0.25 # x-y grid resolution [m] 117 | yawreso = np.deg2rad(10.0) # yaw angle resolution [rad] 118 | 119 | for i in range(5): 120 | ox = (np.random.rand(4) - 0.5) * 10.0 121 | oy = (np.random.rand(4) - 0.5) * 10.0 122 | pmap, minx, maxx, miny, maxy, xyreso = generate_ray_casting_grid_map( 123 | ox, oy, xyreso, yawreso) 124 | 125 | if show_animation: # pragma: no cover 126 | plt.cla() 127 | draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) 128 | plt.plot(ox, oy, "xr") 129 | plt.plot(0.0, 0.0, "ob") 130 | plt.pause(1.0) 131 | 132 | 133 | if __name__ == '__main__': 134 | main() 135 | -------------------------------------------------------------------------------- /PathPlanning/LQRPlanner/LQRplanner.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | LQR local path planning 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import random 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | import scipy.linalg as la 15 | 16 | SHOW_ANIMATION = True 17 | 18 | 19 | class LQRPlanner: 20 | 21 | def __init__(self): 22 | self.MAX_TIME = 100.0 # Maximum simulation time 23 | self.DT = 0.1 # Time tick 24 | self.GOAL_DIST = 0.1 25 | self.MAX_ITER = 150 26 | self.EPS = 0.01 27 | 28 | def lqr_planning(self, sx, sy, gx, gy, show_animation=SHOW_ANIMATION): 29 | 30 | rx, ry = [sx], [sy] 31 | 32 | x = np.array([sx - gx, sy - gy]).reshape(2, 1) # State vector 33 | 34 | # Linear system model 35 | A, B = self.get_system_model() 36 | 37 | found_path = False 38 | 39 | time = 0.0 40 | while time <= self.MAX_TIME: 41 | time += self.DT 42 | 43 | u = self.lqr_control(A, B, x) 44 | 45 | x = A @ x + B @ u 46 | 47 | rx.append(x[0, 0] + gx) 48 | ry.append(x[1, 0] + gy) 49 | 50 | d = math.sqrt((gx - rx[-1]) ** 2 + (gy - ry[-1]) ** 2) 51 | if d <= self.GOAL_DIST: 52 | found_path = True 53 | break 54 | 55 | # animation 56 | if show_animation: # pragma: no cover 57 | plt.plot(sx, sy, "or") 58 | plt.plot(gx, gy, "ob") 59 | plt.plot(rx, ry, "-r") 60 | plt.axis("equal") 61 | plt.pause(1.0) 62 | 63 | if not found_path: 64 | print("Cannot found path") 65 | return [], [] 66 | 67 | return rx, ry 68 | 69 | def solve_dare(self, A, B, Q, R): 70 | """ 71 | solve a discrete time_Algebraic Riccati equation (DARE) 72 | """ 73 | X, Xn = Q, Q 74 | 75 | for i in range(self.MAX_ITER): 76 | Xn = A.T * X * A - A.T * X * B * \ 77 | la.inv(R + B.T * X * B) * B.T * X * A + Q 78 | if (abs(Xn - X)).max() < self.EPS: 79 | break 80 | X = Xn 81 | 82 | return Xn 83 | 84 | def dlqr(self, A, B, Q, R): 85 | """Solve the discrete time lqr controller. 86 | x[k+1] = A x[k] + B u[k] 87 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 88 | # ref Bertsekas, p.151 89 | """ 90 | 91 | # first, try to solve the ricatti equation 92 | X = self.solve_dare(A, B, Q, R) 93 | 94 | # compute the LQR gain 95 | K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) 96 | 97 | eigValues = la.eigvals(A - B @ K) 98 | 99 | return K, X, eigValues 100 | 101 | def get_system_model(self): 102 | 103 | A = np.array([[self.DT, 1.0], 104 | [0.0, self.DT]]) 105 | B = np.array([0.0, 1.0]).reshape(2, 1) 106 | 107 | return A, B 108 | 109 | def lqr_control(self, A, B, x): 110 | 111 | Kopt, X, ev = self.dlqr(A, B, np.eye(2), np.eye(1)) 112 | 113 | u = -Kopt @ x 114 | 115 | return u 116 | 117 | 118 | def main(): 119 | print(__file__ + " start!!") 120 | 121 | ntest = 10 # number of goal 122 | area = 100.0 # sampling area 123 | 124 | lqr_planner = LQRPlanner() 125 | 126 | for i in range(ntest): 127 | sx = 6.0 128 | sy = 6.0 129 | gx = random.uniform(-area, area) 130 | gy = random.uniform(-area, area) 131 | 132 | rx, ry = lqr_planner.lqr_planning(sx, sy, gx, gy) 133 | 134 | if SHOW_ANIMATION: # pragma: no cover 135 | plt.plot(sx, sy, "or") 136 | plt.plot(gx, gy, "ob") 137 | plt.plot(rx, ry, "-r") 138 | plt.axis("equal") 139 | plt.pause(1.0) 140 | 141 | 142 | if __name__ == '__main__': 143 | main() 144 | -------------------------------------------------------------------------------- /PathPlanning/RRT/rrt_with_pathsmoothing.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path planning Sample Code with RRT with path smoothing 4 | 5 | @author: AtsushiSakai(@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import os 11 | import random 12 | import sys 13 | 14 | import matplotlib.pyplot as plt 15 | 16 | sys.path.append(os.path.dirname(os.path.abspath(__file__))) 17 | 18 | try: 19 | from rrt import RRT 20 | except ImportError: 21 | raise 22 | 23 | show_animation = True 24 | 25 | 26 | def get_path_length(path): 27 | le = 0 28 | for i in range(len(path) - 1): 29 | dx = path[i + 1][0] - path[i][0] 30 | dy = path[i + 1][1] - path[i][1] 31 | d = math.sqrt(dx * dx + dy * dy) 32 | le += d 33 | 34 | return le 35 | 36 | 37 | def get_target_point(path, targetL): 38 | le = 0 39 | ti = 0 40 | lastPairLen = 0 41 | for i in range(len(path) - 1): 42 | dx = path[i + 1][0] - path[i][0] 43 | dy = path[i + 1][1] - path[i][1] 44 | d = math.sqrt(dx * dx + dy * dy) 45 | le += d 46 | if le >= targetL: 47 | ti = i - 1 48 | lastPairLen = d 49 | break 50 | 51 | partRatio = (le - targetL) / lastPairLen 52 | 53 | x = path[ti][0] + (path[ti + 1][0] - path[ti][0]) * partRatio 54 | y = path[ti][1] + (path[ti + 1][1] - path[ti][1]) * partRatio 55 | 56 | return [x, y, ti] 57 | 58 | 59 | def line_collision_check(first, second, obstacleList): 60 | # Line Equation 61 | 62 | x1 = first[0] 63 | y1 = first[1] 64 | x2 = second[0] 65 | y2 = second[1] 66 | 67 | try: 68 | a = y2 - y1 69 | b = -(x2 - x1) 70 | c = y2 * (x2 - x1) - x2 * (y2 - y1) 71 | except ZeroDivisionError: 72 | return False 73 | 74 | for (ox, oy, size) in obstacleList: 75 | d = abs(a * ox + b * oy + c) / (math.sqrt(a * a + b * b)) 76 | if d <= size: 77 | return False 78 | 79 | # print("OK") 80 | 81 | return True # OK 82 | 83 | 84 | def path_smoothing(path, max_iter, obstacle_list): 85 | le = get_path_length(path) 86 | 87 | for i in range(max_iter): 88 | # Sample two points 89 | pickPoints = [random.uniform(0, le), random.uniform(0, le)] 90 | pickPoints.sort() 91 | first = get_target_point(path, pickPoints[0]) 92 | second = get_target_point(path, pickPoints[1]) 93 | 94 | if first[2] <= 0 or second[2] <= 0: 95 | continue 96 | 97 | if (second[2] + 1) > len(path): 98 | continue 99 | 100 | if second[2] == first[2]: 101 | continue 102 | 103 | # collision check 104 | if not line_collision_check(first, second, obstacle_list): 105 | continue 106 | 107 | # Create New path 108 | newPath = [] 109 | newPath.extend(path[:first[2] + 1]) 110 | newPath.append([first[0], first[1]]) 111 | newPath.append([second[0], second[1]]) 112 | newPath.extend(path[second[2] + 1:]) 113 | path = newPath 114 | le = get_path_length(path) 115 | 116 | return path 117 | 118 | 119 | def main(): 120 | # ====Search Path with RRT==== 121 | # Parameter 122 | obstacleList = [ 123 | (5, 5, 1), 124 | (3, 6, 2), 125 | (3, 8, 2), 126 | (3, 10, 2), 127 | (7, 5, 2), 128 | (9, 5, 2) 129 | ] # [x,y,size] 130 | rrt = RRT(start=[0, 0], goal=[5, 10], 131 | rand_area=[-2, 15], obstacle_list=obstacleList) 132 | path = rrt.planning(animation=show_animation) 133 | 134 | # Path smoothing 135 | maxIter = 1000 136 | smoothedPath = path_smoothing(path, maxIter, obstacleList) 137 | 138 | # Draw final path 139 | if show_animation: 140 | rrt.draw_graph() 141 | plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') 142 | 143 | plt.plot([x for (x, y) in smoothedPath], [ 144 | y for (x, y) in smoothedPath], '-b') 145 | 146 | plt.grid(True) 147 | plt.pause(0.01) # Need for Mac 148 | plt.show() 149 | 150 | 151 | if __name__ == '__main__': 152 | main() 153 | -------------------------------------------------------------------------------- /Mapping/circle_fitting/circle_fitting.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Object shape recognition with circle fitting 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import matplotlib.pyplot as plt 10 | import math 11 | import random 12 | import numpy as np 13 | 14 | show_animation = True 15 | 16 | 17 | def circle_fitting(x, y): 18 | """ 19 | Circle Fitting with least squared 20 | input: point x-y positions 21 | output cxe x center position 22 | cye y center position 23 | re radius of circle 24 | error: prediction error 25 | """ 26 | 27 | sumx = sum(x) 28 | sumy = sum(y) 29 | sumx2 = sum([ix ** 2 for ix in x]) 30 | sumy2 = sum([iy ** 2 for iy in y]) 31 | sumxy = sum([ix * iy for (ix, iy) in zip(x, y)]) 32 | 33 | F = np.array([[sumx2, sumxy, sumx], 34 | [sumxy, sumy2, sumy], 35 | [sumx, sumy, len(x)]]) 36 | 37 | G = np.array([[-sum([ix ** 3 + ix * iy ** 2 for (ix, iy) in zip(x, y)])], 38 | [-sum([ix ** 2 * iy + iy ** 3 for (ix, iy) in zip(x, y)])], 39 | [-sum([ix ** 2 + iy ** 2 for (ix, iy) in zip(x, y)])]]) 40 | 41 | T = np.linalg.inv(F).dot(G) 42 | 43 | cxe = float(T[0] / -2) 44 | cye = float(T[1] / -2) 45 | re = math.sqrt(cxe**2 + cye**2 - T[2]) 46 | 47 | error = sum([np.hypot(cxe - ix, cye - iy) - re for (ix, iy) in zip(x, y)]) 48 | 49 | return (cxe, cye, re, error) 50 | 51 | 52 | def get_sample_points(cx, cy, cr, angle_reso): 53 | x, y, angle, r = [], [], [], [] 54 | 55 | # points sampling 56 | for theta in np.arange(0.0, 2.0 * math.pi, angle_reso): 57 | nx = cx + cr * math.cos(theta) 58 | ny = cy + cr * math.sin(theta) 59 | nangle = math.atan2(ny, nx) 60 | nr = math.hypot(nx, ny) * random.uniform(0.95, 1.05) 61 | 62 | x.append(nx) 63 | y.append(ny) 64 | angle.append(nangle) 65 | r.append(nr) 66 | 67 | # ray casting filter 68 | rx, ry = ray_casting_filter(x, y, angle, r, angle_reso) 69 | 70 | return rx, ry 71 | 72 | 73 | def ray_casting_filter(xl, yl, thetal, rangel, angle_reso): 74 | rx, ry = [], [] 75 | rangedb = [float("inf") for _ in range( 76 | int(math.floor((math.pi * 2.0) / angle_reso)) + 1)] 77 | 78 | for i, _ in enumerate(thetal): 79 | angleid = math.floor(thetal[i] / angle_reso) 80 | 81 | if rangedb[angleid] > rangel[i]: 82 | rangedb[angleid] = rangel[i] 83 | 84 | for i, _ in enumerate(rangedb): 85 | t = i * angle_reso 86 | if rangedb[i] != float("inf"): 87 | rx.append(rangedb[i] * math.cos(t)) 88 | ry.append(rangedb[i] * math.sin(t)) 89 | 90 | return rx, ry 91 | 92 | 93 | def plot_circle(x, y, size, color="-b"): # pragma: no cover 94 | deg = list(range(0, 360, 5)) 95 | deg.append(0) 96 | xl = [x + size * math.cos(np.deg2rad(d)) for d in deg] 97 | yl = [y + size * math.sin(np.deg2rad(d)) for d in deg] 98 | plt.plot(xl, yl, color) 99 | 100 | 101 | def main(): 102 | 103 | # simulation parameters 104 | simtime = 15.0 # simulation time 105 | dt = 1.0 # time tick 106 | 107 | cx = -2.0 # initial x position of obstacle 108 | cy = -8.0 # initial y position of obstacle 109 | cr = 1.0 # obstacle radious 110 | theta = np.deg2rad(30.0) # obstacle moving direction 111 | angle_reso = np.deg2rad(3.0) # sensor angle resolution 112 | 113 | time = 0.0 114 | while time <= simtime: 115 | time += dt 116 | 117 | cx += math.cos(theta) 118 | cy += math.cos(theta) 119 | 120 | x, y = get_sample_points(cx, cy, cr, angle_reso) 121 | 122 | ex, ey, er, error = circle_fitting(x, y) 123 | print("Error:", error) 124 | 125 | if show_animation: # pragma: no cover 126 | plt.cla() 127 | plt.axis("equal") 128 | plt.plot(0.0, 0.0, "*r") 129 | plot_circle(cx, cy, cr) 130 | plt.plot(x, y, "xr") 131 | plot_circle(ex, ey, er, "-r") 132 | plt.pause(dt) 133 | 134 | print("Done") 135 | 136 | 137 | if __name__ == '__main__': 138 | main() 139 | -------------------------------------------------------------------------------- /PathTracking/move_to_pose/move_to_pose.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Move to specified pose 4 | 5 | Author: Daniel Ingram (daniel-s-ingram) 6 | Atsushi Sakai(@Atsushi_twi) 7 | 8 | P. I. Corke, "Robotics, Vision & Control", Springer 2017, ISBN 978-3-319-54413-7 9 | 10 | """ 11 | 12 | import matplotlib.pyplot as plt 13 | import numpy as np 14 | from random import random 15 | 16 | # simulation parameters 17 | Kp_rho = 9 18 | Kp_alpha = 15 19 | Kp_beta = -3 20 | dt = 0.01 21 | 22 | show_animation = True 23 | 24 | 25 | def move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal): 26 | """ 27 | rho is the distance between the robot and the goal position 28 | alpha is the angle to the goal relative to the heading of the robot 29 | beta is the angle between the robot's position and the goal position plus the goal angle 30 | 31 | Kp_rho*rho and Kp_alpha*alpha drive the robot along a line towards the goal 32 | Kp_beta*beta rotates the line so that it is parallel to the goal angle 33 | """ 34 | x = x_start 35 | y = y_start 36 | theta = theta_start 37 | 38 | x_diff = x_goal - x 39 | y_diff = y_goal - y 40 | 41 | x_traj, y_traj = [], [] 42 | 43 | rho = np.sqrt(x_diff**2 + y_diff**2) 44 | while rho > 0.001: 45 | x_traj.append(x) 46 | y_traj.append(y) 47 | 48 | x_diff = x_goal - x 49 | y_diff = y_goal - y 50 | 51 | # Restrict alpha and beta (angle differences) to the range 52 | # [-pi, pi] to prevent unstable behavior e.g. difference going 53 | # from 0 rad to 2*pi rad with slight turn 54 | 55 | rho = np.sqrt(x_diff**2 + y_diff**2) 56 | alpha = (np.arctan2(y_diff, x_diff) 57 | - theta + np.pi) % (2 * np.pi) - np.pi 58 | beta = (theta_goal - theta - alpha + np.pi) % (2 * np.pi) - np.pi 59 | 60 | v = Kp_rho * rho 61 | w = Kp_alpha * alpha + Kp_beta * beta 62 | 63 | if alpha > np.pi / 2 or alpha < -np.pi / 2: 64 | v = -v 65 | 66 | theta = theta + w * dt 67 | x = x + v * np.cos(theta) * dt 68 | y = y + v * np.sin(theta) * dt 69 | 70 | if show_animation: # pragma: no cover 71 | plt.cla() 72 | plt.arrow(x_start, y_start, np.cos(theta_start), 73 | np.sin(theta_start), color='r', width=0.1) 74 | plt.arrow(x_goal, y_goal, np.cos(theta_goal), 75 | np.sin(theta_goal), color='g', width=0.1) 76 | plot_vehicle(x, y, theta, x_traj, y_traj) 77 | 78 | 79 | def plot_vehicle(x, y, theta, x_traj, y_traj): # pragma: no cover 80 | # Corners of triangular vehicle when pointing to the right (0 radians) 81 | p1_i = np.array([0.5, 0, 1]).T 82 | p2_i = np.array([-0.5, 0.25, 1]).T 83 | p3_i = np.array([-0.5, -0.25, 1]).T 84 | 85 | T = transformation_matrix(x, y, theta) 86 | p1 = np.matmul(T, p1_i) 87 | p2 = np.matmul(T, p2_i) 88 | p3 = np.matmul(T, p3_i) 89 | 90 | plt.plot([p1[0], p2[0]], [p1[1], p2[1]], 'k-') 91 | plt.plot([p2[0], p3[0]], [p2[1], p3[1]], 'k-') 92 | plt.plot([p3[0], p1[0]], [p3[1], p1[1]], 'k-') 93 | 94 | plt.plot(x_traj, y_traj, 'b--') 95 | 96 | plt.xlim(0, 20) 97 | plt.ylim(0, 20) 98 | 99 | plt.pause(dt) 100 | 101 | 102 | def transformation_matrix(x, y, theta): 103 | return np.array([ 104 | [np.cos(theta), -np.sin(theta), x], 105 | [np.sin(theta), np.cos(theta), y], 106 | [0, 0, 1] 107 | ]) 108 | 109 | 110 | def main(): 111 | 112 | for i in range(5): 113 | x_start = 20 * random() 114 | y_start = 20 * random() 115 | theta_start = 2 * np.pi * random() - np.pi 116 | x_goal = 20 * random() 117 | y_goal = 20 * random() 118 | theta_goal = 2 * np.pi * random() - np.pi 119 | print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % 120 | (x_start, y_start, theta_start)) 121 | print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % 122 | (x_goal, y_goal, theta_goal)) 123 | move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) 124 | 125 | 126 | if __name__ == '__main__': 127 | main() 128 | -------------------------------------------------------------------------------- /SLAM/iterative_closest_point/iterative_closest_point.py: -------------------------------------------------------------------------------- 1 | """ 2 | Iterative Closest Point (ICP) SLAM example 3 | author: Atsushi Sakai (@Atsushi_twi) 4 | """ 5 | 6 | import math 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | # ICP parameters 11 | EPS = 0.0001 12 | MAXITER = 100 13 | 14 | show_animation = True 15 | 16 | 17 | def ICP_matching(ppoints, cpoints): 18 | """ 19 | Iterative Closest Point matching 20 | - input 21 | ppoints: 2D points in the previous frame 22 | cpoints: 2D points in the current frame 23 | - output 24 | R: Rotation matrix 25 | T: Translation vector 26 | """ 27 | H = None # homogeneous transformation matrix 28 | 29 | dError = 1000.0 30 | preError = 1000.0 31 | count = 0 32 | 33 | while dError >= EPS: 34 | count += 1 35 | 36 | if show_animation: # pragma: no cover 37 | plt.cla() 38 | plt.plot(ppoints[0, :], ppoints[1, :], ".r") 39 | plt.plot(cpoints[0, :], cpoints[1, :], ".b") 40 | plt.plot(0.0, 0.0, "xr") 41 | plt.axis("equal") 42 | plt.pause(1.0) 43 | 44 | inds, error = nearest_neighbor_assosiation(ppoints, cpoints) 45 | Rt, Tt = SVD_motion_estimation(ppoints[:, inds], cpoints) 46 | 47 | # update current points 48 | cpoints = (Rt @ cpoints) + Tt[:, np.newaxis] 49 | 50 | H = update_homogeneous_matrix(H, Rt, Tt) 51 | 52 | dError = abs(preError - error) 53 | preError = error 54 | print("Residual:", error) 55 | 56 | if dError <= EPS: 57 | print("Converge", error, dError, count) 58 | break 59 | elif MAXITER <= count: 60 | print("Not Converge...", error, dError, count) 61 | break 62 | 63 | R = np.array(H[0:2, 0:2]) 64 | T = np.array(H[0:2, 2]) 65 | 66 | return R, T 67 | 68 | 69 | def update_homogeneous_matrix(Hin, R, T): 70 | 71 | H = np.zeros((3, 3)) 72 | 73 | H[0, 0] = R[0, 0] 74 | H[1, 0] = R[1, 0] 75 | H[0, 1] = R[0, 1] 76 | H[1, 1] = R[1, 1] 77 | H[2, 2] = 1.0 78 | 79 | H[0, 2] = T[0] 80 | H[1, 2] = T[1] 81 | 82 | if Hin is None: 83 | return H 84 | else: 85 | return Hin @ H 86 | 87 | 88 | def nearest_neighbor_assosiation(ppoints, cpoints): 89 | 90 | # calc the sum of residual errors 91 | dcpoints = ppoints - cpoints 92 | d = np.linalg.norm(dcpoints, axis=0) 93 | error = sum(d) 94 | 95 | # calc index with nearest neighbor assosiation 96 | inds = [] 97 | for i in range(cpoints.shape[1]): 98 | minid = -1 99 | mind = float("inf") 100 | for ii in range(ppoints.shape[1]): 101 | d = np.linalg.norm(ppoints[:, ii] - cpoints[:, i]) 102 | 103 | if mind >= d: 104 | mind = d 105 | minid = ii 106 | 107 | inds.append(minid) 108 | 109 | return inds, error 110 | 111 | 112 | def SVD_motion_estimation(ppoints, cpoints): 113 | 114 | pm = np.mean(ppoints, axis=1) 115 | cm = np.mean(cpoints, axis=1) 116 | 117 | pshift = ppoints - pm[:, np.newaxis] 118 | cshift = cpoints - cm[:, np.newaxis] 119 | 120 | W = cshift @ pshift.T 121 | u, s, vh = np.linalg.svd(W) 122 | 123 | R = (u @ vh).T 124 | t = pm - (R @ cm) 125 | 126 | return R, t 127 | 128 | 129 | def main(): 130 | print(__file__ + " start!!") 131 | 132 | # simulation parameters 133 | nPoint = 10 134 | fieldLength = 50.0 135 | motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] 136 | 137 | nsim = 3 # number of simulation 138 | 139 | for _ in range(nsim): 140 | 141 | # previous points 142 | px = (np.random.rand(nPoint) - 0.5) * fieldLength 143 | py = (np.random.rand(nPoint) - 0.5) * fieldLength 144 | ppoints = np.vstack((px, py)) 145 | 146 | # current points 147 | cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0] 148 | for (x, y) in zip(px, py)] 149 | cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] 150 | for (x, y) in zip(px, py)] 151 | cpoints = np.vstack((cx, cy)) 152 | 153 | R, T = ICP_matching(ppoints, cpoints) 154 | print("R:", R) 155 | print("T:", T) 156 | 157 | 158 | if __name__ == '__main__': 159 | main() -------------------------------------------------------------------------------- /Mapping/rectangle_fitting/simulator.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Simulator 4 | 5 | author: Atsushi Sakai 6 | 7 | """ 8 | 9 | import numpy as np 10 | import matplotlib.pyplot as plt 11 | import math 12 | import random 13 | 14 | 15 | class VehicleSimulator(): 16 | 17 | def __init__(self, ix, iy, iyaw, iv, max_v, w, L): 18 | self.x = ix 19 | self.y = iy 20 | self.yaw = iyaw 21 | self.v = iv 22 | self.max_v = max_v 23 | self.W = w 24 | self.L = L 25 | self._calc_vehicle_contour() 26 | 27 | def update(self, dt, a, omega): 28 | self.x += self.v * np.cos(self.yaw) * dt 29 | self.y += self.v * np.sin(self.yaw) * dt 30 | self.yaw += omega * dt 31 | self.v += a * dt 32 | if self.v >= self.max_v: 33 | self.v = self.max_v 34 | 35 | def plot(self): 36 | plt.plot(self.x, self.y, ".b") 37 | 38 | # convert global coordinate 39 | gx, gy = self.calc_global_contour() 40 | plt.plot(gx, gy, "--b") 41 | 42 | def calc_global_contour(self): 43 | gx = [(ix * np.cos(self.yaw) + iy * np.sin(self.yaw)) + 44 | self.x for (ix, iy) in zip(self.vc_x, self.vc_y)] 45 | gy = [(ix * np.sin(self.yaw) - iy * np.cos(self.yaw)) + 46 | self.y for (ix, iy) in zip(self.vc_x, self.vc_y)] 47 | 48 | return gx, gy 49 | 50 | def _calc_vehicle_contour(self): 51 | 52 | self.vc_x = [] 53 | self.vc_y = [] 54 | 55 | self.vc_x.append(self.L / 2.0) 56 | self.vc_y.append(self.W / 2.0) 57 | 58 | self.vc_x.append(self.L / 2.0) 59 | self.vc_y.append(-self.W / 2.0) 60 | 61 | self.vc_x.append(-self.L / 2.0) 62 | self.vc_y.append(-self.W / 2.0) 63 | 64 | self.vc_x.append(-self.L / 2.0) 65 | self.vc_y.append(self.W / 2.0) 66 | 67 | self.vc_x.append(self.L / 2.0) 68 | self.vc_y.append(self.W / 2.0) 69 | 70 | self.vc_x, self.vc_y = self._interporate(self.vc_x, self.vc_y) 71 | 72 | def _interporate(self, x, y): 73 | rx, ry = [], [] 74 | dtheta = 0.05 75 | for i in range(len(x) - 1): 76 | rx.extend([(1.0 - θ) * x[i] + θ * x[i + 1] 77 | for θ in np.arange(0.0, 1.0, dtheta)]) 78 | ry.extend([(1.0 - θ) * y[i] + θ * y[i + 1] 79 | for θ in np.arange(0.0, 1.0, dtheta)]) 80 | 81 | rx.extend([(1.0 - θ) * x[len(x) - 1] + θ * x[1] 82 | for θ in np.arange(0.0, 1.0, dtheta)]) 83 | ry.extend([(1.0 - θ) * y[len(y) - 1] + θ * y[1] 84 | for θ in np.arange(0.0, 1.0, dtheta)]) 85 | 86 | return rx, ry 87 | 88 | 89 | class LidarSimulator(): 90 | 91 | def __init__(self): 92 | self.range_noise = 0.01 93 | 94 | def get_observation_points(self, vlist, angle_reso): 95 | x, y, angle, r = [], [], [], [] 96 | 97 | # store all points 98 | for v in vlist: 99 | 100 | gx, gy = v.calc_global_contour() 101 | 102 | for vx, vy in zip(gx, gy): 103 | vangle = math.atan2(vy, vx) 104 | vr = np.hypot(vx, vy) * random.uniform(1.0 - self.range_noise, 105 | 1.0 + self.range_noise) 106 | 107 | x.append(vx) 108 | y.append(vy) 109 | angle.append(vangle) 110 | r.append(vr) 111 | 112 | # ray casting filter 113 | rx, ry = self.ray_casting_filter(x, y, angle, r, angle_reso) 114 | 115 | return rx, ry 116 | 117 | def ray_casting_filter(self, xl, yl, thetal, rangel, angle_reso): 118 | rx, ry = [], [] 119 | rangedb = [float("inf") for _ in range( 120 | int(np.floor((np.pi * 2.0) / angle_reso)) + 1)] 121 | 122 | for i in range(len(thetal)): 123 | angleid = int(round(thetal[i] / angle_reso)) 124 | 125 | if rangedb[angleid] > rangel[i]: 126 | rangedb[angleid] = rangel[i] 127 | 128 | for i in range(len(rangedb)): 129 | t = i * angle_reso 130 | if rangedb[i] != float("inf"): 131 | rx.append(rangedb[i] * np.cos(t)) 132 | ry.append(rangedb[i] * np.sin(t)) 133 | 134 | return rx, ry 135 | 136 | 137 | def main(): 138 | print("start!!") 139 | 140 | print("done!!") 141 | 142 | 143 | if __name__ == '__main__': 144 | main() 145 | -------------------------------------------------------------------------------- /Mapping/kmeans_clustering/kmeans_clustering.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Object clustering with k-means algorithm 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import numpy as np 10 | import math 11 | import matplotlib.pyplot as plt 12 | import random 13 | 14 | show_animation = True 15 | 16 | 17 | class Clusters: 18 | 19 | def __init__(self, x, y, nlabel): 20 | self.x = x 21 | self.y = y 22 | self.ndata = len(self.x) 23 | self.nlabel = nlabel 24 | self.labels = [random.randint(0, nlabel - 1) 25 | for _ in range(self.ndata)] 26 | self.cx = [0.0 for _ in range(nlabel)] 27 | self.cy = [0.0 for _ in range(nlabel)] 28 | 29 | 30 | def kmeans_clustering(rx, ry, nc): 31 | 32 | clusters = Clusters(rx, ry, nc) 33 | clusters = calc_centroid(clusters) 34 | 35 | MAX_LOOP = 10 36 | DCOST_TH = 0.1 37 | pcost = 100.0 38 | for loop in range(MAX_LOOP): 39 | # print("Loop:", loop) 40 | clusters, cost = update_clusters(clusters) 41 | clusters = calc_centroid(clusters) 42 | 43 | dcost = abs(cost - pcost) 44 | if dcost < DCOST_TH: 45 | break 46 | pcost = cost 47 | 48 | return clusters 49 | 50 | 51 | def calc_centroid(clusters): 52 | 53 | for ic in range(clusters.nlabel): 54 | x, y = calc_labeled_points(ic, clusters) 55 | ndata = len(x) 56 | clusters.cx[ic] = sum(x) / ndata 57 | clusters.cy[ic] = sum(y) / ndata 58 | 59 | return clusters 60 | 61 | 62 | def update_clusters(clusters): 63 | cost = 0.0 64 | 65 | for ip in range(clusters.ndata): 66 | px = clusters.x[ip] 67 | py = clusters.y[ip] 68 | 69 | dx = [icx - px for icx in clusters.cx] 70 | dy = [icy - py for icy in clusters.cy] 71 | 72 | dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] 73 | mind = min(dlist) 74 | min_id = dlist.index(mind) 75 | clusters.labels[ip] = min_id 76 | cost += mind 77 | 78 | return clusters, cost 79 | 80 | 81 | def calc_labeled_points(ic, clusters): 82 | 83 | inds = np.array([i for i in range(clusters.ndata) 84 | if clusters.labels[i] == ic]) 85 | tx = np.array(clusters.x) 86 | ty = np.array(clusters.y) 87 | 88 | x = tx[inds] 89 | y = ty[inds] 90 | 91 | return x, y 92 | 93 | 94 | def calc_raw_data(cx, cy, npoints, rand_d): 95 | 96 | rx, ry = [], [] 97 | 98 | for (icx, icy) in zip(cx, cy): 99 | for _ in range(npoints): 100 | rx.append(icx + rand_d * (random.random() - 0.5)) 101 | ry.append(icy + rand_d * (random.random() - 0.5)) 102 | 103 | return rx, ry 104 | 105 | 106 | def update_positions(cx, cy): 107 | 108 | DX1 = 0.4 109 | DY1 = 0.5 110 | 111 | cx[0] += DX1 112 | cy[0] += DY1 113 | 114 | DX2 = -0.3 115 | DY2 = -0.5 116 | 117 | cx[1] += DX2 118 | cy[1] += DY2 119 | 120 | return cx, cy 121 | 122 | 123 | def calc_association(cx, cy, clusters): 124 | 125 | inds = [] 126 | 127 | for ic, _ in enumerate(cx): 128 | tcx = cx[ic] 129 | tcy = cy[ic] 130 | 131 | dx = [icx - tcx for icx in clusters.cx] 132 | dy = [icy - tcy for icy in clusters.cy] 133 | 134 | dlist = [math.sqrt(idx**2 + idy**2) for (idx, idy) in zip(dx, dy)] 135 | min_id = dlist.index(min(dlist)) 136 | inds.append(min_id) 137 | 138 | return inds 139 | 140 | 141 | def main(): 142 | print(__file__ + " start!!") 143 | 144 | cx = [0.0, 8.0] 145 | cy = [0.0, 8.0] 146 | npoints = 10 147 | rand_d = 3.0 148 | ncluster = 2 149 | sim_time = 15.0 150 | dt = 1.0 151 | time = 0.0 152 | 153 | while time <= sim_time: 154 | print("Time:", time) 155 | time += dt 156 | 157 | # simulate objects 158 | cx, cy = update_positions(cx, cy) 159 | rx, ry = calc_raw_data(cx, cy, npoints, rand_d) 160 | 161 | clusters = kmeans_clustering(rx, ry, ncluster) 162 | 163 | # for animation 164 | if show_animation: # pragma: no cover 165 | plt.cla() 166 | inds = calc_association(cx, cy, clusters) 167 | for ic in inds: 168 | x, y = calc_labeled_points(ic, clusters) 169 | plt.plot(x, y, "x") 170 | plt.plot(cx, cy, "o") 171 | plt.xlim(-2.0, 10.0) 172 | plt.ylim(-2.0, 10.0) 173 | plt.pause(dt) 174 | 175 | print("Done") 176 | 177 | 178 | if __name__ == '__main__': 179 | main() 180 | -------------------------------------------------------------------------------- /PathPlanning/PotentialFieldPlanning/potential_field_planning.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Potential Field based path planner 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | Ref: 8 | https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf 9 | 10 | """ 11 | 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | 15 | # Parameters 16 | KP = 5.0 # attractive potential gain 17 | ETA = 100.0 # repulsive potential gain 18 | AREA_WIDTH = 30.0 # potential area width [m] 19 | 20 | show_animation = True 21 | 22 | 23 | def calc_potential_field(gx, gy, ox, oy, reso, rr): 24 | minx = min(ox) - AREA_WIDTH / 2.0 25 | miny = min(oy) - AREA_WIDTH / 2.0 26 | maxx = max(ox) + AREA_WIDTH / 2.0 27 | maxy = max(oy) + AREA_WIDTH / 2.0 28 | xw = int(round((maxx - minx) / reso)) 29 | yw = int(round((maxy - miny) / reso)) 30 | 31 | # calc each potential 32 | pmap = [[0.0 for i in range(yw)] for i in range(xw)] 33 | 34 | for ix in range(xw): 35 | x = ix * reso + minx 36 | 37 | for iy in range(yw): 38 | y = iy * reso + miny 39 | ug = calc_attractive_potential(x, y, gx, gy) 40 | uo = calc_repulsive_potential(x, y, ox, oy, rr) 41 | uf = ug + uo 42 | pmap[ix][iy] = uf 43 | 44 | return pmap, minx, miny 45 | 46 | 47 | def calc_attractive_potential(x, y, gx, gy): 48 | return 0.5 * KP * np.hypot(x - gx, y - gy) 49 | 50 | 51 | def calc_repulsive_potential(x, y, ox, oy, rr): 52 | # search nearest obstacle 53 | minid = -1 54 | dmin = float("inf") 55 | for i, _ in enumerate(ox): 56 | d = np.hypot(x - ox[i], y - oy[i]) 57 | if dmin >= d: 58 | dmin = d 59 | minid = i 60 | 61 | # calc repulsive potential 62 | dq = np.hypot(x - ox[minid], y - oy[minid]) 63 | 64 | if dq <= rr: 65 | if dq <= 0.1: 66 | dq = 0.1 67 | 68 | return 0.5 * ETA * (1.0 / dq - 1.0 / rr) ** 2 69 | else: 70 | return 0.0 71 | 72 | 73 | def get_motion_model(): 74 | # dx, dy 75 | motion = [[1, 0], 76 | [0, 1], 77 | [-1, 0], 78 | [0, -1], 79 | [-1, -1], 80 | [-1, 1], 81 | [1, -1], 82 | [1, 1]] 83 | 84 | return motion 85 | 86 | 87 | def potential_field_planning(sx, sy, gx, gy, ox, oy, reso, rr): 88 | 89 | # calc potential field 90 | pmap, minx, miny = calc_potential_field(gx, gy, ox, oy, reso, rr) 91 | 92 | # search path 93 | d = np.hypot(sx - gx, sy - gy) 94 | ix = round((sx - minx) / reso) 95 | iy = round((sy - miny) / reso) 96 | gix = round((gx - minx) / reso) 97 | giy = round((gy - miny) / reso) 98 | 99 | if show_animation: 100 | draw_heatmap(pmap) 101 | plt.plot(ix, iy, "*k") 102 | plt.plot(gix, giy, "*m") 103 | 104 | rx, ry = [sx], [sy] 105 | motion = get_motion_model() 106 | while d >= reso: 107 | minp = float("inf") 108 | minix, miniy = -1, -1 109 | for i, _ in enumerate(motion): 110 | inx = int(ix + motion[i][0]) 111 | iny = int(iy + motion[i][1]) 112 | if inx >= len(pmap) or iny >= len(pmap[0]): 113 | p = float("inf") # outside area 114 | else: 115 | p = pmap[inx][iny] 116 | if minp > p: 117 | minp = p 118 | minix = inx 119 | miniy = iny 120 | ix = minix 121 | iy = miniy 122 | xp = ix * reso + minx 123 | yp = iy * reso + miny 124 | d = np.hypot(gx - xp, gy - yp) 125 | rx.append(xp) 126 | ry.append(yp) 127 | 128 | if show_animation: 129 | plt.plot(ix, iy, ".r") 130 | plt.pause(0.01) 131 | 132 | print("Goal!!") 133 | 134 | return rx, ry 135 | 136 | 137 | def draw_heatmap(data): 138 | data = np.array(data).T 139 | plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues) 140 | 141 | 142 | def main(): 143 | print("potential_field_planning start") 144 | 145 | sx = 0.0 # start x position [m] 146 | sy = 10.0 # start y positon [m] 147 | gx = 30.0 # goal x position [m] 148 | gy = 30.0 # goal y position [m] 149 | grid_size = 0.5 # potential grid size [m] 150 | robot_radius = 5.0 # robot radius [m] 151 | 152 | ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m] 153 | oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m] 154 | 155 | if show_animation: 156 | plt.grid(True) 157 | plt.axis("equal") 158 | 159 | # path generation 160 | _, _ = potential_field_planning( 161 | sx, sy, gx, gy, ox, oy, grid_size, robot_radius) 162 | 163 | if show_animation: 164 | plt.show() 165 | 166 | 167 | if __name__ == '__main__': 168 | print(__file__ + " start!!") 169 | main() 170 | print(__file__ + " Done!!") 171 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/model_predictive_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Model trajectory generator 4 | 5 | author: Atsushi Sakai(@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | 11 | import matplotlib.pyplot as plt 12 | import numpy as np 13 | 14 | import motion_model 15 | 16 | # optimization parameter 17 | max_iter = 100 18 | h = np.array([0.5, 0.02, 0.02]).T # parameter sampling distance 19 | cost_th = 0.1 20 | 21 | show_animation = True 22 | 23 | 24 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): # pragma: no cover 25 | """ 26 | Plot arrow 27 | """ 28 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), 29 | fc=fc, ec=ec, head_width=width, head_length=width) 30 | plt.plot(x, y) 31 | plt.plot(0, 0) 32 | 33 | 34 | def calc_diff(target, x, y, yaw): 35 | d = np.array([target.x - x[-1], 36 | target.y - y[-1], 37 | motion_model.pi_2_pi(target.yaw - yaw[-1])]) 38 | 39 | return d 40 | 41 | 42 | def calc_j(target, p, h, k0): 43 | xp, yp, yawp = motion_model.generate_last_state( 44 | p[0, 0] + h[0], p[1, 0], p[2, 0], k0) 45 | dp = calc_diff(target, [xp], [yp], [yawp]) 46 | xn, yn, yawn = motion_model.generate_last_state( 47 | p[0, 0] - h[0], p[1, 0], p[2, 0], k0) 48 | dn = calc_diff(target, [xn], [yn], [yawn]) 49 | d1 = np.array((dp - dn) / (2.0 * h[0])).reshape(3, 1) 50 | 51 | xp, yp, yawp = motion_model.generate_last_state( 52 | p[0, 0], p[1, 0] + h[1], p[2, 0], k0) 53 | dp = calc_diff(target, [xp], [yp], [yawp]) 54 | xn, yn, yawn = motion_model.generate_last_state( 55 | p[0, 0], p[1, 0] - h[1], p[2, 0], k0) 56 | dn = calc_diff(target, [xn], [yn], [yawn]) 57 | d2 = np.array((dp - dn) / (2.0 * h[1])).reshape(3, 1) 58 | 59 | xp, yp, yawp = motion_model.generate_last_state( 60 | p[0, 0], p[1, 0], p[2, 0] + h[2], k0) 61 | dp = calc_diff(target, [xp], [yp], [yawp]) 62 | xn, yn, yawn = motion_model.generate_last_state( 63 | p[0, 0], p[1, 0], p[2, 0] - h[2], k0) 64 | dn = calc_diff(target, [xn], [yn], [yawn]) 65 | d3 = np.array((dp - dn) / (2.0 * h[2])).reshape(3, 1) 66 | 67 | J = np.hstack((d1, d2, d3)) 68 | 69 | return J 70 | 71 | 72 | def selection_learning_param(dp, p, k0, target): 73 | mincost = float("inf") 74 | mina = 1.0 75 | maxa = 2.0 76 | da = 0.5 77 | 78 | for a in np.arange(mina, maxa, da): 79 | tp = p + a * dp 80 | xc, yc, yawc = motion_model.generate_last_state( 81 | tp[0], tp[1], tp[2], k0) 82 | dc = calc_diff(target, [xc], [yc], [yawc]) 83 | cost = np.linalg.norm(dc) 84 | 85 | if cost <= mincost and a != 0.0: 86 | mina = a 87 | mincost = cost 88 | 89 | # print(mincost, mina) 90 | # input() 91 | 92 | return mina 93 | 94 | 95 | def show_trajectory(target, xc, yc): # pragma: no cover 96 | plt.clf() 97 | plot_arrow(target.x, target.y, target.yaw) 98 | plt.plot(xc, yc, "-r") 99 | plt.axis("equal") 100 | plt.grid(True) 101 | plt.pause(0.1) 102 | 103 | 104 | def optimize_trajectory(target, k0, p): 105 | for i in range(max_iter): 106 | xc, yc, yawc = motion_model.generate_trajectory(p[0], p[1], p[2], k0) 107 | dc = np.array(calc_diff(target, xc, yc, yawc)).reshape(3, 1) 108 | 109 | cost = np.linalg.norm(dc) 110 | if cost <= cost_th: 111 | print("path is ok cost is:" + str(cost)) 112 | break 113 | 114 | J = calc_j(target, p, h, k0) 115 | try: 116 | dp = - np.linalg.inv(J) @ dc 117 | except np.linalg.linalg.LinAlgError: 118 | print("cannot calc path LinAlgError") 119 | xc, yc, yawc, p = None, None, None, None 120 | break 121 | alpha = selection_learning_param(dp, p, k0, target) 122 | 123 | p += alpha * np.array(dp) 124 | # print(p.T) 125 | 126 | if show_animation: # pragma: no cover 127 | show_trajectory(target, xc, yc) 128 | else: 129 | xc, yc, yawc, p = None, None, None, None 130 | print("cannot calc path") 131 | 132 | return xc, yc, yawc, p 133 | 134 | 135 | def test_optimize_trajectory(): # pragma: no cover 136 | 137 | # target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(00.0)) 138 | target = motion_model.State(x=5.0, y=2.0, yaw=np.deg2rad(90.0)) 139 | k0 = 0.0 140 | 141 | init_p = np.array([6.0, 0.0, 0.0]).reshape(3, 1) 142 | 143 | x, y, yaw, p = optimize_trajectory(target, k0, init_p) 144 | 145 | if show_animation: 146 | show_trajectory(target, x, y) 147 | plot_arrow(target.x, target.y, target.yaw) 148 | plt.axis("equal") 149 | plt.grid(True) 150 | plt.show() 151 | 152 | 153 | def main(): # pragma: no cover 154 | print(__file__ + " start!!") 155 | test_optimize_trajectory() 156 | 157 | 158 | if __name__ == '__main__': 159 | main() 160 | -------------------------------------------------------------------------------- /docs/modules/extended_kalman_filter_localization.rst: -------------------------------------------------------------------------------- 1 | 2 | Extended Kalman Filter Localization 3 | ----------------------------------- 4 | 5 | .. code-block:: ipython3 6 | 7 | from IPython.display import Image 8 | Image(filename="ekf.png",width=600) 9 | 10 | 11 | 12 | 13 | .. image:: extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png 14 | :width: 600px 15 | 16 | 17 | 18 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/extended_kalman_filter/animation.gif 19 | :alt: EKF 20 | 21 | EKF 22 | 23 | This is a sensor fusion localization with Extended Kalman Filter(EKF). 24 | 25 | The blue line is true trajectory, the black line is dead reckoning 26 | trajectory, 27 | 28 | the green point is positioning observation (ex. GPS), and the red line 29 | is estimated trajectory with EKF. 30 | 31 | The red ellipse is estimated covariance ellipse with EKF. 32 | 33 | Code: `PythonRobotics/extended_kalman_filter.py at master · 34 | AtsushiSakai/PythonRobotics `__ 35 | 36 | Filter design 37 | ~~~~~~~~~~~~~ 38 | 39 | In this simulation, the robot has a state vector includes 4 states at 40 | time :math:`t`. 41 | 42 | .. math:: \textbf{x}_t=[x_t, y_t, \phi_t, v_t] 43 | 44 | x, y are a 2D x-y position, :math:`\phi` is orientation, and v is 45 | velocity. 46 | 47 | In the code, “xEst” means the state vector. 48 | `code `__ 49 | 50 | And, :math:`P_t` is covariace matrix of the state, 51 | 52 | :math:`Q` is covariance matrix of process noise, 53 | 54 | :math:`R` is covariance matrix of observation noise at time :math:`t` 55 | 56 | 　 57 | 58 | The robot has a speed sensor and a gyro sensor. 59 | 60 | So, the input vecor can be used as each time step 61 | 62 | .. math:: \textbf{u}_t=[v_t, \omega_t] 63 | 64 | Also, the robot has a GNSS sensor, it means that the robot can observe 65 | x-y position at each time. 66 | 67 | .. math:: \textbf{z}_t=[x_t,y_t] 68 | 69 | The input and observation vector includes sensor noise. 70 | 71 | In the code, “observation” function generates the input and observation 72 | vector with noise 73 | `code `__ 74 | 75 | Motion Model 76 | ~~~~~~~~~~~~ 77 | 78 | The robot model is 79 | 80 | .. math:: \dot{x} = vcos(\phi) 81 | 82 | .. math:: \dot{y} = vsin((\phi) 83 | 84 | .. math:: \dot{\phi} = \omega 85 | 86 | So, the motion model is 87 | 88 | .. math:: \textbf{x}_{t+1} = F\textbf{x}_t+B\textbf{u}_t 89 | 90 | where 91 | 92 | :math:`\begin{equation*} F= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ \end{bmatrix} \end{equation*}` 93 | 94 | :math:`\begin{equation*} B= \begin{bmatrix} cos(\phi)dt & 0\\ sin(\phi)dt & 0\\ 0 & dt\\ 1 & 0\\ \end{bmatrix} \end{equation*}` 95 | 96 | :math:`dt` is a time interval. 97 | 98 | This is implemented at 99 | `code `__ 100 | 101 | Its Jacobian matrix is 102 | 103 | :math:`\begin{equation*} J_F= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \frac{d\phi}{dx}& \frac{d\phi}{dy} & \frac{d\phi}{d\phi} & \frac{d\phi}{dv}\\ \frac{dv}{dx}& \frac{dv}{dy} & \frac{dv}{d\phi} & \frac{dv}{dv}\\ \end{bmatrix} \end{equation*}` 104 | 105 | :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & -v sin(\phi)dt & cos(\phi)dt\\ 0 & 1 & v cos(\phi)dt & sin(\phi) dt\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1\\ \end{bmatrix} \end{equation*}` 106 | 107 | Observation Model 108 | ~~~~~~~~~~~~~~~~~ 109 | 110 | The robot can get x-y position infomation from GPS. 111 | 112 | So GPS Observation model is 113 | 114 | .. math:: \textbf{z}_{t} = H\textbf{x}_t 115 | 116 | where 117 | 118 | :math:`\begin{equation*} B= \begin{bmatrix} 1 & 0 & 0& 0\\ 0 & 1 & 0& 0\\ \end{bmatrix} \end{equation*}` 119 | 120 | Its Jacobian matrix is 121 | 122 | :math:`\begin{equation*} J_H= \begin{bmatrix} \frac{dx}{dx}& \frac{dx}{dy} & \frac{dx}{d\phi} & \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy} & \frac{dy}{d\phi} & \frac{dy}{dv}\\ \end{bmatrix} \end{equation*}` 123 | 124 | :math:`\begin{equation*} 　= \begin{bmatrix} 1& 0 & 0 & 0\\ 0 & 1 & 0 & 0\\ \end{bmatrix} \end{equation*}` 125 | 126 | Extented Kalman Filter 127 | ~~~~~~~~~~~~~~~~~~~~~~ 128 | 129 | Localization process using Extendted Kalman Filter:EKF is 130 | 131 | === Predict === 132 | 133 | :math:`x_{Pred} = Fx_t+Bu_t` 134 | 135 | :math:`P_{Pred} = J_FP_t J_F^T + Q` 136 | 137 | === Update === 138 | 139 | :math:`z_{Pred} = Hx_{Pred}` 140 | 141 | :math:`y = z - z_{Pred}` 142 | 143 | :math:`S = J_H P_{Pred}.J_H^T + R` 144 | 145 | :math:`K = P_{Pred}.J_H^T S^{-1}` 146 | 147 | :math:`x_{t+1} = x_{Pred} + Ky` 148 | 149 | :math:`P_{t+1} = ( I - K J_H) P_{Pred}` 150 | 151 | Ref: 152 | ~~~~ 153 | 154 | - `PROBABILISTIC-ROBOTICS.ORG `__ 155 | -------------------------------------------------------------------------------- /Localization/extended_kalman_filter/extended_kalman_filter.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Extended kalman filter (EKF) localization 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 | 13 | # Covariance for EKF simulation 14 | Q = np.diag([ 15 | 0.1, # variance of location on x-axis 16 | 0.1, # variance of location on y-axis 17 | np.deg2rad(1.0), # variance of yaw angle 18 | 1.0 # variance of velocity 19 | ])**2 # predict state covariance 20 | R = np.diag([1.0, 1.0])**2 # Observation x,y position covariance 21 | 22 | # Simulation parameter 23 | INPUT_NOISE = np.diag([1.0, np.deg2rad(30.0)])**2 24 | GPS_NOISE = np.diag([0.5, 0.5])**2 25 | 26 | DT = 0.1 # time tick [s] 27 | SIM_TIME = 50.0 # simulation time [s] 28 | 29 | show_animation = True 30 | 31 | 32 | def calc_input(): 33 | v = 1.0 # [m/s] 34 | yawrate = 0.1 # [rad/s] 35 | u = np.array([[v], [yawrate]]) 36 | return u 37 | 38 | 39 | def observation(xTrue, xd, u): 40 | 41 | xTrue = motion_model(xTrue, u) 42 | 43 | # add noise to gps x-y 44 | z = observation_model(xTrue) + GPS_NOISE @ np.random.randn(2, 1) 45 | 46 | # add noise to input 47 | ud = u + INPUT_NOISE @ np.random.randn(2, 1) 48 | 49 | xd = motion_model(xd, ud) 50 | 51 | return xTrue, z, xd, ud 52 | 53 | 54 | def motion_model(x, u): 55 | 56 | F = np.array([[1.0, 0, 0, 0], 57 | [0, 1.0, 0, 0], 58 | [0, 0, 1.0, 0], 59 | [0, 0, 0, 0]]) 60 | 61 | B = np.array([[DT * math.cos(x[2, 0]), 0], 62 | [DT * math.sin(x[2, 0]), 0], 63 | [0.0, DT], 64 | [1.0, 0.0]]) 65 | 66 | x = F @ x + B @ u 67 | 68 | return x 69 | 70 | 71 | def observation_model(x): 72 | H = np.array([ 73 | [1, 0, 0, 0], 74 | [0, 1, 0, 0] 75 | ]) 76 | 77 | z = H @ x 78 | 79 | return z 80 | 81 | 82 | def jacobF(x, u): 83 | """ 84 | Jacobian of Motion Model 85 | 86 | motion model 87 | x_{t+1} = x_t+v*dt*cos(yaw) 88 | y_{t+1} = y_t+v*dt*sin(yaw) 89 | yaw_{t+1} = yaw_t+omega*dt 90 | v_{t+1} = v{t} 91 | so 92 | dx/dyaw = -v*dt*sin(yaw) 93 | dx/dv = dt*cos(yaw) 94 | dy/dyaw = v*dt*cos(yaw) 95 | dy/dv = dt*sin(yaw) 96 | """ 97 | yaw = x[2, 0] 98 | v = u[0, 0] 99 | jF = np.array([ 100 | [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)], 101 | [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)], 102 | [0.0, 0.0, 1.0, 0.0], 103 | [0.0, 0.0, 0.0, 1.0]]) 104 | 105 | return jF 106 | 107 | 108 | def jacobH(x): 109 | # Jacobian of Observation Model 110 | jH = np.array([ 111 | [1, 0, 0, 0], 112 | [0, 1, 0, 0] 113 | ]) 114 | 115 | return jH 116 | 117 | 118 | def ekf_estimation(xEst, PEst, z, u): 119 | 120 | # Predict 121 | xPred = motion_model(xEst, u) 122 | jF = jacobF(xPred, u) 123 | PPred = jF@PEst@jF.T + Q 124 | 125 | # Update 126 | jH = jacobH(xPred) 127 | zPred = observation_model(xPred) 128 | y = z - zPred 129 | S = jH@PPred@jH.T + R 130 | K = PPred@jH.T@np.linalg.inv(S) 131 | xEst = xPred + K@y 132 | PEst = (np.eye(len(xEst)) - K@jH)@PPred 133 | 134 | return xEst, PEst 135 | 136 | 137 | def plot_covariance_ellipse(xEst, PEst): # pragma: no cover 138 | Pxy = PEst[0:2, 0:2] 139 | eigval, eigvec = np.linalg.eig(Pxy) 140 | 141 | if eigval[0] >= eigval[1]: 142 | bigind = 0 143 | smallind = 1 144 | else: 145 | bigind = 1 146 | smallind = 0 147 | 148 | t = np.arange(0, 2 * math.pi + 0.1, 0.1) 149 | a = math.sqrt(eigval[bigind]) 150 | b = math.sqrt(eigval[smallind]) 151 | x = [a * math.cos(it) for it in t] 152 | y = [b * math.sin(it) for it in t] 153 | angle = math.atan2(eigvec[bigind, 1], eigvec[bigind, 0]) 154 | R = np.array([[math.cos(angle), math.sin(angle)], 155 | [-math.sin(angle), math.cos(angle)]]) 156 | fx = R@(np.array([x, y])) 157 | px = np.array(fx[0, :] + xEst[0, 0]).flatten() 158 | py = np.array(fx[1, :] + xEst[1, 0]).flatten() 159 | plt.plot(px, py, "--r") 160 | 161 | 162 | def main(): 163 | print(__file__ + " start!!") 164 | 165 | time = 0.0 166 | 167 | # State Vector [x y yaw v]' 168 | xEst = np.zeros((4, 1)) 169 | xTrue = np.zeros((4, 1)) 170 | PEst = np.eye(4) 171 | 172 | xDR = np.zeros((4, 1)) # Dead reckoning 173 | 174 | # history 175 | hxEst = xEst 176 | hxTrue = xTrue 177 | hxDR = xTrue 178 | hz = np.zeros((2, 1)) 179 | 180 | while SIM_TIME >= time: 181 | time += DT 182 | u = calc_input() 183 | 184 | xTrue, z, xDR, ud = observation(xTrue, xDR, u) 185 | 186 | xEst, PEst = ekf_estimation(xEst, PEst, z, ud) 187 | 188 | # store data history 189 | hxEst = np.hstack((hxEst, xEst)) 190 | hxDR = np.hstack((hxDR, xDR)) 191 | hxTrue = np.hstack((hxTrue, xTrue)) 192 | hz = np.hstack((hz, z)) 193 | 194 | if show_animation: 195 | plt.cla() 196 | plt.plot(hz[0, :], hz[1, :], ".g") 197 | plt.plot(hxTrue[0, :].flatten(), 198 | hxTrue[1, :].flatten(), "-b") 199 | plt.plot(hxDR[0, :].flatten(), 200 | hxDR[1, :].flatten(), "-k") 201 | plt.plot(hxEst[0, :].flatten(), 202 | hxEst[1, :].flatten(), "-r") 203 | plot_covariance_ellipse(xEst, PEst) 204 | plt.axis("equal") 205 | plt.grid(True) 206 | plt.pause(0.001) 207 | 208 | 209 | if __name__ == '__main__': 210 | main() 211 | -------------------------------------------------------------------------------- /docs/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # Configuration file for the Sphinx documentation builder. 4 | # 5 | # This file does only contain a selection of the most common options. For a 6 | # full list see the documentation: 7 | # http://www.sphinx-doc.org/en/master/config 8 | 9 | # -- Path setup -------------------------------------------------------------- 10 | 11 | # If extensions (or modules to document with autodoc) are in another directory, 12 | # add these directories to sys.path here. If the directory is relative to the 13 | # documentation root, use os.path.abspath to make it absolute, like shown here. 14 | # 15 | import os 16 | # import sys 17 | # sys.path.insert(0, os.path.abspath('.')) 18 | 19 | 20 | # -- Project information ----------------------------------------------------- 21 | 22 | project = 'PythonRobotics' 23 | copyright = '2018, Atsushi Sakai' 24 | author = 'Atsushi Sakai' 25 | 26 | # The short X.Y version 27 | version = '' 28 | # The full version, including alpha/beta/rc tags 29 | release = '' 30 | 31 | 32 | # -- General configuration --------------------------------------------------- 33 | 34 | # If your documentation needs a minimal Sphinx version, state it here. 35 | # 36 | # needs_sphinx = '1.0' 37 | 38 | # Add any Sphinx extension module names here, as strings. They can be 39 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 40 | # ones. 41 | extensions = [ 42 | 'sphinx.ext.autodoc', 43 | 'sphinx.ext.mathjax', 44 | 'sphinx.ext.viewcode', 45 | 'sphinx.ext.viewcode', 46 | ] 47 | 48 | # Add any paths that contain templates here, relative to this directory. 49 | templates_path = ['_templates'] 50 | 51 | # The suffix(es) of source filenames. 52 | # You can specify multiple suffix as a list of string: 53 | # 54 | # source_suffix = ['.rst', '.md'] 55 | source_suffix = '.rst' 56 | 57 | # The master toctree document. 58 | master_doc = 'index' 59 | 60 | # The language for content autogenerated by Sphinx. Refer to documentation 61 | # for a list of supported languages. 62 | # 63 | # This is also used if you do content translation via gettext catalogs. 64 | # Usually you set "language" from the command line for these cases. 65 | language = None 66 | 67 | # List of patterns, relative to source directory, that match files and 68 | # directories to ignore when looking for source files. 69 | # This pattern also affects html_static_path and html_extra_path . 70 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 71 | 72 | # The name of the Pygments (syntax highlighting) style to use. 73 | pygments_style = 'sphinx' 74 | 75 | 76 | # -- Options for HTML output ------------------------------------------------- 77 | 78 | # The theme to use for HTML and HTML Help pages. See the documentation for 79 | # a list of builtin themes. 80 | # 81 | # Fix for read the docs 82 | on_rtd = os.environ.get('READTHEDOCS') == 'True' 83 | if on_rtd: 84 | html_theme = 'default' 85 | else: 86 | html_theme = 'sphinx_rtd_theme' 87 | 88 | # Theme options are theme-specific and customize the look and feel of a theme 89 | # further. For a list of options available for each theme, see the 90 | # documentation. 91 | # 92 | html_logo = '../icon.png' 93 | html_theme_options = { 94 | 'display_version': False, 95 | } 96 | 97 | # Add any paths that contain custom static files (such as style sheets) here, 98 | # relative to this directory. They are copied after the builtin static files, 99 | # so a file named "default.css" will overwrite the builtin "default.css". 100 | html_static_path = ['_static'] 101 | 102 | # Custom sidebar templates, must be a dictionary that maps document names 103 | # to template names. 104 | # 105 | # The default sidebars (for documents that don't match any pattern) are 106 | # defined by theme itself. Builtin themes are using these templates by 107 | # default: ``['localtoc.html', 'relations.html', 'sourcelink.html', 108 | # 'searchbox.html']``. 109 | # 110 | # html_sidebars = {} 111 | 112 | 113 | # -- Options for HTMLHelp output --------------------------------------------- 114 | 115 | # Output file base name for HTML help builder. 116 | htmlhelp_basename = 'PythonRoboticsdoc' 117 | 118 | 119 | # -- Options for LaTeX output ------------------------------------------------ 120 | 121 | latex_elements = { 122 | # The paper size ('letterpaper' or 'a4paper'). 123 | # 124 | # 'papersize': 'letterpaper', 125 | 126 | # The font size ('10pt', '11pt' or '12pt'). 127 | # 128 | # 'pointsize': '10pt', 129 | 130 | # Additional stuff for the LaTeX preamble. 131 | # 132 | # 'preamble': '', 133 | 134 | # Latex figure (float) alignment 135 | # 136 | # 'figure_align': 'htbp', 137 | } 138 | 139 | # Grouping the document tree into LaTeX files. List of tuples 140 | # (source start file, target name, title, 141 | # author, documentclass [howto, manual, or own class]). 142 | latex_documents = [ 143 | (master_doc, 'PythonRobotics.tex', 'PythonRobotics Documentation', 144 | 'Atsushi Sakai', 'manual'), 145 | ] 146 | 147 | 148 | # -- Options for manual page output ------------------------------------------ 149 | 150 | # One entry per manual page. List of tuples 151 | # (source start file, name, description, authors, manual section). 152 | man_pages = [ 153 | (master_doc, 'pythonrobotics', 'PythonRobotics Documentation', 154 | [author], 1) 155 | ] 156 | 157 | 158 | # -- Options for Texinfo output ---------------------------------------------- 159 | 160 | # Grouping the document tree into Texinfo files. List of tuples 161 | # (source start file, target name, title, author, 162 | # dir menu entry, description, category) 163 | texinfo_documents = [ 164 | (master_doc, 'PythonRobotics', 'PythonRobotics Documentation', 165 | author, 'PythonRobotics', 'One line description of project.', 166 | 'Miscellaneous'), 167 | ] 168 | 169 | 170 | # -- Extension configuration ------------------------------------------------- 171 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/n_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse kinematics for an n-link arm using the Jacobian inverse method 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | Atsushi Sakai (@Atsushi_twi) 6 | """ 7 | import numpy as np 8 | 9 | from NLinkArm import NLinkArm 10 | 11 | # Simulation parameters 12 | Kp = 2 13 | dt = 0.1 14 | N_LINKS = 10 15 | N_ITERATIONS = 10000 16 | 17 | # States 18 | WAIT_FOR_NEW_GOAL = 1 19 | MOVING_TO_GOAL = 2 20 | 21 | show_animation = True 22 | 23 | 24 | def main(): # pragma: no cover 25 | """ 26 | Creates an arm using the NLinkArm class and uses its inverse kinematics 27 | to move it to the desired position. 28 | """ 29 | link_lengths = [1] * N_LINKS 30 | joint_angles = np.array([0] * N_LINKS) 31 | goal_pos = [N_LINKS, 0] 32 | arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) 33 | state = WAIT_FOR_NEW_GOAL 34 | solution_found = False 35 | while True: 36 | old_goal = np.array(goal_pos) 37 | goal_pos = np.array(arm.goal) 38 | end_effector = arm.end_effector 39 | errors, distance = distance_to_goal(end_effector, goal_pos) 40 | 41 | # State machine to allow changing of goal before current goal has been reached 42 | if state is WAIT_FOR_NEW_GOAL: 43 | if distance > 0.1 and not solution_found: 44 | joint_goal_angles, solution_found = inverse_kinematics( 45 | link_lengths, joint_angles, goal_pos) 46 | if not solution_found: 47 | print("Solution could not be found.") 48 | state = WAIT_FOR_NEW_GOAL 49 | arm.goal = end_effector 50 | elif solution_found: 51 | state = MOVING_TO_GOAL 52 | elif state is MOVING_TO_GOAL: 53 | if distance > 0.1 and all(old_goal == goal_pos): 54 | joint_angles = joint_angles + Kp * \ 55 | ang_diff(joint_goal_angles, joint_angles) * dt 56 | else: 57 | state = WAIT_FOR_NEW_GOAL 58 | solution_found = False 59 | 60 | arm.update_joints(joint_angles) 61 | 62 | 63 | def inverse_kinematics(link_lengths, joint_angles, goal_pos): 64 | """ 65 | Calculates the inverse kinematics using the Jacobian inverse method. 66 | """ 67 | for iteration in range(N_ITERATIONS): 68 | current_pos = forward_kinematics(link_lengths, joint_angles) 69 | errors, distance = distance_to_goal(current_pos, goal_pos) 70 | if distance < 0.1: 71 | print("Solution found in %d iterations." % iteration) 72 | return joint_angles, True 73 | J = jacobian_inverse(link_lengths, joint_angles) 74 | joint_angles = joint_angles + np.matmul(J, errors) 75 | return joint_angles, False 76 | 77 | 78 | def get_random_goal(): 79 | from random import random 80 | SAREA = 15.0 81 | return [SAREA * random() - SAREA / 2.0, 82 | SAREA * random() - SAREA / 2.0] 83 | 84 | 85 | def animation(): 86 | link_lengths = [1] * N_LINKS 87 | joint_angles = np.array([0] * N_LINKS) 88 | goal_pos = get_random_goal() 89 | arm = NLinkArm(link_lengths, joint_angles, goal_pos, show_animation) 90 | state = WAIT_FOR_NEW_GOAL 91 | solution_found = False 92 | 93 | i_goal = 0 94 | while True: 95 | old_goal = np.array(goal_pos) 96 | goal_pos = np.array(arm.goal) 97 | end_effector = arm.end_effector 98 | errors, distance = distance_to_goal(end_effector, goal_pos) 99 | 100 | # State machine to allow changing of goal before current goal has been reached 101 | if state is WAIT_FOR_NEW_GOAL: 102 | 103 | if distance > 0.1 and not solution_found: 104 | joint_goal_angles, solution_found = inverse_kinematics( 105 | link_lengths, joint_angles, goal_pos) 106 | if not solution_found: 107 | print("Solution could not be found.") 108 | state = WAIT_FOR_NEW_GOAL 109 | arm.goal = get_random_goal() 110 | elif solution_found: 111 | state = MOVING_TO_GOAL 112 | elif state is MOVING_TO_GOAL: 113 | if distance > 0.1 and all(old_goal == goal_pos): 114 | joint_angles = joint_angles + Kp * \ 115 | ang_diff(joint_goal_angles, joint_angles) * dt 116 | else: 117 | state = WAIT_FOR_NEW_GOAL 118 | solution_found = False 119 | arm.goal = get_random_goal() 120 | i_goal += 1 121 | 122 | if i_goal >= 5: 123 | break 124 | 125 | arm.update_joints(joint_angles) 126 | 127 | 128 | def forward_kinematics(link_lengths, joint_angles): 129 | x = y = 0 130 | for i in range(1, N_LINKS + 1): 131 | x += link_lengths[i - 1] * np.cos(np.sum(joint_angles[:i])) 132 | y += link_lengths[i - 1] * np.sin(np.sum(joint_angles[:i])) 133 | return np.array([x, y]).T 134 | 135 | 136 | def jacobian_inverse(link_lengths, joint_angles): 137 | J = np.zeros((2, N_LINKS)) 138 | for i in range(N_LINKS): 139 | J[0, i] = 0 140 | J[1, i] = 0 141 | for j in range(i, N_LINKS): 142 | J[0, i] -= link_lengths[j] * np.sin(np.sum(joint_angles[:j])) 143 | J[1, i] += link_lengths[j] * np.cos(np.sum(joint_angles[:j])) 144 | 145 | return np.linalg.pinv(J) 146 | 147 | 148 | def distance_to_goal(current_pos, goal_pos): 149 | x_diff = goal_pos[0] - current_pos[0] 150 | y_diff = goal_pos[1] - current_pos[1] 151 | return np.array([x_diff, y_diff]).T, np.math.sqrt(x_diff**2 + y_diff**2) 152 | 153 | 154 | def ang_diff(theta1, theta2): 155 | """ 156 | Returns the difference between two angles in the range -pi to +pi 157 | """ 158 | return (theta1 - theta2 + np.pi) % (2 * np.pi) - np.pi 159 | 160 | 161 | if __name__ == '__main__': 162 | # main() 163 | animation() -------------------------------------------------------------------------------- /PathTracking/pure_pursuit/pure_pursuit.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path tracking simulation with pure pursuit steering control and PID speed control. 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | import numpy as np 9 | import math 10 | import matplotlib.pyplot as plt 11 | 12 | 13 | k = 0.1 # look forward gain 14 | Lfc = 2.0 # look-ahead distance 15 | Kp = 1.0 # speed proportional gain 16 | dt = 0.1 # [s] 17 | L = 2.9 # [m] wheel base of vehicle 18 | 19 | 20 | old_nearest_point_index = None 21 | show_animation = True 22 | 23 | 24 | class State: 25 | 26 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 27 | self.x = x 28 | self.y = y 29 | self.yaw = yaw 30 | self.v = v 31 | self.rear_x = self.x - ((L / 2) * math.cos(self.yaw)) 32 | self.rear_y = self.y - ((L / 2) * math.sin(self.yaw)) 33 | 34 | 35 | def update(state, a, delta): 36 | 37 | state.x = state.x + state.v * math.cos(state.yaw) * dt 38 | state.y = state.y + state.v * math.sin(state.yaw) * dt 39 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 40 | state.v = state.v + a * dt 41 | state.rear_x = state.x - ((L / 2) * math.cos(state.yaw)) 42 | state.rear_y = state.y - ((L / 2) * math.sin(state.yaw)) 43 | 44 | return state 45 | 46 | 47 | def PIDControl(target, current): 48 | a = Kp * (target - current) 49 | 50 | return a 51 | 52 | 53 | def pure_pursuit_control(state, cx, cy, pind): 54 | 55 | ind = calc_target_index(state, cx, cy) 56 | 57 | if pind >= ind: 58 | ind = pind 59 | 60 | if ind < len(cx): 61 | tx = cx[ind] 62 | ty = cy[ind] 63 | else: 64 | tx = cx[-1] 65 | ty = cy[-1] 66 | ind = len(cx) - 1 67 | 68 | alpha = math.atan2(ty - state.rear_y, tx - state.rear_x) - state.yaw 69 | 70 | Lf = k * state.v + Lfc 71 | 72 | delta = math.atan2(2.0 * L * math.sin(alpha) / Lf, 1.0) 73 | 74 | return delta, ind 75 | 76 | def calc_distance(state, point_x, point_y): 77 | 78 | dx = state.rear_x - point_x 79 | dy = state.rear_y - point_y 80 | return math.sqrt(dx ** 2 + dy ** 2) 81 | 82 | 83 | def calc_target_index(state, cx, cy): 84 | 85 | global old_nearest_point_index 86 | 87 | if old_nearest_point_index is None: 88 | # search nearest point index 89 | dx = [state.rear_x - icx for icx in cx] 90 | dy = [state.rear_y - icy for icy in cy] 91 | d = [abs(math.sqrt(idx ** 2 + idy ** 2)) for (idx, idy) in zip(dx, dy)] 92 | ind = d.index(min(d)) 93 | old_nearest_point_index = ind 94 | else: 95 | ind = old_nearest_point_index 96 | distance_this_index = calc_distance(state, cx[ind], cy[ind]) 97 | while True: 98 | ind = ind + 1 if (ind + 1) < len(cx) else ind 99 | distance_next_index = calc_distance(state, cx[ind], cy[ind]) 100 | if distance_this_index < distance_next_index: 101 | break 102 | distance_this_index = distance_next_index 103 | old_nearest_point_index = ind 104 | 105 | L = 0.0 106 | 107 | Lf = k * state.v + Lfc 108 | 109 | # search look ahead target point index 110 | while Lf > L and (ind + 1) < len(cx): 111 | dx = cx[ind] - state.rear_x 112 | dy = cy[ind] - state.rear_y 113 | L = math.sqrt(dx ** 2 + dy ** 2) 114 | ind += 1 115 | 116 | return ind 117 | 118 | 119 | def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): 120 | """ 121 | Plot arrow 122 | """ 123 | 124 | if not isinstance(x, float): 125 | for (ix, iy, iyaw) in zip(x, y, yaw): 126 | plot_arrow(ix, iy, iyaw) 127 | else: 128 | plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), 129 | fc=fc, ec=ec, head_width=width, head_length=width) 130 | plt.plot(x, y) 131 | 132 | 133 | def main(): 134 | # target course 135 | cx = np.arange(0, 50, 0.1) 136 | cy = [math.sin(ix / 5.0) * ix / 2.0 for ix in cx] 137 | 138 | target_speed = 10.0 / 3.6 # [m/s] 139 | 140 | T = 100.0 # max simulation time 141 | 142 | # initial state 143 | state = State(x=-0.0, y=-3.0, yaw=0.0, v=0.0) 144 | 145 | lastIndex = len(cx) - 1 146 | time = 0.0 147 | x = [state.x] 148 | y = [state.y] 149 | yaw = [state.yaw] 150 | v = [state.v] 151 | t = [0.0] 152 | target_ind = calc_target_index(state, cx, cy) 153 | 154 | while T >= time and lastIndex > target_ind: 155 | ai = PIDControl(target_speed, state.v) 156 | di, target_ind = pure_pursuit_control(state, cx, cy, target_ind) 157 | state = update(state, ai, di) 158 | 159 | time = time + dt 160 | 161 | x.append(state.x) 162 | y.append(state.y) 163 | yaw.append(state.yaw) 164 | v.append(state.v) 165 | t.append(time) 166 | 167 | if show_animation: # pragma: no cover 168 | plt.cla() 169 | plot_arrow(state.x, state.y, state.yaw) 170 | plt.plot(cx, cy, "-r", label="course") 171 | plt.plot(x, y, "-b", label="trajectory") 172 | plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") 173 | plt.axis("equal") 174 | plt.grid(True) 175 | plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) 176 | plt.pause(0.001) 177 | 178 | # Test 179 | assert lastIndex >= target_ind, "Cannot goal" 180 | 181 | if show_animation: # pragma: no cover 182 | plt.cla() 183 | plt.plot(cx, cy, ".r", label="course") 184 | plt.plot(x, y, "-b", label="trajectory") 185 | plt.legend() 186 | plt.xlabel("x[m]") 187 | plt.ylabel("y[m]") 188 | plt.axis("equal") 189 | plt.grid(True) 190 | 191 | plt.subplots(1) 192 | plt.plot(t, [iv * 3.6 for iv in v], "-r") 193 | plt.xlabel("Time[s]") 194 | plt.ylabel("Speed[km/h]") 195 | plt.grid(True) 196 | plt.show() 197 | 198 | 199 | if __name__ == '__main__': 200 | print("Pure pursuit path tracking simulation start") 201 | main() 202 | -------------------------------------------------------------------------------- /docs/modules/Model_predictive_speed_and_steering_control.rst: -------------------------------------------------------------------------------- 1 | 2 | Model predictive speed and steering control 3 | ------------------------------------------- 4 | 5 | .. figure:: https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/PathTracking/model_predictive_speed_and_steer_control/animation.gif?raw=true 6 | :alt: Model predictive speed and steering control 7 | 8 | Model predictive speed and steering control 9 | 10 | code: 11 | 12 | `PythonRobotics/model_predictive_speed_and_steer_control.py at master · 13 | AtsushiSakai/PythonRobotics `__ 14 | 15 | This is a path tracking simulation using model predictive control (MPC). 16 | 17 | The MPC controller controls vehicle speed and steering base on 18 | linealized model. 19 | 20 | This code uses cvxpy as an optimization modeling tool. 21 | 22 | - `Welcome to CVXPY 1.0 — CVXPY 1.0.6 23 | documentation `__ 24 | 25 | MPC modeling 26 | ~~~~~~~~~~~~ 27 | 28 | State vector is: 29 | 30 | .. math:: z = [x, y, v,\phi] 31 | 32 | x: x-position, y:y-position, v:velocity, φ: yaw angle 33 | 34 | Input vector is: 35 | 36 | .. math:: u = [a, \delta] 37 | 38 | a: accellation, δ: steering angle 39 | 40 | The MPC cotroller minimize this cost function for path tracking: 41 | 42 | .. math:: min\ Q_f(z_{T,ref}-z_{T})^2+Q\Sigma({z_{t,ref}-z_{t}})^2+R\Sigma{u_t}^2+R_d\Sigma({u_{t+1}-u_{t}})^2 43 | 44 | z_ref come from target path and speed. 45 | 46 | subject to: 47 | 48 | - Linearlied vehicle model 49 | 50 | .. math:: z_{t+1}=Az_t+Bu+C 51 | 52 | - Maximum steering speed 53 | 54 | .. math:: |u_{t+1}-u_{t}|`__ 77 | 78 | Vehicle model linearization 79 | ~~~~~~~~~~~~~~~~~~~~~~~~~~~ 80 | 81 | Vehicle model is 82 | 83 | .. math:: \dot{x} = vcos(\phi) 84 | 85 | .. math:: \dot{y} = vsin((\phi) 86 | 87 | .. math:: \dot{v} = a 88 | 89 | .. math:: \dot{\phi} = \frac{vtan(\delta)}{L} 90 | 91 | ODE is 92 | 93 | .. math:: \dot{z} =\frac{\partial }{\partial z} z = f(z, u) = A'z+B'u 94 | 95 | where 96 | 97 | :math:`\begin{equation*} A' = \begin{bmatrix} \frac{\partial }{\partial x}vcos(\phi) & \frac{\partial }{\partial y}vcos(\phi) & \frac{\partial }{\partial v}vcos(\phi) & \frac{\partial }{\partial \phi}vcos(\phi)\\ \frac{\partial }{\partial x}vsin(\phi) & \frac{\partial }{\partial y}vsin(\phi) & \frac{\partial }{\partial v}vsin(\phi) & \frac{\partial }{\partial \phi}vsin(\phi)\\ \frac{\partial }{\partial x}a& \frac{\partial }{\partial y}a& \frac{\partial }{\partial v}a& \frac{\partial }{\partial \phi}a\\ \frac{\partial }{\partial x}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial y}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial v}\frac{vtan(\delta)}{L}& \frac{\partial }{\partial \phi}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 & cos(\bar{\phi}) & -\bar{v}sin(\bar{\phi})\\ 0 & 0 & sin(\bar{\phi}) & \bar{v}cos(\bar{\phi}) \\ 0 & 0 & 0 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L} & 0 \\ \end{bmatrix} \end{equation*}` 98 | 99 | :math:`\begin{equation*} B' = \begin{bmatrix} \frac{\partial }{\partial a}vcos(\phi) & \frac{\partial }{\partial \delta}vcos(\phi)\\ \frac{\partial }{\partial a}vsin(\phi) & \frac{\partial }{\partial \delta}vsin(\phi)\\ \frac{\partial }{\partial a}a & \frac{\partial }{\partial \delta}a\\ \frac{\partial }{\partial a}\frac{vtan(\delta)}{L} & \frac{\partial }{\partial \delta}\frac{vtan(\delta)}{L}\\ \end{bmatrix} \\ 　= \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ 1 & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})} \\ \end{bmatrix} \end{equation*}` 100 | 101 | You can get a discrete-time mode with Forward Euler Discretization with 102 | sampling time dt. 103 | 104 | .. math:: z_{k+1}=z_k+f(z_k,u_k)dt 105 | 106 | Using first degree Tayer expantion around zbar and ubar 107 | 108 | .. math:: z_{k+1}=z_k+(f(\bar{z},\bar{u})+A'z_k+B'u_k-A'\bar{z}-B'\bar{u})dt 109 | 110 | .. math:: z_{k+1}=(I + dtA')z_k+(dtB')u_k + (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt 111 | 112 | So, 113 | 114 | .. math:: z_{k+1}=Az_k+Bu_k +C 115 | 116 | where, 117 | 118 | :math:`\begin{equation*} A = (I + dtA')\\ = \begin{bmatrix} 1 & 0 & cos(\bar{\phi})dt & -\bar{v}sin(\bar{\phi})dt\\ 0 & 1 & sin(\bar{\phi})dt & \bar{v}cos(\bar{\phi})dt \\ 0 & 0 & 1 & 0 \\ 0 & 0 &\frac{tan(\bar{\delta})}{L}dt & 1 \\ \end{bmatrix} \end{equation*}` 119 | 120 | :math:`\begin{equation*} B = dtB'\\ = \begin{bmatrix} 0 & 0 \\ 0 & 0 \\ dt & 0 \\ 0 & \frac{\bar{v}}{Lcos^2(\bar{\delta})}dt \\ \end{bmatrix} \end{equation*}` 121 | 122 | :math:`\begin{equation*} C = (f(\bar{z},\bar{u})-A'\bar{z}-B'\bar{u})dt\\ = dt( \begin{bmatrix} \bar{v}cos(\bar{\phi})\\ \bar{v}sin(\bar{\phi}) \\ \bar{a}\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} \bar{v}cos(\bar{\phi})-\bar{v}sin(\bar{\phi})\bar{\phi}\\ \bar{v}sin(\bar{\phi})+\bar{v}cos(\bar{\phi})\bar{\phi}\\ 0\\ \frac{\bar{v}tan(\bar{\delta})}{L}\\ \end{bmatrix} - \begin{bmatrix} 0\\ 0 \\ \bar{a}\\ \frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}\\ \end{bmatrix} )\\ = \begin{bmatrix} \bar{v}sin(\bar{\phi})\bar{\phi}dt\\ -\bar{v}cos(\bar{\phi})\bar{\phi}dt\\ 0\\ -\frac{\bar{v}\bar{\delta}}{Lcos^2(\bar{\delta})}dt\\ \end{bmatrix} \end{equation*}` 123 | 124 | This equation is implemented at 125 | 126 | `PythonRobotics/model_predictive_speed_and_steer_control.py at 127 | eb6d1cbe6fc90c7be9210bf153b3a04f177cc138 · 128 | AtsushiSakai/PythonRobotics `__ 129 | 130 | Reference 131 | ~~~~~~~~~ 132 | 133 | - `Vehicle Dynamics and Control \| Rajesh Rajamani \| 134 | Springer `__ 135 | 136 | - `MPC Course Material - MPC Lab @ 137 | UC-Berkeley `__ 138 | -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/drone_3d_trajectory_following.py: -------------------------------------------------------------------------------- 1 | """ 2 | Simulate a quadrotor following a 3D trajectory 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | from Quadrotor import Quadrotor 10 | from TrajectoryGenerator import TrajectoryGenerator 11 | from mpl_toolkits.mplot3d import Axes3D 12 | 13 | show_animation = True 14 | 15 | # Simulation parameters 16 | g = 9.81 17 | m = 0.2 18 | Ixx = 1 19 | Iyy = 1 20 | Izz = 1 21 | T = 5 22 | 23 | # Proportional coefficients 24 | Kp_x = 1 25 | Kp_y = 1 26 | Kp_z = 1 27 | Kp_roll = 25 28 | Kp_pitch = 25 29 | Kp_yaw = 25 30 | 31 | # Derivative coefficients 32 | Kd_x = 10 33 | Kd_y = 10 34 | Kd_z = 1 35 | 36 | 37 | def quad_sim(x_c, y_c, z_c): 38 | """ 39 | Calculates the necessary thrust and torques for the quadrotor to 40 | follow the trajectory described by the sets of coefficients 41 | x_c, y_c, and z_c. 42 | """ 43 | x_pos = -5 44 | y_pos = -5 45 | z_pos = 5 46 | x_vel = 0 47 | y_vel = 0 48 | z_vel = 0 49 | x_acc = 0 50 | y_acc = 0 51 | z_acc = 0 52 | roll = 0 53 | pitch = 0 54 | yaw = 0 55 | roll_vel = 0 56 | pitch_vel = 0 57 | yaw_vel = 0 58 | 59 | des_yaw = 0 60 | 61 | dt = 0.1 62 | t = 0 63 | 64 | q = Quadrotor(x=x_pos, y=y_pos, z=z_pos, roll=roll, 65 | pitch=pitch, yaw=yaw, size=1, show_animation=show_animation) 66 | 67 | i = 0 68 | n_run = 8 69 | irun = 0 70 | 71 | while True: 72 | while t <= T: 73 | # des_x_pos = calculate_position(x_c[i], t) 74 | # des_y_pos = calculate_position(y_c[i], t) 75 | des_z_pos = calculate_position(z_c[i], t) 76 | # des_x_vel = calculate_velocity(x_c[i], t) 77 | # des_y_vel = calculate_velocity(y_c[i], t) 78 | des_z_vel = calculate_velocity(z_c[i], t) 79 | des_x_acc = calculate_acceleration(x_c[i], t) 80 | des_y_acc = calculate_acceleration(y_c[i], t) 81 | des_z_acc = calculate_acceleration(z_c[i], t) 82 | 83 | thrust = m * (g + des_z_acc + Kp_z * (des_z_pos - 84 | z_pos) + Kd_z * (des_z_vel - z_vel)) 85 | 86 | roll_torque = Kp_roll * \ 87 | (((des_x_acc * sin(des_yaw) - des_y_acc * cos(des_yaw)) / g) - roll) 88 | pitch_torque = Kp_pitch * \ 89 | (((des_x_acc * cos(des_yaw) - des_y_acc * sin(des_yaw)) / g) - pitch) 90 | yaw_torque = Kp_yaw * (des_yaw - yaw) 91 | 92 | roll_vel += roll_torque * dt / Ixx 93 | pitch_vel += pitch_torque * dt / Iyy 94 | yaw_vel += yaw_torque * dt / Izz 95 | 96 | roll += roll_vel * dt 97 | pitch += pitch_vel * dt 98 | yaw += yaw_vel * dt 99 | 100 | R = rotation_matrix(roll, pitch, yaw) 101 | acc = (np.matmul(R, np.array( 102 | [0, 0, thrust]).T) - np.array([0, 0, m * g]).T) / m 103 | x_acc = acc[0] 104 | y_acc = acc[1] 105 | z_acc = acc[2] 106 | x_vel += x_acc * dt 107 | y_vel += y_acc * dt 108 | z_vel += z_acc * dt 109 | x_pos += x_vel * dt 110 | y_pos += y_vel * dt 111 | z_pos += z_vel * dt 112 | 113 | q.update_pose(x_pos, y_pos, z_pos, roll, pitch, yaw) 114 | 115 | t += dt 116 | 117 | t = 0 118 | i = (i + 1) % 4 119 | irun += 1 120 | if irun >= n_run: 121 | break 122 | 123 | print("Done") 124 | 125 | 126 | def calculate_position(c, t): 127 | """ 128 | Calculates a position given a set of quintic coefficients and a time. 129 | 130 | Args 131 | c: List of coefficients generated by a quintic polynomial 132 | trajectory generator. 133 | t: Time at which to calculate the position 134 | 135 | Returns 136 | Position 137 | """ 138 | return c[0] * t**5 + c[1] * t**4 + c[2] * t**3 + c[3] * t**2 + c[4] * t + c[5] 139 | 140 | 141 | def calculate_velocity(c, t): 142 | """ 143 | Calculates a velocity given a set of quintic coefficients and a time. 144 | 145 | Args 146 | c: List of coefficients generated by a quintic polynomial 147 | trajectory generator. 148 | t: Time at which to calculate the velocity 149 | 150 | Returns 151 | Velocity 152 | """ 153 | return 5 * c[0] * t**4 + 4 * c[1] * t**3 + 3 * c[2] * t**2 + 2 * c[3] * t + c[4] 154 | 155 | 156 | def calculate_acceleration(c, t): 157 | """ 158 | Calculates an acceleration given a set of quintic coefficients and a time. 159 | 160 | Args 161 | c: List of coefficients generated by a quintic polynomial 162 | trajectory generator. 163 | t: Time at which to calculate the acceleration 164 | 165 | Returns 166 | Acceleration 167 | """ 168 | return 20 * c[0] * t**3 + 12 * c[1] * t**2 + 6 * c[2] * t + 2 * c[3] 169 | 170 | 171 | def rotation_matrix(roll, pitch, yaw): 172 | """ 173 | Calculates the ZYX rotation matrix. 174 | 175 | Args 176 | Roll: Angular position about the x-axis in radians. 177 | Pitch: Angular position about the y-axis in radians. 178 | Yaw: Angular position about the z-axis in radians. 179 | 180 | Returns 181 | 3x3 rotation matrix as NumPy array 182 | """ 183 | return np.array( 184 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll)], 185 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) * 186 | sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll)], 187 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw)] 188 | ]) 189 | 190 | 191 | def main(): 192 | """ 193 | Calculates the x, y, z coefficients for the four segments 194 | of the trajectory 195 | """ 196 | x_coeffs = [[], [], [], []] 197 | y_coeffs = [[], [], [], []] 198 | z_coeffs = [[], [], [], []] 199 | waypoints = [[-5, -5, 5], [5, -5, 5], [5, 5, 5], [-5, 5, 5]] 200 | 201 | for i in range(4): 202 | traj = TrajectoryGenerator(waypoints[i], waypoints[(i + 1) % 4], T) 203 | traj.solve() 204 | x_coeffs[i] = traj.x_c 205 | y_coeffs[i] = traj.y_c 206 | z_coeffs[i] = traj.z_c 207 | 208 | quad_sim(x_coeffs, y_coeffs, z_coeffs) 209 | 210 | 211 | if __name__ == "__main__": 212 | main() -------------------------------------------------------------------------------- /PathTracking/rear_wheel_feedback/rear_wheel_feedback.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path tracking simulation with rear wheel feedback steering control and PID speed control. 4 | 5 | author: Atsushi Sakai(@Atsushi_twi) 6 | 7 | """ 8 | import matplotlib.pyplot as plt 9 | import math 10 | import numpy as np 11 | import sys 12 | sys.path.append("../../PathPlanning/CubicSpline/") 13 | 14 | try: 15 | import cubic_spline_planner 16 | except: 17 | raise 18 | 19 | 20 | Kp = 1.0 # speed propotional gain 21 | # steering control parameter 22 | KTH = 1.0 23 | KE = 0.5 24 | 25 | dt = 0.1 # [s] 26 | L = 2.9 # [m] 27 | 28 | show_animation = True 29 | # show_animation = False 30 | 31 | 32 | class State: 33 | 34 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 35 | self.x = x 36 | self.y = y 37 | self.yaw = yaw 38 | self.v = v 39 | 40 | 41 | def update(state, a, delta): 42 | 43 | state.x = state.x + state.v * math.cos(state.yaw) * dt 44 | state.y = state.y + state.v * math.sin(state.yaw) * dt 45 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 46 | state.v = state.v + a * dt 47 | 48 | return state 49 | 50 | 51 | def PIDControl(target, current): 52 | a = Kp * (target - current) 53 | 54 | return a 55 | 56 | 57 | def pi_2_pi(angle): 58 | while(angle > math.pi): 59 | angle = angle - 2.0 * math.pi 60 | 61 | while(angle < -math.pi): 62 | angle = angle + 2.0 * math.pi 63 | 64 | return angle 65 | 66 | 67 | def rear_wheel_feedback_control(state, cx, cy, cyaw, ck, preind): 68 | ind, e = calc_nearest_index(state, cx, cy, cyaw) 69 | 70 | k = ck[ind] 71 | v = state.v 72 | th_e = pi_2_pi(state.yaw - cyaw[ind]) 73 | 74 | omega = v * k * math.cos(th_e) / (1.0 - k * e) - \ 75 | KTH * abs(v) * th_e - KE * v * math.sin(th_e) * e / th_e 76 | 77 | if th_e == 0.0 or omega == 0.0: 78 | return 0.0, ind 79 | 80 | delta = math.atan2(L * omega / v, 1.0) 81 | # print(k, v, e, th_e, omega, delta) 82 | 83 | return delta, ind 84 | 85 | 86 | def calc_nearest_index(state, cx, cy, cyaw): 87 | dx = [state.x - icx for icx in cx] 88 | dy = [state.y - icy for icy in cy] 89 | 90 | d = [idx ** 2 + idy ** 2 for (idx, idy) in zip(dx, dy)] 91 | 92 | mind = min(d) 93 | 94 | ind = d.index(mind) 95 | 96 | mind = math.sqrt(mind) 97 | 98 | dxl = cx[ind] - state.x 99 | dyl = cy[ind] - state.y 100 | 101 | angle = pi_2_pi(cyaw[ind] - math.atan2(dyl, dxl)) 102 | if angle < 0: 103 | mind *= -1 104 | 105 | return ind, mind 106 | 107 | 108 | def closed_loop_prediction(cx, cy, cyaw, ck, speed_profile, goal): 109 | 110 | T = 500.0 # max simulation time 111 | goal_dis = 0.3 112 | stop_speed = 0.05 113 | 114 | state = State(x=-0.0, y=-0.0, yaw=0.0, v=0.0) 115 | 116 | time = 0.0 117 | x = [state.x] 118 | y = [state.y] 119 | yaw = [state.yaw] 120 | v = [state.v] 121 | t = [0.0] 122 | goal_flag = False 123 | target_ind = calc_nearest_index(state, cx, cy, cyaw) 124 | 125 | while T >= time: 126 | di, target_ind = rear_wheel_feedback_control( 127 | state, cx, cy, cyaw, ck, target_ind) 128 | ai = PIDControl(speed_profile[target_ind], state.v) 129 | state = update(state, ai, di) 130 | 131 | if abs(state.v) <= stop_speed: 132 | target_ind += 1 133 | 134 | time = time + dt 135 | 136 | # check goal 137 | dx = state.x - goal[0] 138 | dy = state.y - goal[1] 139 | if math.sqrt(dx ** 2 + dy ** 2) <= goal_dis: 140 | print("Goal") 141 | goal_flag = True 142 | break 143 | 144 | x.append(state.x) 145 | y.append(state.y) 146 | yaw.append(state.yaw) 147 | v.append(state.v) 148 | t.append(time) 149 | 150 | if target_ind % 1 == 0 and show_animation: 151 | plt.cla() 152 | plt.plot(cx, cy, "-r", label="course") 153 | plt.plot(x, y, "ob", label="trajectory") 154 | plt.plot(cx[target_ind], cy[target_ind], "xg", label="target") 155 | plt.axis("equal") 156 | plt.grid(True) 157 | plt.title("speed[km/h]:" + str(round(state.v * 3.6, 2)) + 158 | ",target index:" + str(target_ind)) 159 | plt.pause(0.0001) 160 | 161 | return t, x, y, yaw, v, goal_flag 162 | 163 | 164 | def calc_speed_profile(cx, cy, cyaw, target_speed): 165 | 166 | speed_profile = [target_speed] * len(cx) 167 | 168 | direction = 1.0 169 | 170 | # Set stop point 171 | for i in range(len(cx) - 1): 172 | dyaw = cyaw[i + 1] - cyaw[i] 173 | switch = math.pi / 4.0 <= dyaw < math.pi / 2.0 174 | 175 | if switch: 176 | direction *= -1 177 | 178 | if direction != 1.0: 179 | speed_profile[i] = - target_speed 180 | else: 181 | speed_profile[i] = target_speed 182 | 183 | if switch: 184 | speed_profile[i] = 0.0 185 | 186 | speed_profile[-1] = 0.0 187 | 188 | return speed_profile 189 | 190 | 191 | def main(): 192 | print("rear wheel feedback tracking start!!") 193 | ax = [0.0, 6.0, 12.5, 5.0, 7.5, 3.0, -1.0] 194 | ay = [0.0, 0.0, 5.0, 6.5, 3.0, 5.0, -2.0] 195 | goal = [ax[-1], ay[-1]] 196 | 197 | cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course( 198 | ax, ay, ds=0.1) 199 | target_speed = 10.0 / 3.6 200 | 201 | sp = calc_speed_profile(cx, cy, cyaw, target_speed) 202 | 203 | t, x, y, yaw, v, goal_flag = closed_loop_prediction( 204 | cx, cy, cyaw, ck, sp, goal) 205 | 206 | # Test 207 | assert goal_flag, "Cannot goal" 208 | 209 | if show_animation: # pragma: no cover 210 | plt.close() 211 | plt.subplots(1) 212 | plt.plot(ax, ay, "xb", label="input") 213 | plt.plot(cx, cy, "-r", label="spline") 214 | plt.plot(x, y, "-g", label="tracking") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in cyaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, ck, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': 239 | main() 240 | -------------------------------------------------------------------------------- /PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | """ 2 | cubic spline planner 3 | 4 | Author: Atsushi Sakai 5 | 6 | """ 7 | import math 8 | import numpy as np 9 | import bisect 10 | 11 | 12 | class Spline: 13 | """ 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | """ 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | 60 | return result 61 | 62 | def calcd(self, t): 63 | """ 64 | Calc first derivative 65 | 66 | if t is outside of the input x, return None 67 | """ 68 | 69 | if t < self.x[0]: 70 | return None 71 | elif t > self.x[-1]: 72 | return None 73 | 74 | i = self.__search_index(t) 75 | dx = t - self.x[i] 76 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 77 | return result 78 | 79 | def calcdd(self, t): 80 | """ 81 | Calc second derivative 82 | """ 83 | 84 | if t < self.x[0]: 85 | return None 86 | elif t > self.x[-1]: 87 | return None 88 | 89 | i = self.__search_index(t) 90 | dx = t - self.x[i] 91 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 92 | return result 93 | 94 | def __search_index(self, x): 95 | """ 96 | search data segment index 97 | """ 98 | return bisect.bisect(self.x, x) - 1 99 | 100 | def __calc_A(self, h): 101 | """ 102 | calc matrix A for spline coefficient c 103 | """ 104 | A = np.zeros((self.nx, self.nx)) 105 | A[0, 0] = 1.0 106 | for i in range(self.nx - 1): 107 | if i != (self.nx - 2): 108 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 109 | A[i + 1, i] = h[i] 110 | A[i, i + 1] = h[i] 111 | 112 | A[0, 1] = 0.0 113 | A[self.nx - 1, self.nx - 2] = 0.0 114 | A[self.nx - 1, self.nx - 1] = 1.0 115 | # print(A) 116 | return A 117 | 118 | def __calc_B(self, h): 119 | """ 120 | calc matrix B for spline coefficient c 121 | """ 122 | B = np.zeros(self.nx) 123 | for i in range(self.nx - 2): 124 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 125 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 126 | # print(B) 127 | return B 128 | 129 | 130 | class Spline2D: 131 | """ 132 | 2D Cubic Spline class 133 | 134 | """ 135 | 136 | def __init__(self, x, y): 137 | self.s = self.__calc_s(x, y) 138 | self.sx = Spline(self.s, x) 139 | self.sy = Spline(self.s, y) 140 | 141 | def __calc_s(self, x, y): 142 | dx = np.diff(x) 143 | dy = np.diff(y) 144 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 145 | for (idx, idy) in zip(dx, dy)] 146 | s = [0] 147 | s.extend(np.cumsum(self.ds)) 148 | return s 149 | 150 | def calc_position(self, s): 151 | """ 152 | calc position 153 | """ 154 | x = self.sx.calc(s) 155 | y = self.sy.calc(s) 156 | 157 | return x, y 158 | 159 | def calc_curvature(self, s): 160 | """ 161 | calc curvature 162 | """ 163 | dx = self.sx.calcd(s) 164 | ddx = self.sx.calcdd(s) 165 | dy = self.sy.calcd(s) 166 | ddy = self.sy.calcdd(s) 167 | k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) 168 | return k 169 | 170 | def calc_yaw(self, s): 171 | """ 172 | calc yaw 173 | """ 174 | dx = self.sx.calcd(s) 175 | dy = self.sy.calcd(s) 176 | yaw = math.atan2(dy, dx) 177 | return yaw 178 | 179 | 180 | def calc_spline_course(x, y, ds=0.1): 181 | sp = Spline2D(x, y) 182 | s = list(np.arange(0, sp.s[-1], ds)) 183 | 184 | rx, ry, ryaw, rk = [], [], [], [] 185 | for i_s in s: 186 | ix, iy = sp.calc_position(i_s) 187 | rx.append(ix) 188 | ry.append(iy) 189 | ryaw.append(sp.calc_yaw(i_s)) 190 | rk.append(sp.calc_curvature(i_s)) 191 | 192 | return rx, ry, ryaw, rk, s 193 | 194 | 195 | def main(): # pragma: no cover 196 | print("Spline 2D test") 197 | import matplotlib.pyplot as plt 198 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 199 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 200 | 201 | sp = Spline2D(x, y) 202 | s = np.arange(0, sp.s[-1], 0.1) 203 | 204 | rx, ry, ryaw, rk = [], [], [], [] 205 | for i_s in s: 206 | ix, iy = sp.calc_position(i_s) 207 | rx.append(ix) 208 | ry.append(iy) 209 | ryaw.append(sp.calc_yaw(i_s)) 210 | rk.append(sp.calc_curvature(i_s)) 211 | 212 | plt.subplots(1) 213 | plt.plot(x, y, "xb", label="input") 214 | plt.plot(rx, ry, "-r", label="spline") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, rk, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': # pragma: no cover 239 | main() 240 | -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/cubic_spline_planner.py: -------------------------------------------------------------------------------- 1 | """ 2 | Cubic spline planner 3 | 4 | Author: Atsushi Sakai(@Atsushi_twi) 5 | 6 | """ 7 | import math 8 | import numpy as np 9 | import bisect 10 | 11 | 12 | class Spline: 13 | """ 14 | Cubic Spline class 15 | """ 16 | 17 | def __init__(self, x, y): 18 | self.b, self.c, self.d, self.w = [], [], [], [] 19 | 20 | self.x = x 21 | self.y = y 22 | 23 | self.nx = len(x) # dimension of x 24 | h = np.diff(x) 25 | 26 | # calc coefficient c 27 | self.a = [iy for iy in y] 28 | 29 | # calc coefficient c 30 | A = self.__calc_A(h) 31 | B = self.__calc_B(h) 32 | self.c = np.linalg.solve(A, B) 33 | # print(self.c1) 34 | 35 | # calc spline coefficient b and d 36 | for i in range(self.nx - 1): 37 | self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) 38 | tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ 39 | (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 40 | self.b.append(tb) 41 | 42 | def calc(self, t): 43 | """ 44 | Calc position 45 | 46 | if t is outside of the input x, return None 47 | 48 | """ 49 | 50 | if t < self.x[0]: 51 | return None 52 | elif t > self.x[-1]: 53 | return None 54 | 55 | i = self.__search_index(t) 56 | dx = t - self.x[i] 57 | result = self.a[i] + self.b[i] * dx + \ 58 | self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 59 | 60 | return result 61 | 62 | def calcd(self, t): 63 | """ 64 | Calc first derivative 65 | 66 | if t is outside of the input x, return None 67 | """ 68 | 69 | if t < self.x[0]: 70 | return None 71 | elif t > self.x[-1]: 72 | return None 73 | 74 | i = self.__search_index(t) 75 | dx = t - self.x[i] 76 | result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 77 | return result 78 | 79 | def calcdd(self, t): 80 | """ 81 | Calc second derivative 82 | """ 83 | 84 | if t < self.x[0]: 85 | return None 86 | elif t > self.x[-1]: 87 | return None 88 | 89 | i = self.__search_index(t) 90 | dx = t - self.x[i] 91 | result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx 92 | return result 93 | 94 | def __search_index(self, x): 95 | """ 96 | search data segment index 97 | """ 98 | return bisect.bisect(self.x, x) - 1 99 | 100 | def __calc_A(self, h): 101 | """ 102 | calc matrix A for spline coefficient c 103 | """ 104 | A = np.zeros((self.nx, self.nx)) 105 | A[0, 0] = 1.0 106 | for i in range(self.nx - 1): 107 | if i != (self.nx - 2): 108 | A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) 109 | A[i + 1, i] = h[i] 110 | A[i, i + 1] = h[i] 111 | 112 | A[0, 1] = 0.0 113 | A[self.nx - 1, self.nx - 2] = 0.0 114 | A[self.nx - 1, self.nx - 1] = 1.0 115 | # print(A) 116 | return A 117 | 118 | def __calc_B(self, h): 119 | """ 120 | calc matrix B for spline coefficient c 121 | """ 122 | B = np.zeros(self.nx) 123 | for i in range(self.nx - 2): 124 | B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ 125 | h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] 126 | return B 127 | 128 | 129 | class Spline2D: 130 | """ 131 | 2D Cubic Spline class 132 | 133 | """ 134 | 135 | def __init__(self, x, y): 136 | self.s = self.__calc_s(x, y) 137 | self.sx = Spline(self.s, x) 138 | self.sy = Spline(self.s, y) 139 | 140 | def __calc_s(self, x, y): 141 | dx = np.diff(x) 142 | dy = np.diff(y) 143 | self.ds = [math.sqrt(idx ** 2 + idy ** 2) 144 | for (idx, idy) in zip(dx, dy)] 145 | s = [0] 146 | s.extend(np.cumsum(self.ds)) 147 | return s 148 | 149 | def calc_position(self, s): 150 | """ 151 | calc position 152 | """ 153 | x = self.sx.calc(s) 154 | y = self.sy.calc(s) 155 | 156 | return x, y 157 | 158 | def calc_curvature(self, s): 159 | """ 160 | calc curvature 161 | """ 162 | dx = self.sx.calcd(s) 163 | ddx = self.sx.calcdd(s) 164 | dy = self.sy.calcd(s) 165 | ddy = self.sy.calcdd(s) 166 | k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) 167 | return k 168 | 169 | def calc_yaw(self, s): 170 | """ 171 | calc yaw 172 | """ 173 | dx = self.sx.calcd(s) 174 | dy = self.sy.calcd(s) 175 | yaw = math.atan2(dy, dx) 176 | return yaw 177 | 178 | 179 | def calc_spline_course(x, y, ds=0.1): 180 | sp = Spline2D(x, y) 181 | s = list(np.arange(0, sp.s[-1], ds)) 182 | 183 | rx, ry, ryaw, rk = [], [], [], [] 184 | for i_s in s: 185 | ix, iy = sp.calc_position(i_s) 186 | rx.append(ix) 187 | ry.append(iy) 188 | ryaw.append(sp.calc_yaw(i_s)) 189 | rk.append(sp.calc_curvature(i_s)) 190 | 191 | return rx, ry, ryaw, rk, s 192 | 193 | 194 | def main(): 195 | print("Spline 2D test") 196 | import matplotlib.pyplot as plt 197 | x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] 198 | y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] 199 | ds = 0.1 # [m] distance of each intepolated points 200 | 201 | sp = Spline2D(x, y) 202 | s = np.arange(0, sp.s[-1], ds) 203 | 204 | rx, ry, ryaw, rk = [], [], [], [] 205 | for i_s in s: 206 | ix, iy = sp.calc_position(i_s) 207 | rx.append(ix) 208 | ry.append(iy) 209 | ryaw.append(sp.calc_yaw(i_s)) 210 | rk.append(sp.calc_curvature(i_s)) 211 | 212 | plt.subplots(1) 213 | plt.plot(x, y, "xb", label="input") 214 | plt.plot(rx, ry, "-r", label="spline") 215 | plt.grid(True) 216 | plt.axis("equal") 217 | plt.xlabel("x[m]") 218 | plt.ylabel("y[m]") 219 | plt.legend() 220 | 221 | plt.subplots(1) 222 | plt.plot(s, [np.rad2deg(iyaw) for iyaw in ryaw], "-r", label="yaw") 223 | plt.grid(True) 224 | plt.legend() 225 | plt.xlabel("line length[m]") 226 | plt.ylabel("yaw angle[deg]") 227 | 228 | plt.subplots(1) 229 | plt.plot(s, rk, "-r", label="curvature") 230 | plt.grid(True) 231 | plt.legend() 232 | plt.xlabel("line length[m]") 233 | plt.ylabel("curvature [1/m]") 234 | 235 | plt.show() 236 | 237 | 238 | if __name__ == '__main__': 239 | main() 240 | -------------------------------------------------------------------------------- /PathTracking/stanley_controller/stanley_controller.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path tracking simulation with Stanley steering control and PID speed control. 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | Ref: 8 | - [Stanley: The robot that won the DARPA grand challenge](http://isl.ecst.csuchico.edu/DOCS/darpa2005/DARPA%202005%20Stanley.pdf) 9 | - [Autonomous Automobile Path Tracking](https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf) 10 | 11 | """ 12 | import numpy as np 13 | import matplotlib.pyplot as plt 14 | import sys 15 | sys.path.append("../../PathPlanning/CubicSpline/") 16 | 17 | try: 18 | import cubic_spline_planner 19 | except: 20 | raise 21 | 22 | 23 | k = 0.5 # control gain 24 | Kp = 1.0 # speed propotional gain 25 | dt = 0.1 # [s] time difference 26 | L = 2.9 # [m] Wheel base of vehicle 27 | max_steer = np.radians(30.0) # [rad] max steering angle 28 | 29 | show_animation = True 30 | 31 | 32 | class State(object): 33 | """ 34 | Class representing the state of a vehicle. 35 | 36 | :param x: (float) x-coordinate 37 | :param y: (float) y-coordinate 38 | :param yaw: (float) yaw angle 39 | :param v: (float) speed 40 | """ 41 | 42 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 43 | """Instantiate the object.""" 44 | super(State, self).__init__() 45 | self.x = x 46 | self.y = y 47 | self.yaw = yaw 48 | self.v = v 49 | 50 | def update(self, acceleration, delta): 51 | """ 52 | Update the state of the vehicle. 53 | 54 | Stanley Control uses bicycle model. 55 | 56 | :param acceleration: (float) Acceleration 57 | :param delta: (float) Steering 58 | """ 59 | delta = np.clip(delta, -max_steer, max_steer) 60 | 61 | self.x += self.v * np.cos(self.yaw) * dt 62 | self.y += self.v * np.sin(self.yaw) * dt 63 | self.yaw += self.v / L * np.tan(delta) * dt 64 | self.yaw = normalize_angle(self.yaw) 65 | self.v += acceleration * dt 66 | 67 | 68 | def pid_control(target, current): 69 | """ 70 | Proportional control for the speed. 71 | 72 | :param target: (float) 73 | :param current: (float) 74 | :return: (float) 75 | """ 76 | return Kp * (target - current) 77 | 78 | 79 | def stanley_control(state, cx, cy, cyaw, last_target_idx): 80 | """ 81 | Stanley steering control. 82 | 83 | :param state: (State object) 84 | :param cx: ([float]) 85 | :param cy: ([float]) 86 | :param cyaw: ([float]) 87 | :param last_target_idx: (int) 88 | :return: (float, int) 89 | """ 90 | current_target_idx, error_front_axle = calc_target_index(state, cx, cy) 91 | 92 | if last_target_idx >= current_target_idx: 93 | current_target_idx = last_target_idx 94 | 95 | # theta_e corrects the heading error 96 | theta_e = normalize_angle(cyaw[current_target_idx] - state.yaw) 97 | # theta_d corrects the cross track error 98 | theta_d = np.arctan2(k * error_front_axle, state.v) 99 | # Steering control 100 | delta = theta_e + theta_d 101 | 102 | return delta, current_target_idx 103 | 104 | 105 | def normalize_angle(angle): 106 | """ 107 | Normalize an angle to [-pi, pi]. 108 | 109 | :param angle: (float) 110 | :return: (float) Angle in radian in [-pi, pi] 111 | """ 112 | while angle > np.pi: 113 | angle -= 2.0 * np.pi 114 | 115 | while angle < -np.pi: 116 | angle += 2.0 * np.pi 117 | 118 | return angle 119 | 120 | 121 | def calc_target_index(state, cx, cy): 122 | """ 123 | Compute index in the trajectory list of the target. 124 | 125 | :param state: (State object) 126 | :param cx: [float] 127 | :param cy: [float] 128 | :return: (int, float) 129 | """ 130 | # Calc front axle position 131 | fx = state.x + L * np.cos(state.yaw) 132 | fy = state.y + L * np.sin(state.yaw) 133 | 134 | # Search nearest point index 135 | dx = [fx - icx for icx in cx] 136 | dy = [fy - icy for icy in cy] 137 | d = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)] 138 | closest_error = min(d) 139 | target_idx = d.index(closest_error) 140 | 141 | # Project RMS error onto front axle vector 142 | front_axle_vec = [-np.cos(state.yaw + np.pi / 2), 143 | - np.sin(state.yaw + np.pi / 2)] 144 | error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vec) 145 | 146 | return target_idx, error_front_axle 147 | 148 | 149 | def main(): 150 | """Plot an example of Stanley steering control on a cubic spline.""" 151 | # target course 152 | ax = [0.0, 100.0, 100.0, 50.0, 60.0] 153 | ay = [0.0, 0.0, -30.0, -20.0, 0.0] 154 | 155 | cx, cy, cyaw, ck, s = cubic_spline_planner.calc_spline_course( 156 | ax, ay, ds=0.1) 157 | 158 | target_speed = 30.0 / 3.6 # [m/s] 159 | 160 | max_simulation_time = 100.0 161 | 162 | # Initial state 163 | state = State(x=-0.0, y=5.0, yaw=np.radians(20.0), v=0.0) 164 | 165 | last_idx = len(cx) - 1 166 | time = 0.0 167 | x = [state.x] 168 | y = [state.y] 169 | yaw = [state.yaw] 170 | v = [state.v] 171 | t = [0.0] 172 | target_idx, _ = calc_target_index(state, cx, cy) 173 | 174 | while max_simulation_time >= time and last_idx > target_idx: 175 | ai = pid_control(target_speed, state.v) 176 | di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx) 177 | state.update(ai, di) 178 | 179 | time += dt 180 | 181 | x.append(state.x) 182 | y.append(state.y) 183 | yaw.append(state.yaw) 184 | v.append(state.v) 185 | t.append(time) 186 | 187 | if show_animation: # pragma: no cover 188 | plt.cla() 189 | plt.plot(cx, cy, ".r", label="course") 190 | plt.plot(x, y, "-b", label="trajectory") 191 | plt.plot(cx[target_idx], cy[target_idx], "xg", label="target") 192 | plt.axis("equal") 193 | plt.grid(True) 194 | plt.title("Speed[km/h]:" + str(state.v * 3.6)[:4]) 195 | plt.pause(0.001) 196 | 197 | # Test 198 | assert last_idx >= target_idx, "Cannot reach goal" 199 | 200 | if show_animation: # pragma: no cover 201 | plt.plot(cx, cy, ".r", label="course") 202 | plt.plot(x, y, "-b", label="trajectory") 203 | plt.legend() 204 | plt.xlabel("x[m]") 205 | plt.ylabel("y[m]") 206 | plt.axis("equal") 207 | plt.grid(True) 208 | 209 | plt.subplots(1) 210 | plt.plot(t, [iv * 3.6 for iv in v], "-r") 211 | plt.xlabel("Time[s]") 212 | plt.ylabel("Speed[km/h]") 213 | plt.grid(True) 214 | plt.show() 215 | 216 | 217 | if __name__ == '__main__': 218 | main() 219 | --------------------------------------------------------------------------------