├── .github ├── FUNDING.yml ├── ISSUE_TEMPLATE │ └── bug_report.md └── workflows │ ├── Linux_CI.yml │ └── MacOS_CI.yml ├── .gitignore ├── .lgtm.yml ├── .travis.yml ├── AerialNavigation ├── drone_3d_trajectory_following │ ├── Quadrotor.py │ ├── TrajectoryGenerator.py │ └── drone_3d_trajectory_following.py └── rocket_powered_landing │ ├── figure.png │ ├── rocket_powered_landing.ipynb │ └── rocket_powered_landing.py ├── ArmNavigation ├── __init__.py ├── arm_obstacle_navigation │ ├── arm_obstacle_navigation.py │ └── arm_obstacle_navigation_2.py ├── n_joint_arm_3d │ ├── NLinkArm3d.py │ ├── __init__py.py │ ├── random_forward_kinematics.py │ └── random_inverse_kinematics.py ├── n_joint_arm_to_point_control │ ├── NLinkArm.py │ ├── __init__.py │ └── n_joint_arm_to_point_control.py └── two_joint_arm_to_point_control │ ├── Planar_Two_Link_IK.ipynb │ └── two_joint_arm_to_point_control.py ├── Bipedal ├── __init__.py └── bipedal_planner │ └── bipedal_planner.py ├── CONTRIBUTING.md ├── Introduction └── introduction.ipynb ├── InvertedPendulumCart └── inverted_pendulum_mpc_control.py ├── LICENSE ├── Localization ├── Kalmanfilter_basics.ipynb ├── Kalmanfilter_basics_2.ipynb ├── bayes_filter.png ├── ensemble_kalman_filter │ └── ensemble_kalman_filter.py ├── extended_kalman_filter │ ├── ekf.png │ ├── extended_kalman_filter.py │ └── extended_kalman_filter_localization.ipynb ├── histogram_filter │ └── histogram_filter.py ├── particle_filter │ ├── particle_filter.ipynb │ └── particle_filter.py └── unscented_kalman_filter │ └── unscented_kalman_filter.py ├── Mapping ├── circle_fitting │ └── circle_fitting.py ├── gaussian_grid_map │ └── gaussian_grid_map.py ├── grid_map_lib │ └── grid_map_lib.py ├── kmeans_clustering │ └── kmeans_clustering.py ├── lidar_to_grid_map │ ├── animation.gif │ ├── grid_map_example.png │ ├── lidar01.csv │ ├── lidar_to_grid_map.py │ └── lidar_to_grid_map_tutorial.ipynb ├── raycasting_grid_map │ └── raycasting_grid_map.py └── rectangle_fitting │ ├── rectangle_fitting.py │ └── simulator.py ├── PathPlanning ├── AStar │ └── a_star.py ├── BSplinePath │ ├── Figure_1.png │ └── bspline_path.py ├── BatchInformedRRTStar │ └── batch_informed_rrtstar.py ├── BezierPath │ ├── Figure_1.png │ ├── Figure_2.png │ └── bezier_path.py ├── BidirectionalAStar │ └── bidirectional_a_star.py ├── BidirectionalBreadthFirstSearch │ └── bidirectional_breadth_first_search.py ├── BreadthFirstSearch │ └── breadth_first_search.py ├── ClosedLoopRRTStar │ ├── Figure_1.png │ ├── Figure_3.png │ ├── Figure_4.png │ ├── Figure_5.png │ ├── __init__.py │ ├── closed_loop_rrt_star_car.py │ ├── pure_pursuit.py │ └── unicycle_model.py ├── CubicSpline │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ ├── __init__.py │ └── cubic_spline_planner.py ├── DepthFirstSearch │ └── depth_first_search.py ├── Dijkstra │ └── dijkstra.py ├── DubinsPath │ ├── __init__.py │ └── dubins_path_planning.py ├── DynamicWindowApproach │ └── dynamic_window_approach.py ├── Eta3SplinePath │ └── eta3_spline_path.py ├── Eta3SplineTrajectory │ └── eta3_spline_trajectory.py ├── FrenetOptimalTrajectory │ └── frenet_optimal_trajectory.py ├── GreedyBestFirstSearch │ └── greedy_best_first_search.py ├── GridBasedSweepCPP │ └── grid_based_sweep_coverage_path_planner.py ├── HybridAStar │ ├── __init__.py │ ├── a_star_heuristic.py │ ├── car.py │ └── hybrid_a_star.py ├── InformedRRTStar │ └── informed_rrt_star.py ├── LQRPlanner │ └── LQRplanner.py ├── LQRRRTStar │ └── lqr_rrt_star.py ├── ModelPredictiveTrajectoryGenerator │ ├── __init__.py │ ├── lookuptable.png │ ├── lookuptable_generator.py │ ├── model_predictive_trajectory_generator.py │ └── motion_model.py ├── PotentialFieldPlanning │ └── potential_field_planning.py ├── ProbabilisticRoadMap │ └── probabilistic_road_map.py ├── QuinticPolynomialsPlanner │ ├── quintic_polynomials_planner.ipynb │ └── quintic_polynomials_planner.py ├── RRT │ ├── __init__.py │ ├── figure_1.png │ ├── rrt.py │ └── rrt_with_pathsmoothing.py ├── RRTDubins │ ├── __init__.py │ └── rrt_dubins.py ├── RRTStar │ ├── Figure_1.png │ ├── __init__.py │ ├── rrt_star.ipynb │ └── rrt_star.py ├── RRTStarDubins │ └── rrt_star_dubins.py ├── RRTStarReedsShepp │ ├── figure_1.png │ └── rrt_star_reeds_shepp.py ├── ReedsSheppPath │ └── reeds_shepp_path_planning.py ├── StateLatticePlanner │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ ├── Figure_4.png │ ├── Figure_5.png │ ├── Figure_6.png │ ├── __init__.py │ ├── lookuptable.csv │ └── state_lattice_planner.py ├── VisibilityRoadMap │ ├── __init__.py │ ├── geometry.py │ └── visibility_road_map.py ├── VoronoiRoadMap │ ├── dijkstra_search.py │ ├── kdtree.py │ └── voronoi_road_map.py └── __init__.py ├── PathTracking ├── cgmres_nmpc │ ├── Figure_1.png │ ├── Figure_2.png │ ├── Figure_3.png │ ├── Figure_4.png │ ├── cgmres_nmpc.ipynb │ └── cgmres_nmpc.py ├── lqr_speed_steer_control │ └── lqr_speed_steer_control.py ├── lqr_steer_control │ ├── __init__.py │ └── lqr_steer_control.py ├── model_predictive_speed_and_steer_control │ ├── Model_predictive_speed_and_steering_control.ipynb │ └── model_predictive_speed_and_steer_control.py ├── move_to_pose │ └── move_to_pose.py ├── pure_pursuit │ └── pure_pursuit.py ├── rear_wheel_feedback │ ├── Figure_2.png │ └── rear_wheel_feedback.py └── stanley_controller │ └── stanley_controller.py ├── README.md ├── SLAM ├── EKFSLAM │ ├── animation.png │ ├── ekf_slam.ipynb │ └── ekf_slam.py ├── FastSLAM1 │ ├── FastSLAM1.ipynb │ ├── animation.png │ └── fast_slam1.py ├── FastSLAM2 │ └── fast_slam2.py ├── GraphBasedSLAM │ ├── LaTeX │ │ ├── .gitignore │ │ ├── graphSLAM.bib │ │ └── graphSLAM_formulation.tex │ ├── data │ │ ├── README.rst │ │ └── input_INTEL.g2o │ ├── graphSLAM_SE2_example.ipynb │ ├── graphSLAM_doc.ipynb │ ├── graphSLAM_formulation.pdf │ ├── graph_based_slam.py │ ├── graphslam │ │ ├── __init__.py │ │ ├── edge │ │ │ ├── __init__.py │ │ │ └── edge_odometry.py │ │ ├── graph.py │ │ ├── load.py │ │ ├── pose │ │ │ ├── __init__.py │ │ │ └── se2.py │ │ ├── util.py │ │ └── vertex.py │ └── images │ │ └── Graph_SLAM_optimization.gif ├── __init__.py └── iterative_closest_point │ └── iterative_closest_point.py ├── _config.yml ├── appveyor.yml ├── docs ├── Makefile ├── README.md ├── _static │ └── .gitkeep ├── conf.py ├── getting_started.rst ├── index.rst ├── jupyternotebook2rst.py ├── make.bat ├── modules │ ├── FastSLAM1.rst │ ├── FastSLAM1_files │ │ ├── FastSLAM1_12_0.png │ │ ├── FastSLAM1_12_1.png │ │ └── FastSLAM1_1_0.png │ ├── Kalmanfilter_basics.rst │ ├── Kalmanfilter_basics_2.rst │ ├── Kalmanfilter_basics_2_files │ │ └── Kalmanfilter_basics_2_5_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 │ ├── Model_predictive_speed_and_steering_control.rst │ ├── Planar_Two_Link_IK.rst │ ├── 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 │ ├── aerial_navigation.rst │ ├── appendix.rst │ ├── arm_navigation.rst │ ├── cgmres_nmpc.rst │ ├── cgmres_nmpc_files │ │ ├── cgmres_nmpc_1_0.png │ │ ├── cgmres_nmpc_2_0.png │ │ ├── cgmres_nmpc_3_0.png │ │ └── cgmres_nmpc_4_0.png │ ├── extended_kalman_filter_localization.rst │ ├── extended_kalman_filter_localization_files │ │ └── extended_kalman_filter_localization_1_0.png │ ├── introduction.rst │ ├── localization.rst │ ├── mapping.rst │ ├── path_planning.rst │ ├── path_tracking.rst │ ├── rocket_powered_landing.rst │ ├── rrt_star.rst │ ├── rrt_star_files │ │ └── rrt_star_1_0.png │ └── slam.rst └── notebook_template.ipynb ├── environment.yml ├── icon.png ├── mypy.ini ├── requirements.txt ├── rundiffstylecheck.sh ├── runtests.sh ├── tests ├── .gitkeep ├── test_LQR_planner.py ├── test_a_star.py ├── test_batch_informed_rrt_star.py ├── test_bezier_path.py ├── test_cgmres_nmpc.py ├── test_circle_fitting.py ├── test_closed_loop_rrt_star_car.py ├── test_dijkstra.py ├── test_drone_3d_trajectory_following.py ├── test_dubins_path_planning.py ├── test_dynamic_window_approach.py ├── test_ekf_slam.py ├── test_eta3_spline_path.py ├── test_extended_kalman_filter.py ├── test_fast_slam1.py ├── test_fast_slam2.py ├── test_frenet_optimal_trajectory.py ├── test_gaussian_grid_map.py ├── test_graph_based_slam.py ├── test_grid_based_sweep_coverage_path_planner.py ├── test_grid_map_lib.py ├── test_histogram_filter.py ├── test_hybrid_a_star.py ├── test_informed_rrt_star.py ├── test_inverted_pendulum_mpc_control.py ├── test_iterative_closest_point.py ├── test_kmeans_clustering.py ├── test_lqr_rrt_star.py ├── test_lqr_speed_steer_control.py ├── test_lqr_steer_control.py ├── test_model_predictive_speed_and_steer_control.py ├── test_move_to_pose.py ├── test_n_joint_arm_to_point_control.py ├── test_particle_filter.py ├── test_potential_field_planning.py ├── test_probabilistic_road_map.py ├── test_pure_pursuit.py ├── test_quintic_polynomials_planner.py ├── test_raycasting_grid_map.py ├── test_rear_wheel_feedback.py ├── test_reeds_shepp_path_planning.py ├── test_rocket_powered_landing.py ├── test_rrt.py ├── test_rrt_dubins.py ├── test_rrt_star.py ├── test_rrt_star_dubins.py ├── test_rrt_star_reeds_shepp.py ├── test_stanley_controller.py ├── test_state_lattice_planner.py ├── test_two_joint_arm_to_point_control.py ├── test_unscented_kalman_filter.py ├── test_visibility_road_map_planner.py └── test_voronoi_road_map_planner.py └── users_comments.md /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | github: AtsushiSakai 3 | patreon: myenigma 4 | custom: https://www.paypal.me/myenigmapay/ 5 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **Expected behavior** 14 | A clear and concise description of what you expected to happen. 15 | 16 | **Screenshots** 17 | If applicable, add screenshots to help explain your problem. 18 | 19 | **Desktop (please complete the following information):** 20 | - Python version (This repo only supports Python 3.7.x or higher). 21 | - Each library version 22 | -------------------------------------------------------------------------------- /.github/workflows/Linux_CI.yml: -------------------------------------------------------------------------------- 1 | name: Linux_CI 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | 8 | runs-on: ubuntu-latest 9 | strategy: 10 | matrix: 11 | python-version: ['3.8'] 12 | 13 | name: Python ${{ matrix.python-version }} CI 14 | 15 | steps: 16 | - uses: actions/checkout@v2 17 | - run: git fetch --prune --unshallow 18 | 19 | - name: Setup python 20 | uses: actions/setup-python@v2 21 | with: 22 | python-version: ${{ matrix.python-version }} 23 | - name: Install dependencies 24 | run: | 25 | python -m pip install --upgrade pip 26 | pip install -r requirements.txt 27 | - name: install coverage 28 | run: pip install coverage 29 | - name: install mypy 30 | run: pip install mypy 31 | - name: install pycodestyle 32 | run: pip install pycodestyle 33 | - name: mypy check 34 | run: | 35 | find AerialNavigation -name "*.py" | xargs mypy 36 | find ArmNavigation -name "*.py" | xargs mypy 37 | find Bipedal -name "*.py" | xargs mypy 38 | find InvertedPendulumCart -name "*.py" | xargs mypy 39 | find Localization -name "*.py" | xargs mypy 40 | find Mapping -name "*.py" | xargs mypy 41 | find PathPlanning -name "*.py" | xargs mypy 42 | find PathTracking -name "*.py" | xargs mypy 43 | find SLAM -name "*.py" | xargs mypy 44 | - name: do diff style check 45 | run: bash rundiffstylecheck.sh 46 | - name: do all unit tests 47 | run: bash runtests.sh 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /.github/workflows/MacOS_CI.yml: -------------------------------------------------------------------------------- 1 | # This is a basic workflow to help you get started with Actions 2 | 3 | name: MacOS_CI 4 | 5 | # Controls when the action will run. Triggers the workflow on push or pull request 6 | # events but only for the master branch 7 | on: [push, pull_request] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-latest 12 | strategy: 13 | matrix: 14 | python-version: [ '3.8' ] 15 | name: Python ${{ matrix.python-version }} CI 16 | steps: 17 | - uses: actions/checkout@v2 18 | - run: git fetch --prune --unshallow 19 | 20 | - name: Setup python 21 | uses: actions/setup-python@v2 22 | with: 23 | python-version: ${{ matrix.python-version }} 24 | 25 | - name: Install dependencies 26 | run: | 27 | python -m pip install --upgrade pip 28 | pip install -r requirements.txt 29 | - name: install coverage 30 | run: pip install coverage 31 | 32 | - name: install mypy 33 | run: pip install mypy 34 | 35 | - name: install pycodestyle 36 | run: pip install pycodestyle 37 | 38 | - name: mypy check 39 | run: | 40 | find AerialNavigation -name "*.py" | xargs mypy 41 | find ArmNavigation -name "*.py" | xargs mypy 42 | find Bipedal -name "*.py" | xargs mypy 43 | find InvertedPendulumCart -name "*.py" | xargs mypy 44 | find Localization -name "*.py" | xargs mypy 45 | find Mapping -name "*.py" | xargs mypy 46 | find PathPlanning -name "*.py" | xargs mypy 47 | find PathTracking -name "*.py" | xargs mypy 48 | find SLAM -name "*.py" | xargs mypy 49 | 50 | - name: do diff style check 51 | run: bash rundiffstylecheck.sh 52 | 53 | - name: do all unit tests 54 | run: bash runtests.sh 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.csv 2 | *.gif 3 | *.g2o 4 | 5 | # Byte-compiled / optimized / DLL files 6 | __pycache__/ 7 | *.py[cod] 8 | *$py.class 9 | _build/ 10 | .idea/ 11 | 12 | # C extensions 13 | *.so 14 | 15 | # Distribution / packaging 16 | .Python 17 | env/ 18 | build/ 19 | develop-eggs/ 20 | dist/ 21 | downloads/ 22 | eggs/ 23 | .eggs/ 24 | lib/ 25 | lib64/ 26 | parts/ 27 | sdist/ 28 | var/ 29 | *.egg-info/ 30 | .installed.cfg 31 | *.egg 32 | 33 | .DS_Store 34 | 35 | # PyInstaller 36 | # Usually these files are written by a python script from a template 37 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 38 | *.manifest 39 | *.spec 40 | 41 | # Installer logs 42 | pip-log.txt 43 | pip-delete-this-directory.txt 44 | 45 | # Unit test / coverage reports 46 | htmlcov/ 47 | .tox/ 48 | .coverage 49 | .coverage.* 50 | .cache 51 | nosetests.xml 52 | coverage.xml 53 | *,cover 54 | .hypothesis/ 55 | 56 | # Translations 57 | *.mo 58 | *.pot 59 | 60 | # Django stuff: 61 | *.log 62 | 63 | # Sphinx documentation 64 | docs/_build/ 65 | 66 | # PyBuilder 67 | target/ 68 | 69 | #Ipython Notebook 70 | .ipynb_checkpoints 71 | 72 | matplotrecorder/* 73 | .vscode/settings.json 74 | -------------------------------------------------------------------------------- /.lgtm.yml: -------------------------------------------------------------------------------- 1 | extraction: 2 | python: 3 | python_setup: 4 | version: 3 5 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | 3 | matrix: 4 | include: 5 | - os: linux 6 | 7 | python: 8 | - 3.8 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 | -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/Quadrotor.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for plotting a quadrotor 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | from math import cos, sin 8 | import numpy as np 9 | import matplotlib.pyplot as plt 10 | 11 | class Quadrotor(): 12 | def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=0.25, show_animation=True): 13 | self.p1 = np.array([size / 2, 0, 0, 1]).T 14 | self.p2 = np.array([-size / 2, 0, 0, 1]).T 15 | self.p3 = np.array([0, size / 2, 0, 1]).T 16 | self.p4 = np.array([0, -size / 2, 0, 1]).T 17 | 18 | self.x_data = [] 19 | self.y_data = [] 20 | self.z_data = [] 21 | self.show_animation = show_animation 22 | 23 | if self.show_animation: 24 | plt.ion() 25 | fig = plt.figure() 26 | # for stopping simulation with the esc key. 27 | fig.canvas.mpl_connect('key_release_event', 28 | lambda event: [exit(0) if event.key == 'escape' else None]) 29 | 30 | self.ax = fig.add_subplot(111, projection='3d') 31 | 32 | self.update_pose(x, y, z, roll, pitch, yaw) 33 | 34 | def update_pose(self, x, y, z, roll, pitch, yaw): 35 | self.x = x 36 | self.y = y 37 | self.z = z 38 | self.roll = roll 39 | self.pitch = pitch 40 | self.yaw = yaw 41 | self.x_data.append(x) 42 | self.y_data.append(y) 43 | self.z_data.append(z) 44 | 45 | if self.show_animation: 46 | self.plot() 47 | 48 | def transformation_matrix(self): 49 | x = self.x 50 | y = self.y 51 | z = self.z 52 | roll = self.roll 53 | pitch = self.pitch 54 | yaw = self.yaw 55 | return np.array( 56 | [[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x], 57 | [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) 58 | * sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y], 59 | [-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z] 60 | ]) 61 | 62 | def plot(self): # pragma: no cover 63 | T = self.transformation_matrix() 64 | 65 | p1_t = np.matmul(T, self.p1) 66 | p2_t = np.matmul(T, self.p2) 67 | p3_t = np.matmul(T, self.p3) 68 | p4_t = np.matmul(T, self.p4) 69 | 70 | plt.cla() 71 | 72 | self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]], 73 | [p1_t[1], p2_t[1], p3_t[1], p4_t[1]], 74 | [p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.') 75 | 76 | self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]], 77 | [p1_t[2], p2_t[2]], 'r-') 78 | self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]], 79 | [p3_t[2], p4_t[2]], 'r-') 80 | 81 | self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:') 82 | 83 | plt.xlim(-5, 5) 84 | plt.ylim(-5, 5) 85 | self.ax.set_zlim(0, 10) 86 | 87 | plt.pause(0.001) 88 | -------------------------------------------------------------------------------- /AerialNavigation/drone_3d_trajectory_following/TrajectoryGenerator.py: -------------------------------------------------------------------------------- 1 | """ 2 | Generates a quintic polynomial trajectory. 3 | 4 | Author: Daniel Ingram (daniel-s-ingram) 5 | """ 6 | 7 | import numpy as np 8 | 9 | class TrajectoryGenerator(): 10 | def __init__(self, start_pos, des_pos, T, start_vel=[0,0,0], des_vel=[0,0,0], start_acc=[0,0,0], des_acc=[0,0,0]): 11 | self.start_x = start_pos[0] 12 | self.start_y = start_pos[1] 13 | self.start_z = start_pos[2] 14 | 15 | self.des_x = des_pos[0] 16 | self.des_y = des_pos[1] 17 | self.des_z = des_pos[2] 18 | 19 | self.start_x_vel = start_vel[0] 20 | self.start_y_vel = start_vel[1] 21 | self.start_z_vel = start_vel[2] 22 | 23 | self.des_x_vel = des_vel[0] 24 | self.des_y_vel = des_vel[1] 25 | self.des_z_vel = des_vel[2] 26 | 27 | self.start_x_acc = start_acc[0] 28 | self.start_y_acc = start_acc[1] 29 | self.start_z_acc = start_acc[2] 30 | 31 | self.des_x_acc = des_acc[0] 32 | self.des_y_acc = des_acc[1] 33 | self.des_z_acc = des_acc[2] 34 | 35 | self.T = T 36 | 37 | def solve(self): 38 | A = np.array( 39 | [[0, 0, 0, 0, 0, 1], 40 | [self.T**5, self.T**4, self.T**3, self.T**2, self.T, 1], 41 | [0, 0, 0, 0, 1, 0], 42 | [5*self.T**4, 4*self.T**3, 3*self.T**2, 2*self.T, 1, 0], 43 | [0, 0, 0, 2, 0, 0], 44 | [20*self.T**3, 12*self.T**2, 6*self.T, 2, 0, 0] 45 | ]) 46 | 47 | b_x = np.array( 48 | [[self.start_x], 49 | [self.des_x], 50 | [self.start_x_vel], 51 | [self.des_x_vel], 52 | [self.start_x_acc], 53 | [self.des_x_acc] 54 | ]) 55 | 56 | b_y = np.array( 57 | [[self.start_y], 58 | [self.des_y], 59 | [self.start_y_vel], 60 | [self.des_y_vel], 61 | [self.start_y_acc], 62 | [self.des_y_acc] 63 | ]) 64 | 65 | b_z = np.array( 66 | [[self.start_z], 67 | [self.des_z], 68 | [self.start_z_vel], 69 | [self.des_z_vel], 70 | [self.start_z_acc], 71 | [self.des_z_acc] 72 | ]) 73 | 74 | self.x_c = np.linalg.solve(A, b_x) 75 | self.y_c = np.linalg.solve(A, b_y) 76 | self.z_c = np.linalg.solve(A, b_z) -------------------------------------------------------------------------------- /AerialNavigation/rocket_powered_landing/figure.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/AerialNavigation/rocket_powered_landing/figure.png -------------------------------------------------------------------------------- /ArmNavigation/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/ArmNavigation/__init__.py -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/__init__py.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/ArmNavigation/n_joint_arm_3d/__init__py.py -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/random_forward_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Forward Kinematics for an n-link arm in 3D 3 | Author: Takayuki Murooka (takayuki5168) 4 | """ 5 | import math 6 | from NLinkArm3d import NLinkArm 7 | import random 8 | 9 | 10 | def random_val(min_val, max_val): 11 | return min_val + random.random() * (max_val - min_val) 12 | 13 | 14 | if __name__ == "__main__": 15 | print("Start solving Forward Kinematics 10 times") 16 | 17 | # init NLinkArm with Denavit-Hartenberg parameters of PR2 18 | n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], 19 | [math.pi / 2, math.pi / 2, 0., 0.], 20 | [0., -math.pi / 2, 0., .4], 21 | [0., math.pi / 2, 0., 0.], 22 | [0., -math.pi / 2, 0., .321], 23 | [0., math.pi / 2, 0., 0.], 24 | [0., 0., 0., 0.]]) 25 | 26 | # execute FK 10 times 27 | for i in range(10): 28 | n_link_arm.set_joint_angles( 29 | [random_val(-1, 1) for j in range(len(n_link_arm.link_list))]) 30 | 31 | ee_pose = n_link_arm.forward_kinematics(plot=True) 32 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_3d/random_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverse Kinematics for an n-link arm in 3D 3 | Author: Takayuki Murooka (takayuki5168) 4 | """ 5 | import math 6 | from NLinkArm3d import NLinkArm 7 | import random 8 | 9 | 10 | def random_val(min_val, max_val): 11 | return min_val + random.random() * (max_val - min_val) 12 | 13 | 14 | if __name__ == "__main__": 15 | print("Start solving Inverse Kinematics 10 times") 16 | 17 | # init NLinkArm with Denavit-Hartenberg parameters of PR2 18 | n_link_arm = NLinkArm([[0., -math.pi / 2, .1, 0.], 19 | [math.pi / 2, math.pi / 2, 0., 0.], 20 | [0., -math.pi / 2, 0., .4], 21 | [0., math.pi / 2, 0., 0.], 22 | [0., -math.pi / 2, 0., .321], 23 | [0., math.pi / 2, 0., 0.], 24 | [0., 0., 0., 0.]]) 25 | 26 | # execute IK 10 times 27 | for i in range(10): 28 | n_link_arm.inverse_kinematics([random_val(-0.5, 0.5), 29 | random_val(-0.5, 0.5), 30 | random_val(-0.5, 0.5), 31 | random_val(-0.5, 0.5), 32 | random_val(-0.5, 0.5), 33 | random_val(-0.5, 0.5)], plot=True) 34 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/NLinkArm.py: -------------------------------------------------------------------------------- 1 | """ 2 | Class for controlling and plotting an arm with an arbitrary number of links. 3 | 4 | Author: Daniel Ingram 5 | """ 6 | 7 | import numpy as np 8 | import matplotlib.pyplot as plt 9 | 10 | 11 | class NLinkArm(object): 12 | def __init__(self, link_lengths, joint_angles, goal, show_animation): 13 | self.show_animation = show_animation 14 | self.n_links = len(link_lengths) 15 | if self.n_links != len(joint_angles): 16 | raise ValueError() 17 | 18 | self.link_lengths = np.array(link_lengths) 19 | self.joint_angles = np.array(joint_angles) 20 | self.points = [[0, 0] for _ in range(self.n_links + 1)] 21 | 22 | self.lim = sum(link_lengths) 23 | self.goal = np.array(goal).T 24 | 25 | if show_animation: # pragma: no cover 26 | self.fig = plt.figure() 27 | self.fig.canvas.mpl_connect('button_press_event', self.click) 28 | 29 | plt.ion() 30 | plt.show() 31 | 32 | self.update_points() 33 | 34 | def update_joints(self, joint_angles): 35 | self.joint_angles = joint_angles 36 | 37 | self.update_points() 38 | 39 | def update_points(self): 40 | for i in range(1, self.n_links + 1): 41 | self.points[i][0] = self.points[i - 1][0] + \ 42 | self.link_lengths[i - 1] * \ 43 | np.cos(np.sum(self.joint_angles[:i])) 44 | self.points[i][1] = self.points[i - 1][1] + \ 45 | self.link_lengths[i - 1] * \ 46 | np.sin(np.sum(self.joint_angles[:i])) 47 | 48 | self.end_effector = np.array(self.points[self.n_links]).T 49 | if self.show_animation: # pragma: no cover 50 | self.plot() 51 | 52 | def plot(self): # pragma: no cover 53 | plt.cla() 54 | # for stopping simulation with the esc key. 55 | plt.gcf().canvas.mpl_connect('key_release_event', 56 | lambda event: [exit(0) if event.key == 'escape' else None]) 57 | 58 | for i in range(self.n_links + 1): 59 | if i is not self.n_links: 60 | plt.plot([self.points[i][0], self.points[i + 1][0]], 61 | [self.points[i][1], self.points[i + 1][1]], 'r-') 62 | plt.plot(self.points[i][0], self.points[i][1], 'ko') 63 | 64 | plt.plot(self.goal[0], self.goal[1], 'gx') 65 | 66 | plt.plot([self.end_effector[0], self.goal[0]], [ 67 | self.end_effector[1], self.goal[1]], 'g--') 68 | 69 | plt.xlim([-self.lim, self.lim]) 70 | plt.ylim([-self.lim, self.lim]) 71 | plt.draw() 72 | plt.pause(0.0001) 73 | 74 | def click(self, event): 75 | self.goal = np.array([event.xdata, event.ydata]).T 76 | self.plot() 77 | -------------------------------------------------------------------------------- /ArmNavigation/n_joint_arm_to_point_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/ArmNavigation/n_joint_arm_to_point_control/__init__.py -------------------------------------------------------------------------------- /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.hypot(wrist[0] - x, wrist[1] - y) 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 | # for stopping simulation with the esc key. 118 | fig.canvas.mpl_connect('key_release_event', 119 | lambda event: [exit(0) if event.key == 'escape' else None]) 120 | two_joint_arm() 121 | 122 | 123 | if __name__ == "__main__": 124 | # animation() 125 | main() 126 | -------------------------------------------------------------------------------- /Bipedal/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/Bipedal/__init__.py -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /InvertedPendulumCart/inverted_pendulum_mpc_control.py: -------------------------------------------------------------------------------- 1 | """ 2 | Inverted Pendulum MPC control 3 | author: Atsushi Sakai 4 | """ 5 | 6 | import matplotlib.pyplot as plt 7 | import numpy as np 8 | import math 9 | import time 10 | import cvxpy 11 | 12 | # Model parameters 13 | 14 | l_bar = 2.0 # length of bar 15 | M = 1.0 # [kg] 16 | m = 0.3 # [kg] 17 | g = 9.8 # [m/s^2] 18 | 19 | Q = np.diag([0.0, 1.0, 1.0, 0.0]) 20 | R = np.diag([0.01]) 21 | nx = 4 # number of state 22 | nu = 1 # number of input 23 | T = 30 # Horizon length 24 | delta_t = 0.1 # time tick 25 | 26 | animation = True 27 | 28 | 29 | def main(): 30 | x0 = np.array([ 31 | [0.0], 32 | [0.0], 33 | [0.3], 34 | [0.0] 35 | ]) 36 | 37 | x = np.copy(x0) 38 | 39 | for i in range(50): 40 | 41 | # calc control input 42 | optimized_x, optimized_delta_x, optimized_theta, optimized_delta_theta, optimized_input = mpc_control(x) 43 | 44 | # get input 45 | u = optimized_input[0] 46 | 47 | # simulate inverted pendulum cart 48 | x = simulation(x, u) 49 | 50 | if animation: 51 | plt.clf() 52 | px = float(x[0]) 53 | theta = float(x[2]) 54 | plot_cart(px, theta) 55 | plt.xlim([-5.0, 2.0]) 56 | plt.pause(0.001) 57 | 58 | 59 | def simulation(x, u): 60 | A, B = get_model_matrix() 61 | 62 | x = np.dot(A, x) + np.dot(B, u) 63 | 64 | return x 65 | 66 | 67 | def mpc_control(x0): 68 | x = cvxpy.Variable((nx, T + 1)) 69 | u = cvxpy.Variable((nu, T)) 70 | 71 | A, B = get_model_matrix() 72 | 73 | cost = 0.0 74 | constr = [] 75 | for t in range(T): 76 | cost += cvxpy.quad_form(x[:, t + 1], Q) 77 | cost += cvxpy.quad_form(u[:, t], R) 78 | constr += [x[:, t + 1] == A * x[:, t] + B * u[:, t]] 79 | 80 | constr += [x[:, 0] == x0[:, 0]] 81 | prob = cvxpy.Problem(cvxpy.Minimize(cost), constr) 82 | 83 | start = time.time() 84 | prob.solve(verbose=False) 85 | elapsed_time = time.time() - start 86 | print("calc time:{0} [sec]".format(elapsed_time)) 87 | 88 | if prob.status == cvxpy.OPTIMAL: 89 | ox = get_nparray_from_matrix(x.value[0, :]) 90 | dx = get_nparray_from_matrix(x.value[1, :]) 91 | theta = get_nparray_from_matrix(x.value[2, :]) 92 | dtheta = get_nparray_from_matrix(x.value[3, :]) 93 | 94 | ou = get_nparray_from_matrix(u.value[0, :]) 95 | 96 | return ox, dx, theta, dtheta, ou 97 | 98 | 99 | def get_nparray_from_matrix(x): 100 | """ 101 | get build-in list from matrix 102 | """ 103 | return np.array(x).flatten() 104 | 105 | 106 | def get_model_matrix(): 107 | A = np.array([ 108 | [0.0, 1.0, 0.0, 0.0], 109 | [0.0, 0.0, m * g / M, 0.0], 110 | [0.0, 0.0, 0.0, 1.0], 111 | [0.0, 0.0, g * (M + m) / (l_bar * M), 0.0] 112 | ]) 113 | A = np.eye(nx) + delta_t * A 114 | 115 | B = np.array([ 116 | [0.0], 117 | [1.0 / M], 118 | [0.0], 119 | [1.0 / (l_bar * M)] 120 | ]) 121 | B = delta_t * B 122 | 123 | return A, B 124 | 125 | 126 | def flatten(a): 127 | return np.array(a).flatten() 128 | 129 | 130 | def plot_cart(xt, theta): 131 | cart_w = 1.0 132 | cart_h = 0.5 133 | radius = 0.1 134 | 135 | cx = np.array([-cart_w / 2.0, cart_w / 2.0, cart_w / 136 | 2.0, -cart_w / 2.0, -cart_w / 2.0]) 137 | cy = np.array([0.0, 0.0, cart_h, cart_h, 0.0]) 138 | cy += radius * 2.0 139 | 140 | cx = cx + xt 141 | 142 | bx = np.array([0.0, l_bar * math.sin(-theta)]) 143 | bx += xt 144 | by = np.array([cart_h, l_bar * math.cos(-theta) + cart_h]) 145 | by += radius * 2.0 146 | 147 | angles = np.arange(0.0, math.pi * 2.0, math.radians(3.0)) 148 | ox = np.array([radius * math.cos(a) for a in angles]) 149 | oy = np.array([radius * math.sin(a) for a in angles]) 150 | 151 | rwx = np.copy(ox) + cart_w / 4.0 + xt 152 | rwy = np.copy(oy) + radius 153 | lwx = np.copy(ox) - cart_w / 4.0 + xt 154 | lwy = np.copy(oy) + radius 155 | 156 | wx = np.copy(ox) + bx[-1] 157 | wy = np.copy(oy) + by[-1] 158 | 159 | plt.plot(flatten(cx), flatten(cy), "-b") 160 | plt.plot(flatten(bx), flatten(by), "-k") 161 | plt.plot(flatten(rwx), flatten(rwy), "-k") 162 | plt.plot(flatten(lwx), flatten(lwy), "-k") 163 | plt.plot(flatten(wx), flatten(wy), "-k") 164 | plt.title("x:" + str(round(xt, 2)) + ",theta:" + 165 | str(round(math.degrees(theta), 2))) 166 | 167 | plt.axis("equal") 168 | 169 | 170 | if __name__ == '__main__': 171 | main() 172 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /Localization/bayes_filter.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/Localization/bayes_filter.png -------------------------------------------------------------------------------- /Localization/extended_kalman_filter/ekf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/Localization/extended_kalman_filter/ekf.png -------------------------------------------------------------------------------- /Localization/particle_filter/particle_filter.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": { 6 | "collapsed": true, 7 | "pycharm": { 8 | "name": "#%% md\n" 9 | } 10 | }, 11 | "source": [ 12 | "# Particle Filter Localization\n", 13 | "\n" 14 | ] 15 | }, 16 | { 17 | "cell_type": "markdown", 18 | "source": [ 19 | "## How to calculate covariance matrix from particles\n", 20 | "\n", 21 | "The covariance matrix $\\Xi$ from particle information is calculated by the following equation: \n", 22 | "\n", 23 | "$\\Xi_{j,k}=\\frac{1}{1-\\sum^N_{i=1}(w^i)^2}\\sum^N_{i=1}w^i(x^i_j-\\mu_j)(x^i_k-\\mu_k)$\n", 24 | "\n", 25 | "- $\\Xi_{j,k}$ is covariance matrix element at row $i$ and column $k$.\n", 26 | "\n", 27 | "- $w^i$ is the weight of the $i$ th particle. \n", 28 | "\n", 29 | "- $x^i_j$ is the $j$ th state of the $i$ th particle. \n", 30 | "\n", 31 | "- $\\mu_j$ is the $j$ th mean state of particles.\n", 32 | "\n", 33 | "Ref:\n", 34 | "\n", 35 | "- [Improving the particle filter in high dimensions using conjugate artificial process noise](https://arxiv.org/pdf/1801.07000.pdf)\n" 36 | ], 37 | "metadata": { 38 | "collapsed": false 39 | } 40 | } 41 | ], 42 | "metadata": { 43 | "kernelspec": { 44 | "display_name": "Python 3", 45 | "language": "python", 46 | "name": "python3" 47 | }, 48 | "language_info": { 49 | "codemirror_mode": { 50 | "name": "ipython", 51 | "version": 2 52 | }, 53 | "file_extension": ".py", 54 | "mimetype": "text/x-python", 55 | "name": "python", 56 | "nbconvert_exporter": "python", 57 | "pygments_lexer": "ipython2", 58 | "version": "2.7.6" 59 | }, 60 | "pycharm": { 61 | "stem_cell": { 62 | "cell_type": "raw", 63 | "source": [], 64 | "metadata": { 65 | "collapsed": false 66 | } 67 | } 68 | } 69 | }, 70 | "nbformat": 4, 71 | "nbformat_minor": 0 72 | } -------------------------------------------------------------------------------- /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 | # for stopping simulation with the esc key. 128 | plt.gcf().canvas.mpl_connect('key_release_event', 129 | lambda event: [exit(0) if event.key == 'escape' else None]) 130 | plt.axis("equal") 131 | plt.plot(0.0, 0.0, "*r") 132 | plot_circle(cx, cy, cr) 133 | plt.plot(x, y, "xr") 134 | plot_circle(ex, ey, er, "-r") 135 | plt.pause(dt) 136 | 137 | print("Done") 138 | 139 | 140 | if __name__ == '__main__': 141 | main() 142 | -------------------------------------------------------------------------------- /Mapping/gaussian_grid_map/gaussian_grid_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | 2D gaussian grid map sample 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | from scipy.stats import norm 13 | 14 | EXTEND_AREA = 10.0 # [m] grid map extention length 15 | 16 | show_animation = True 17 | 18 | 19 | def generate_gaussian_grid_map(ox, oy, xyreso, std): 20 | 21 | minx, miny, maxx, maxy, xw, yw = calc_grid_map_config(ox, oy, xyreso) 22 | 23 | gmap = [[0.0 for i in range(yw)] for i in range(xw)] 24 | 25 | for ix in range(xw): 26 | for iy in range(yw): 27 | 28 | x = ix * xyreso + minx 29 | y = iy * xyreso + miny 30 | 31 | # Search minimum distance 32 | mindis = float("inf") 33 | for (iox, ioy) in zip(ox, oy): 34 | d = math.hypot(iox - x, ioy - y) 35 | if mindis >= d: 36 | mindis = d 37 | 38 | pdf = (1.0 - norm.cdf(mindis, 0.0, std)) 39 | gmap[ix][iy] = pdf 40 | 41 | return gmap, minx, maxx, miny, maxy 42 | 43 | 44 | def calc_grid_map_config(ox, oy, xyreso): 45 | minx = round(min(ox) - EXTEND_AREA / 2.0) 46 | miny = round(min(oy) - EXTEND_AREA / 2.0) 47 | maxx = round(max(ox) + EXTEND_AREA / 2.0) 48 | maxy = round(max(oy) + EXTEND_AREA / 2.0) 49 | xw = int(round((maxx - minx) / xyreso)) 50 | yw = int(round((maxy - miny) / xyreso)) 51 | 52 | return minx, miny, maxx, maxy, xw, yw 53 | 54 | 55 | def draw_heatmap(data, minx, maxx, miny, maxy, xyreso): 56 | x, y = np.mgrid[slice(minx - xyreso / 2.0, maxx + xyreso / 2.0, xyreso), 57 | slice(miny - xyreso / 2.0, maxy + xyreso / 2.0, xyreso)] 58 | plt.pcolor(x, y, data, vmax=1.0, cmap=plt.cm.Blues) 59 | plt.axis("equal") 60 | 61 | 62 | def main(): 63 | print(__file__ + " start!!") 64 | 65 | xyreso = 0.5 # xy grid resolution 66 | STD = 5.0 # standard diviation for gaussian distribution 67 | 68 | for i in range(5): 69 | ox = (np.random.rand(4) - 0.5) * 10.0 70 | oy = (np.random.rand(4) - 0.5) * 10.0 71 | gmap, minx, maxx, miny, maxy = generate_gaussian_grid_map( 72 | ox, oy, xyreso, STD) 73 | 74 | if show_animation: # pragma: no cover 75 | plt.cla() 76 | # for stopping simulation with the esc key. 77 | plt.gcf().canvas.mpl_connect('key_release_event', 78 | lambda event: [exit(0) if event.key == 'escape' else None]) 79 | draw_heatmap(gmap, minx, maxx, miny, maxy, xyreso) 80 | plt.plot(ox, oy, "xr") 81 | plt.plot(0.0, 0.0, "ob") 82 | plt.pause(1.0) 83 | 84 | 85 | if __name__ == '__main__': 86 | main() 87 | -------------------------------------------------------------------------------- /Mapping/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 math 10 | import matplotlib.pyplot as plt 11 | import random 12 | 13 | # k means parameters 14 | MAX_LOOP = 10 15 | DCOST_TH = 0.1 16 | show_animation = True 17 | 18 | 19 | def kmeans_clustering(rx, ry, nc): 20 | clusters = Clusters(rx, ry, nc) 21 | clusters.calc_centroid() 22 | 23 | pre_cost = float("inf") 24 | for loop in range(MAX_LOOP): 25 | print("loop:", loop) 26 | cost = clusters.update_clusters() 27 | clusters.calc_centroid() 28 | 29 | d_cost = abs(cost - pre_cost) 30 | if d_cost < DCOST_TH: 31 | break 32 | pre_cost = cost 33 | 34 | return clusters 35 | 36 | 37 | class Clusters: 38 | 39 | def __init__(self, x, y, n_label): 40 | self.x = x 41 | self.y = y 42 | self.n_data = len(self.x) 43 | self.n_label = n_label 44 | self.labels = [random.randint(0, n_label - 1) 45 | for _ in range(self.n_data)] 46 | self.center_x = [0.0 for _ in range(n_label)] 47 | self.center_y = [0.0 for _ in range(n_label)] 48 | 49 | def plot_cluster(self): 50 | for label in set(self.labels): 51 | x, y = self._get_labeled_x_y(label) 52 | plt.plot(x, y, ".") 53 | 54 | def calc_centroid(self): 55 | for label in set(self.labels): 56 | x, y = self._get_labeled_x_y(label) 57 | n_data = len(x) 58 | self.center_x[label] = sum(x) / n_data 59 | self.center_y[label] = sum(y) / n_data 60 | 61 | def update_clusters(self): 62 | cost = 0.0 63 | 64 | for ip in range(self.n_data): 65 | px = self.x[ip] 66 | py = self.y[ip] 67 | 68 | dx = [icx - px for icx in self.center_x] 69 | dy = [icy - py for icy in self.center_y] 70 | 71 | dist_list = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)] 72 | min_dist = min(dist_list) 73 | min_id = dist_list.index(min_dist) 74 | self.labels[ip] = min_id 75 | cost += min_dist 76 | 77 | return cost 78 | 79 | def _get_labeled_x_y(self, target_label): 80 | x = [self.x[i] for i, label in enumerate(self.labels) if label == target_label] 81 | y = [self.y[i] for i, label in enumerate(self.labels) if label == target_label] 82 | return x, y 83 | 84 | 85 | def calc_raw_data(cx, cy, n_points, rand_d): 86 | rx, ry = [], [] 87 | 88 | for (icx, icy) in zip(cx, cy): 89 | for _ in range(n_points): 90 | rx.append(icx + rand_d * (random.random() - 0.5)) 91 | ry.append(icy + rand_d * (random.random() - 0.5)) 92 | 93 | return rx, ry 94 | 95 | 96 | def update_positions(cx, cy): 97 | # object moving parameters 98 | DX1 = 0.4 99 | DY1 = 0.5 100 | DX2 = -0.3 101 | DY2 = -0.5 102 | 103 | cx[0] += DX1 104 | cy[0] += DY1 105 | cx[1] += DX2 106 | cy[1] += DY2 107 | 108 | return cx, cy 109 | 110 | 111 | def main(): 112 | print(__file__ + " start!!") 113 | 114 | cx = [0.0, 8.0] 115 | cy = [0.0, 8.0] 116 | n_points = 10 117 | rand_d = 3.0 118 | n_cluster = 2 119 | sim_time = 15.0 120 | dt = 1.0 121 | time = 0.0 122 | 123 | while time <= sim_time: 124 | print("Time:", time) 125 | time += dt 126 | 127 | # objects moving simulation 128 | cx, cy = update_positions(cx, cy) 129 | raw_x, raw_y = calc_raw_data(cx, cy, n_points, rand_d) 130 | 131 | clusters = kmeans_clustering(raw_x, raw_y, n_cluster) 132 | 133 | # for animation 134 | if show_animation: # pragma: no cover 135 | plt.cla() 136 | # for stopping simulation with the esc key. 137 | plt.gcf().canvas.mpl_connect('key_release_event', 138 | lambda event: [exit(0) if event.key == 'escape' else None]) 139 | clusters.plot_cluster() 140 | plt.plot(cx, cy, "or") 141 | plt.xlim(-2.0, 10.0) 142 | plt.ylim(-2.0, 10.0) 143 | plt.pause(dt) 144 | 145 | print("Done") 146 | 147 | 148 | if __name__ == '__main__': 149 | main() 150 | -------------------------------------------------------------------------------- /Mapping/lidar_to_grid_map/animation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/Mapping/lidar_to_grid_map/animation.gif -------------------------------------------------------------------------------- /Mapping/lidar_to_grid_map/grid_map_example.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/Mapping/lidar_to_grid_map/grid_map_example.png -------------------------------------------------------------------------------- /Mapping/lidar_to_grid_map/lidar01.csv: -------------------------------------------------------------------------------- 1 | 0.008450416037156572,0.5335 2 | 0.046902201120156306,0.5345 3 | 0.08508127850753233,0.537 4 | 0.1979822644959155,0.2605 5 | 0.21189035697274505,0.2625 6 | 0.2587960806200922,0.26475 7 | 0.3043382657893199,0.2675 8 | 0.34660795861105775,0.27075 9 | 0.43632879047139106,0.59 10 | 0.4739624524675188,0.60025 11 | 0.5137777760286397,0.611 12 | 0.5492297764597742,0.6265 13 | 0.5895905154121426,0.64 14 | 0.6253152235389017,0.6565 15 | 0.6645851317087743,0.676 16 | 0.6997644244442851,0.6975 17 | 0.7785769484796541,0.3345 18 | 0.7772134100015329,0.74575 19 | 0.8652979956881222,0.3315 20 | 0.8996591653367609,0.31775 21 | 0.9397471965935056,0.31275 22 | 0.9847439663714841,0.31125 23 | 1.0283771976713423,0.31325 24 | 1.0641019057981014,0.31975 25 | 1.1009174447073562,0.3335 26 | 1.2012738766970301,0.92275 27 | 1.2397256617800307,0.95325 28 | 1.2779047391674068,0.9865 29 | 1.316629231946031,1.01775 30 | 1.3561718478115274,1.011 31 | 1.3948963405901518,1.0055 32 | 1.4330754179775278,1.00225 33 | 1.4731634492342724,0.99975 34 | 1.5113425266216485,0.9975 35 | 1.5517032655740168,1.001 36 | 1.5896096352657691,1.00275 37 | 1.6288795434356418,1.008 38 | 1.6684221593011381,1.0135 39 | 1.7066012366885142,1.022 40 | 1.7453257294671385,1.02875 41 | 1.7862318838107551,0.9935 42 | 1.8257744996762515,1.0025 43 | 1.8661352386286207,0.96075 44 | 1.9045870237116205,0.92125 45 | 1.9465840088377355,0.8855 46 | 1.986944747790103,0.85725 47 | 2.025669240568728,0.832 48 | 2.065757271825472,0.80675 49 | 2.1066634261690886,0.78875 50 | 2.1464787497302105,0.7705 51 | 2.1865667809869542,0.75625 52 | 2.2261093968524506,0.74475 53 | 2.2683790896741876,0.68275 54 | 2.3090125363221823,0.6375 55 | 2.3510095214482956,0.59925 56 | 2.3916429680962885,0.5665 57 | 2.4330945378311526,0.538 58 | 2.4783640153047557,0.50825 59 | 2.5203610004308707,0.4875 60 | 2.562903400948233,0.46825 61 | 2.599173524466238,0.45 62 | 2.642806755766097,0.4355 63 | 2.685076448587836,0.42275 64 | 2.722437402888339,0.4125 65 | 2.766888757275069,0.40125 66 | 2.8007045115324587,0.39525 67 | 2.841883373571701,0.385 68 | 2.8819714048284446,0.3805 69 | 2.922332143780814,0.38575 70 | 2.9637837135156797,0.38425 71 | 3.0005992524249336,0.36575 72 | 3.0401418682904318,0.3765 73 | 3.079957191851552,0.3915 74 | 3.115409192282687,0.408 75 | 3.154679100452558,0.4265 76 | 3.184949654666836,0.447 77 | 3.2242195628367085,0.4715 78 | 3.2574899017028507,0.49875 79 | 3.2959416867858504,0.52875 80 | 3.3292120256519926,0.564 81 | 3.3665729799524957,0.6055 82 | 3.4031158111661277,0.6515 83 | 3.438022396206014,0.70675 84 | 3.4756560582021407,0.771 85 | 3.513562427893893,0.77075 86 | 3.5522869206725183,0.7785 87 | 3.621827383056667,0.79575 88 | 3.65918833735717,0.8045 89 | 3.697367414744546,0.81725 90 | 3.7377281536969154,0.83325 91 | 3.775634523388667,0.8415 92 | 3.8135408930804187,0.85575 93 | 3.8522653858590425,0.87325 94 | 3.8898990478551703,0.88725 95 | 3.9299870791119154,0.906 96 | 3.9665299103255465,0.9265 97 | 4.006072526191043,0.94575 98 | 4.043978895882795,0.97175 99 | 4.081885265574547,1.02075 100 | 4.1206097583531704,1.046 101 | 4.1587888357405465,1.07025 102 | 4.196422497736674,1.097 103 | 4.234874282819675,1.12575 104 | 4.286688744988257,0.73475 105 | 4.324322406984384,0.72225 106 | 4.364410438241129,0.731 107 | 4.405862007975994,0.7405 108 | 4.44267754688525,0.749 109 | 4.484129116620115,0.76025 110 | 4.522853609398739,0.76825 111 | 4.560759979090491,0.77125 112 | 4.5989390564778665,0.77725 113 | 4.640117918517108,0.782 114 | 4.679115118991357,0.78425 115 | 4.717294196378733,0.789 116 | 4.757109519939853,0.78825 117 | 4.796652135805349,0.7855 118 | 4.8337403824102285,0.786 119 | 4.871646752101981,0.78275 120 | 4.9109166602718535,0.7785 121 | 4.950186568441726,0.7635 122 | 4.990274599698471,0.74725 123 | 5.028180969390222,0.737 124 | 5.0677235852557185,0.72575 125 | 5.109720570381833,0.71525 126 | 5.149263186247329,0.70625 127 | 5.1863514328522085,0.69975 128 | 5.230530079543315,0.693 129 | 5.269799987713188,0.68925 130 | 5.307979065100563,0.68425 131 | 5.347248973270435,0.68275 132 | 5.386518881440308,0.68075 133 | 5.426606912697053,0.68175 134 | 5.465604113171301,0.67825 135 | 5.502419652080556,0.6835 136 | 5.543871221815422,0.6885 137 | 5.580959468420302,0.67925 138 | 5.624319992024535,0.6555 139 | 5.660044700151294,0.639 140 | 5.700950854494911,0.623 141 | 5.740220762664784,0.6075 142 | 5.783581286269018,0.59475 143 | 5.820124117482649,0.58475 144 | 5.861848394913139,0.57325 145 | 5.899209349213642,0.565 146 | 5.938751965079138,0.55525 147 | 5.9782945809446355,0.55175 148 | 6.017564489114507,0.546 149 | 6.059288766544997,0.5405 150 | 6.097467843932373,0.53825 151 | 6.139464829058487,0.534 152 | 6.176825783358991,0.5325 153 | 6.215822983833238,0.53125 154 | 6.252911230438118,0.53075 -------------------------------------------------------------------------------- /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.hypot(px, py) 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.hypot(x, y) 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 | # for stopping simulation with the esc key. 128 | plt.gcf().canvas.mpl_connect('key_release_event', 129 | lambda event: [exit(0) if event.key == 'escape' else None]) 130 | draw_heatmap(pmap, minx, maxx, miny, maxy, xyreso) 131 | plt.plot(ox, oy, "xr") 132 | plt.plot(0.0, 0.0, "ob") 133 | plt.pause(1.0) 134 | 135 | 136 | if __name__ == '__main__': 137 | main() 138 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/BSplinePath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BSplinePath/bspline_path.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Path Planner 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 scipy_interpolate 12 | 13 | 14 | def approximate_b_spline_path(x: list, y: list, n_path_points: int, 15 | degree: int = 3) -> tuple: 16 | """ 17 | approximate points with a B-Spline path 18 | 19 | :param x: x position list of approximated points 20 | :param y: y position list of approximated points 21 | :param n_path_points: number of path points 22 | :param degree: (Optional) B Spline curve degree 23 | :return: x and y position list of the result path 24 | """ 25 | t = range(len(x)) 26 | x_tup = scipy_interpolate.splrep(t, x, k=degree) 27 | y_tup = scipy_interpolate.splrep(t, y, k=degree) 28 | 29 | x_list = list(x_tup) 30 | x_list[1] = x + [0.0, 0.0, 0.0, 0.0] 31 | 32 | y_list = list(y_tup) 33 | y_list[1] = y + [0.0, 0.0, 0.0, 0.0] 34 | 35 | ipl_t = np.linspace(0.0, len(x) - 1, n_path_points) 36 | rx = scipy_interpolate.splev(ipl_t, x_list) 37 | ry = scipy_interpolate.splev(ipl_t, y_list) 38 | 39 | return rx, ry 40 | 41 | 42 | def interpolate_b_spline_path(x: list, y: list, n_path_points: int, 43 | degree: int = 3) -> tuple: 44 | """ 45 | interpolate points with a B-Spline path 46 | 47 | :param x: x positions of interpolated points 48 | :param y: y positions of interpolated points 49 | :param n_path_points: number of path points 50 | :param degree: B-Spline degree 51 | :return: x and y position list of the result path 52 | """ 53 | ipl_t = np.linspace(0.0, len(x) - 1, len(x)) 54 | spl_i_x = scipy_interpolate.make_interp_spline(ipl_t, x, k=degree) 55 | spl_i_y = scipy_interpolate.make_interp_spline(ipl_t, y, k=degree) 56 | 57 | travel = np.linspace(0.0, len(x) - 1, n_path_points) 58 | return spl_i_x(travel), spl_i_y(travel) 59 | 60 | 61 | def main(): 62 | print(__file__ + " start!!") 63 | # way points 64 | way_point_x = [-1.0, 3.0, 4.0, 2.0, 1.0] 65 | way_point_y = [0.0, -3.0, 1.0, 1.0, 3.0] 66 | n_course_point = 100 # sampling number 67 | 68 | rax, ray = approximate_b_spline_path(way_point_x, way_point_y, 69 | n_course_point) 70 | rix, riy = interpolate_b_spline_path(way_point_x, way_point_y, 71 | n_course_point) 72 | 73 | # show results 74 | plt.plot(way_point_x, way_point_y, '-og', label="way points") 75 | plt.plot(rax, ray, '-r', label="Approximated B-Spline path") 76 | plt.plot(rix, riy, '-b', label="Interpolated B-Spline path") 77 | plt.grid(True) 78 | plt.legend() 79 | plt.axis("equal") 80 | plt.show() 81 | 82 | 83 | if __name__ == '__main__': 84 | main() 85 | -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/BezierPath/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/BezierPath/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/BezierPath/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ClosedLoopRRTStar/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ClosedLoopRRTStar/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ClosedLoopRRTStar/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ClosedLoopRRTStar/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ClosedLoopRRTStar/__init__.py -------------------------------------------------------------------------------- /PathPlanning/ClosedLoopRRTStar/unicycle_model.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Unicycle model class 4 | 5 | author Atsushi Sakai 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | 12 | dt = 0.05 # [s] 13 | L = 0.9 # [m] 14 | steer_max = np.deg2rad(40.0) 15 | curvature_max = math.tan(steer_max) / L 16 | curvature_max = 1.0 / curvature_max + 1.0 17 | 18 | accel_max = 5.0 19 | 20 | 21 | class State: 22 | 23 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 24 | self.x = x 25 | self.y = y 26 | self.yaw = yaw 27 | self.v = v 28 | 29 | 30 | def update(state, a, delta): 31 | 32 | state.x = state.x + state.v * math.cos(state.yaw) * dt 33 | state.y = state.y + state.v * math.sin(state.yaw) * dt 34 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 35 | state.yaw = pi_2_pi(state.yaw) 36 | state.v = state.v + a * dt 37 | 38 | return state 39 | 40 | 41 | def pi_2_pi(angle): 42 | return (angle + math.pi) % (2 * math.pi) - math.pi 43 | 44 | 45 | if __name__ == '__main__': # pragma: no cover 46 | print("start unicycle simulation") 47 | import matplotlib.pyplot as plt 48 | 49 | T = 100 50 | a = [1.0] * T 51 | delta = [np.deg2rad(1.0)] * T 52 | # print(delta) 53 | # print(a, delta) 54 | 55 | state = State() 56 | 57 | x = [] 58 | y = [] 59 | yaw = [] 60 | v = [] 61 | 62 | for (ai, di) in zip(a, delta): 63 | state = update(state, ai, di) 64 | 65 | x.append(state.x) 66 | y.append(state.y) 67 | yaw.append(state.yaw) 68 | v.append(state.v) 69 | 70 | plt.subplots(1) 71 | plt.plot(x, y) 72 | plt.axis("equal") 73 | plt.grid(True) 74 | 75 | plt.subplots(1) 76 | plt.plot(v) 77 | plt.grid(True) 78 | 79 | plt.show() 80 | -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/CubicSpline/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/CubicSpline/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/CubicSpline/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/CubicSpline/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/CubicSpline/__init__.py -------------------------------------------------------------------------------- /PathPlanning/DubinsPath/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/DubinsPath/__init__.py -------------------------------------------------------------------------------- /PathPlanning/HybridAStar/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/HybridAStar/__init__.py -------------------------------------------------------------------------------- /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() -------------------------------------------------------------------------------- /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=True): 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 | # for stopping simulation with the esc key. 58 | plt.gcf().canvas.mpl_connect('key_release_event', 59 | lambda event: [exit(0) if event.key == 'escape' else None]) 60 | plt.plot(sx, sy, "or") 61 | plt.plot(gx, gy, "ob") 62 | plt.plot(rx, ry, "-r") 63 | plt.axis("equal") 64 | plt.pause(1.0) 65 | 66 | if not found_path: 67 | print("Cannot found path") 68 | return [], [] 69 | 70 | return rx, ry 71 | 72 | def solve_dare(self, A, B, Q, R): 73 | """ 74 | solve a discrete time_Algebraic Riccati equation (DARE) 75 | """ 76 | X, Xn = Q, Q 77 | 78 | for i in range(self.MAX_ITER): 79 | Xn = A.T * X * A - A.T * X * B * \ 80 | la.inv(R + B.T * X * B) * B.T * X * A + Q 81 | if (abs(Xn - X)).max() < self.EPS: 82 | break 83 | X = Xn 84 | 85 | return Xn 86 | 87 | def dlqr(self, A, B, Q, R): 88 | """Solve the discrete time lqr controller. 89 | x[k+1] = A x[k] + B u[k] 90 | cost = sum x[k].T*Q*x[k] + u[k].T*R*u[k] 91 | # ref Bertsekas, p.151 92 | """ 93 | 94 | # first, try to solve the ricatti equation 95 | X = self.solve_dare(A, B, Q, R) 96 | 97 | # compute the LQR gain 98 | K = la.inv(B.T @ X @ B + R) @ (B.T @ X @ A) 99 | 100 | eigValues = la.eigvals(A - B @ K) 101 | 102 | return K, X, eigValues 103 | 104 | def get_system_model(self): 105 | 106 | A = np.array([[self.DT, 1.0], 107 | [0.0, self.DT]]) 108 | B = np.array([0.0, 1.0]).reshape(2, 1) 109 | 110 | return A, B 111 | 112 | def lqr_control(self, A, B, x): 113 | 114 | Kopt, X, ev = self.dlqr(A, B, np.eye(2), np.eye(1)) 115 | 116 | u = -Kopt @ x 117 | 118 | return u 119 | 120 | 121 | def main(): 122 | print(__file__ + " start!!") 123 | 124 | ntest = 10 # number of goal 125 | area = 100.0 # sampling area 126 | 127 | lqr_planner = LQRPlanner() 128 | 129 | for i in range(ntest): 130 | sx = 6.0 131 | sy = 6.0 132 | gx = random.uniform(-area, area) 133 | gy = random.uniform(-area, area) 134 | 135 | rx, ry = lqr_planner.lqr_planning(sx, sy, gx, gy, show_animation=SHOW_ANIMATION) 136 | 137 | if SHOW_ANIMATION: # pragma: no cover 138 | plt.plot(sx, sy, "or") 139 | plt.plot(gx, gy, "ob") 140 | plt.plot(rx, ry, "-r") 141 | plt.axis("equal") 142 | plt.pause(1.0) 143 | 144 | 145 | if __name__ == '__main__': 146 | main() 147 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ModelPredictiveTrajectoryGenerator/__init__.py -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/ModelPredictiveTrajectoryGenerator/lookuptable.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /PathPlanning/ModelPredictiveTrajectoryGenerator/motion_model.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import scipy.interpolate 4 | 5 | # motion parameter 6 | L = 1.0 # wheel base 7 | ds = 0.1 # course distanse 8 | v = 10.0 / 3.6 # velocity [m/s] 9 | 10 | 11 | class State: 12 | 13 | def __init__(self, x=0.0, y=0.0, yaw=0.0, v=0.0): 14 | self.x = x 15 | self.y = y 16 | self.yaw = yaw 17 | self.v = v 18 | 19 | 20 | def pi_2_pi(angle): 21 | return (angle + math.pi) % (2 * math.pi) - math.pi 22 | 23 | 24 | def update(state, v, delta, dt, L): 25 | 26 | state.v = v 27 | state.x = state.x + state.v * math.cos(state.yaw) * dt 28 | state.y = state.y + state.v * math.sin(state.yaw) * dt 29 | state.yaw = state.yaw + state.v / L * math.tan(delta) * dt 30 | state.yaw = pi_2_pi(state.yaw) 31 | 32 | return state 33 | 34 | 35 | def generate_trajectory(s, km, kf, k0): 36 | 37 | n = s / ds 38 | time = s / v # [s] 39 | 40 | if isinstance(time, type(np.array([]))): time = time[0] 41 | if isinstance(km, type(np.array([]))): km = km[0] 42 | if isinstance(kf, type(np.array([]))): kf = kf[0] 43 | 44 | tk = np.array([0.0, time / 2.0, time]) 45 | kk = np.array([k0, km, kf]) 46 | t = np.arange(0.0, time, time / n) 47 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 48 | kp = [fkp(ti) for ti in t] 49 | dt = float(time / n) 50 | 51 | # plt.plot(t, kp) 52 | # plt.show() 53 | 54 | state = State() 55 | x, y, yaw = [state.x], [state.y], [state.yaw] 56 | 57 | for ikp in kp: 58 | state = update(state, v, ikp, dt, L) 59 | x.append(state.x) 60 | y.append(state.y) 61 | yaw.append(state.yaw) 62 | 63 | return x, y, yaw 64 | 65 | 66 | def generate_last_state(s, km, kf, k0): 67 | 68 | n = s / ds 69 | time = s / v # [s] 70 | 71 | if isinstance(time, type(np.array([]))): time = time[0] 72 | if isinstance(km, type(np.array([]))): km = km[0] 73 | if isinstance(kf, type(np.array([]))): kf = kf[0] 74 | 75 | tk = np.array([0.0, time / 2.0, time]) 76 | kk = np.array([k0, km, kf]) 77 | t = np.arange(0.0, time, time / n) 78 | fkp = scipy.interpolate.interp1d(tk, kk, kind="quadratic") 79 | kp = [fkp(ti) for ti in t] 80 | dt = time / n 81 | 82 | # plt.plot(t, kp) 83 | # plt.show() 84 | 85 | state = State() 86 | 87 | _ = [update(state, v, ikp, dt, L) for ikp in kp] 88 | 89 | return state.x, state.y, state.yaw 90 | -------------------------------------------------------------------------------- /PathPlanning/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 | # for stopping simulation with the esc key. 102 | plt.gcf().canvas.mpl_connect('key_release_event', 103 | lambda event: [exit(0) if event.key == 'escape' else None]) 104 | plt.plot(ix, iy, "*k") 105 | plt.plot(gix, giy, "*m") 106 | 107 | rx, ry = [sx], [sy] 108 | motion = get_motion_model() 109 | while d >= reso: 110 | minp = float("inf") 111 | minix, miniy = -1, -1 112 | for i, _ in enumerate(motion): 113 | inx = int(ix + motion[i][0]) 114 | iny = int(iy + motion[i][1]) 115 | if inx >= len(pmap) or iny >= len(pmap[0]): 116 | p = float("inf") # outside area 117 | else: 118 | p = pmap[inx][iny] 119 | if minp > p: 120 | minp = p 121 | minix = inx 122 | miniy = iny 123 | ix = minix 124 | iy = miniy 125 | xp = ix * reso + minx 126 | yp = iy * reso + miny 127 | d = np.hypot(gx - xp, gy - yp) 128 | rx.append(xp) 129 | ry.append(yp) 130 | 131 | if show_animation: 132 | plt.plot(ix, iy, ".r") 133 | plt.pause(0.01) 134 | 135 | print("Goal!!") 136 | 137 | return rx, ry 138 | 139 | 140 | def draw_heatmap(data): 141 | data = np.array(data).T 142 | plt.pcolor(data, vmax=100.0, cmap=plt.cm.Blues) 143 | 144 | 145 | def main(): 146 | print("potential_field_planning start") 147 | 148 | sx = 0.0 # start x position [m] 149 | sy = 10.0 # start y positon [m] 150 | gx = 30.0 # goal x position [m] 151 | gy = 30.0 # goal y position [m] 152 | grid_size = 0.5 # potential grid size [m] 153 | robot_radius = 5.0 # robot radius [m] 154 | 155 | ox = [15.0, 5.0, 20.0, 25.0] # obstacle x position list [m] 156 | oy = [25.0, 15.0, 26.0, 25.0] # obstacle y position list [m] 157 | 158 | if show_animation: 159 | plt.grid(True) 160 | plt.axis("equal") 161 | 162 | # path generation 163 | _, _ = potential_field_planning( 164 | sx, sy, gx, gy, ox, oy, grid_size, robot_radius) 165 | 166 | if show_animation: 167 | plt.show() 168 | 169 | 170 | if __name__ == '__main__': 171 | print(__file__ + " start!!") 172 | main() 173 | print(__file__ + " Done!!") 174 | -------------------------------------------------------------------------------- /PathPlanning/QuinticPolynomialsPlanner/quintic_polynomials_planner.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "markdown", 5 | "metadata": {}, 6 | "source": [ 7 | "# Quintic polynomials planner" 8 | ] 9 | }, 10 | { 11 | "cell_type": "markdown", 12 | "metadata": {}, 13 | "source": [ 14 | "## Quintic polynomials for one dimensional robot motion\n", 15 | "\n", 16 | "We assume a one-dimensional robot motion $x(t)$ at time $t$ is formulated as a quintic polynomials based on time as follows:\n", 17 | "\n", 18 | "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(1)\n", 19 | "\n", 20 | "$a_0, a_1. a_2, a_3, a_4, a_5$ are parameters of the quintic polynomial.\n", 21 | "\n", 22 | "It is assumed that terminal states (start and end) are known as boundary conditions.\n", 23 | "\n", 24 | "Start position, velocity, and acceleration are $x_s, v_s, a_s$ respectively.\n", 25 | "\n", 26 | "End position, velocity, and acceleration are $x_e, v_e, a_e$ respectively.\n", 27 | "\n", 28 | "So, when time is 0.\n", 29 | "\n", 30 | "$x(0) = a_0 = x_s$ -- (2)\n", 31 | "\n", 32 | "Then, differentiating the equation (1) with t, \n", 33 | "\n", 34 | "$x'(t) = a_1+2a_2t+3a_3t^2+4a_4t^3+5a_5t^4$ -- (3)\n", 35 | "\n", 36 | "So, when time is 0,\n", 37 | "\n", 38 | "$x'(0) = a_1 = v_s$ -- (4)\n", 39 | "\n", 40 | "Then, differentiating the equation (3) with t again, \n", 41 | "\n", 42 | "$x''(t) = 2a_2+6a_3t+12a_4t^2$ -- (5)\n", 43 | "\n", 44 | "So, when time is 0,\n", 45 | "\n", 46 | "$x''(0) = 2a_2 = a_s$ -- (6)\n", 47 | "\n", 48 | "so, we can calculate $a_0$, $a_1$, $a_2$ with eq. (2), (4), (6) and boundary conditions.\n", 49 | "\n", 50 | "$a_3, a_4, a_5$ are still unknown in eq(1).\n" 51 | ] 52 | }, 53 | { 54 | "cell_type": "markdown", 55 | "metadata": {}, 56 | "source": [ 57 | "We assume that the end time for a maneuver is $T$, we can get these equations from eq (1), (3), (5):\n", 58 | "\n", 59 | "$x(T)=a_0+a_1T+a_2T^2+a_3T^3+a_4T^4+a_5T^5=x_e$ -- (7)\n", 60 | "\n", 61 | "$x'(T)=a_1+2a_2T+3a_3T^2+4a_4T^3+5a_5T^4=v_e$ -- (8)\n", 62 | "\n", 63 | "$x''(T)=2a_2+6a_3T+12a_4T^2+20a_5T^3=a_e$ -- (9)\n" 64 | ] 65 | }, 66 | { 67 | "cell_type": "markdown", 68 | "metadata": {}, 69 | "source": [ 70 | "From eq (7), (8), (9), we can calculate $a_3, a_4, a_5$ to solve the linear equations.\n", 71 | "\n", 72 | "$Ax=b$\n", 73 | "\n", 74 | "$\\begin{bmatrix} T^3 & T^4 & T^5 \\\\ 3T^2 & 4T^3 & 5T^4 \\\\ 6T & 12T^2 & 20T^3 \\end{bmatrix}\n", 75 | "\\begin{bmatrix} a_3\\\\ a_4\\\\ a_5\\end{bmatrix}=\\begin{bmatrix} x_e-x_s-v_sT-0.5a_sT^2\\\\ v_e-v_s-a_sT\\\\ a_e-a_s\\end{bmatrix}$\n", 76 | "\n", 77 | "We can get all unknown parameters now" 78 | ] 79 | }, 80 | { 81 | "cell_type": "markdown", 82 | "metadata": {}, 83 | "source": [ 84 | "## Quintic polynomials for two dimensional robot motion (x-y)\n", 85 | "\n", 86 | "If you use two quintic polynomials along x axis and y axis, you can plan for two dimensional robot motion in x-y plane.\n", 87 | "\n", 88 | "$x(t) = a_0+a_1t+a_2t^2+a_3t^3+a_4t^4+a_5t^5$ --(10)\n", 89 | "\n", 90 | "$y(t) = b_0+b_1t+b_2t^2+b_3t^3+b_4t^4+b_5t^5$ --(11)\n", 91 | "\n", 92 | "It is assumed that terminal states (start and end) are known as boundary conditions.\n", 93 | "\n", 94 | "Start position, orientation, velocity, and acceleration are $x_s, y_s, \\theta_s, v_s, a_s$ respectively.\n", 95 | "\n", 96 | "End position, orientation, velocity, and acceleration are $x_e, y_e. \\theta_e, v_e, a_e$ respectively.\n", 97 | "\n", 98 | "Each velocity and acceleration boundary condition can be calculated with each orientation.\n", 99 | "\n", 100 | "$v_{xs}=v_scos(\\theta_s), v_{ys}=v_ssin(\\theta_s)$\n", 101 | "\n", 102 | "$v_{xe}=v_ecos(\\theta_e), v_{ye}=v_esin(\\theta_e)$\n" 103 | ] 104 | }, 105 | { 106 | "cell_type": "code", 107 | "execution_count": null, 108 | "metadata": {}, 109 | "outputs": [], 110 | "source": [] 111 | } 112 | ], 113 | "metadata": { 114 | "kernelspec": { 115 | "display_name": "Python 3", 116 | "language": "python", 117 | "name": "python3" 118 | }, 119 | "language_info": { 120 | "codemirror_mode": { 121 | "name": "ipython", 122 | "version": 3 123 | }, 124 | "file_extension": ".py", 125 | "mimetype": "text/x-python", 126 | "name": "python", 127 | "nbconvert_exporter": "python", 128 | "pygments_lexer": "ipython3", 129 | "version": "3.7.5" 130 | }, 131 | "pycharm": { 132 | "stem_cell": { 133 | "cell_type": "raw", 134 | "metadata": { 135 | "collapsed": false 136 | }, 137 | "source": [] 138 | } 139 | } 140 | }, 141 | "nbformat": 4, 142 | "nbformat_minor": 1 143 | } 144 | -------------------------------------------------------------------------------- /PathPlanning/RRT/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRT/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRT/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRT/figure_1.png -------------------------------------------------------------------------------- /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 | return True # OK 80 | 81 | 82 | def path_smoothing(path, max_iter, obstacle_list): 83 | le = get_path_length(path) 84 | 85 | for i in range(max_iter): 86 | # Sample two points 87 | pickPoints = [random.uniform(0, le), random.uniform(0, le)] 88 | pickPoints.sort() 89 | first = get_target_point(path, pickPoints[0]) 90 | second = get_target_point(path, pickPoints[1]) 91 | 92 | if first[2] <= 0 or second[2] <= 0: 93 | continue 94 | 95 | if (second[2] + 1) > len(path): 96 | continue 97 | 98 | if second[2] == first[2]: 99 | continue 100 | 101 | # collision check 102 | if not line_collision_check(first, second, obstacle_list): 103 | continue 104 | 105 | # Create New path 106 | newPath = [] 107 | newPath.extend(path[:first[2] + 1]) 108 | newPath.append([first[0], first[1]]) 109 | newPath.append([second[0], second[1]]) 110 | newPath.extend(path[second[2] + 1:]) 111 | path = newPath 112 | le = get_path_length(path) 113 | 114 | return path 115 | 116 | 117 | def main(): 118 | # ====Search Path with RRT==== 119 | # Parameter 120 | obstacleList = [ 121 | (5, 5, 1), 122 | (3, 6, 2), 123 | (3, 8, 2), 124 | (3, 10, 2), 125 | (7, 5, 2), 126 | (9, 5, 2) 127 | ] # [x,y,size] 128 | rrt = RRT(start=[0, 0], goal=[6, 10], 129 | rand_area=[-2, 15], obstacle_list=obstacleList) 130 | path = rrt.planning(animation=show_animation) 131 | 132 | # Path smoothing 133 | maxIter = 1000 134 | smoothedPath = path_smoothing(path, maxIter, obstacleList) 135 | 136 | # Draw final path 137 | if show_animation: 138 | rrt.draw_graph() 139 | plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r') 140 | 141 | plt.plot([x for (x, y) in smoothedPath], [ 142 | y for (x, y) in smoothedPath], '-c') 143 | 144 | plt.grid(True) 145 | plt.pause(0.01) # Need for Mac 146 | plt.show() 147 | 148 | 149 | if __name__ == '__main__': 150 | main() 151 | -------------------------------------------------------------------------------- /PathPlanning/RRTDubins/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRTDubins/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRTStar/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRTStar/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/RRTStar/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRTStar/__init__.py -------------------------------------------------------------------------------- /PathPlanning/RRTStarReedsShepp/figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/RRTStarReedsShepp/figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_1.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_2.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_3.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_4.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_5.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/Figure_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/Figure_6.png -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/StateLatticePlanner/__init__.py -------------------------------------------------------------------------------- /PathPlanning/StateLatticePlanner/lookuptable.csv: -------------------------------------------------------------------------------- 1 | x,y,yaw,s,km,kf 2 | 1.0,0.0,0.0,1.0,0.0,0.0 3 | 0.9734888894493215,-0.009758406565994977,0.5358080146312756,0.9922329557399788,-0.10222538550473198,3.0262632253145982 4 | 10.980728996433243,-0.0003093605787364978,0.522622783944529,11.000391678142623,0.00010296091030877934,0.2731556687244648 5 | 16.020309241920156,0.0001292339008200291,0.5243399938698222,16.100019813021202,0.00013263212395994706,0.18999138959173634 6 | 20.963495745193626,-0.00033031017429944326,0.5226120033275024,21.10082901143343,0.00011687467551566884,0.14550546012583987 7 | 6.032553475650599,2.008504211720188,0.5050517859971599,6.400329805864408,0.1520002249689879,-0.13105940607691127 8 | 10.977487445230075,2.0078696810700034,0.5263634407901872,11.201040572298973,0.04895863722280565,0.08356555007223682 9 | 15.994057699325753,2.025659106131227,0.5303858891065698,16.200300421483128,0.0235708657178127,0.10002225103921249 10 | 20.977228843605943,2.0281289825388513,0.5300376140865567,21.20043308669372,0.013795675421657671,0.09331700188063087 11 | 25.95453914157977,1.9926432818499131,0.5226203078411618,26.200880299840527,0.00888830054451281,0.0830622000626594 12 | 0.9999999999999999,0.0,0.0,1.0,0.0,0.0 13 | 5.999999999670752,5.231312388722274e-05,1.4636120911014667e-05,5.996117185283419,4.483756968024291e-06,-3.4422519205046243e-06 14 | 10.999749470720566,-0.011978787043239986,0.022694802592583763,10.99783855994015,-0.00024209503125174496,0.01370491008661795 15 | 15.999885224357776,-0.018937230084507616,0.011565580878015763,15.99839381389597,-0.0002086399372890716,0.005267247862667496 16 | 20.999882688881286,-0.030304200126461317,0.009117836885732596,20.99783120184498,-0.00020013159571184735,0.0034529188783636866 17 | 25.999911270030413,-0.025754431694664327,0.0074809531598503615,25.99674782258235,-0.0001111138115390646,0.0021946603965658368 18 | 10.952178818975062,1.993067260835455,0.0045572480669707136,11.17961498195845,0.04836813285436623,-0.19328716251760758 19 | 16.028306009258,2.0086286208315407,0.009306594796759554,16.122411866381054,0.02330689045417979,-0.08877002085985948 20 | 20.971603115419715,1.9864158336174966,0.007016819403167119,21.093006725076872,0.013439123130720928,-0.05238318300611623 21 | 25.997019676818372,1.9818581321430093,0.007020172975955249,26.074021794586585,0.00876496148602802,-0.03362579291686346 22 | 16.017903482982604,4.009490840390534,-5.293114796312698e-05,16.600937712976638,0.044543450568614244,-0.17444651314466567 23 | 20.98845988414163,3.956600199823583,-0.01050744134070728,21.40149119463485,0.02622674388276059,-0.10625681152519345 24 | 25.979448381017914,3.9968223055054977,-0.00012819253386682928,26.30504721211744,0.017467093413146118,-0.06914750106424669 25 | 25.96268055563514,5.9821266846168,4.931311239565056e-05,26.801388563459287,0.025426008913226557,-0.10047663078001536 26 | -------------------------------------------------------------------------------- /PathPlanning/VisibilityRoadMap/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/VisibilityRoadMap/__init__.py -------------------------------------------------------------------------------- /PathPlanning/VisibilityRoadMap/geometry.py: -------------------------------------------------------------------------------- 1 | class Geometry: 2 | class Point: 3 | def __init__(self, x, y): 4 | self.x = x 5 | self.y = y 6 | 7 | @staticmethod 8 | def is_seg_intersect(p1, q1, p2, q2): 9 | 10 | def on_segment(p, q, r): 11 | if ((q.x <= max(p.x, r.x)) and (q.x >= min(p.x, r.x)) and 12 | (q.y <= max(p.y, r.y)) and (q.y >= min(p.y, r.y))): 13 | return True 14 | return False 15 | 16 | def orientation(p, q, r): 17 | val = (float(q.y - p.y) * (r.x - q.x)) - ( 18 | float(q.x - p.x) * (r.y - q.y)) 19 | if val > 0: 20 | return 1 21 | if val < 0: 22 | return 2 23 | return 0 24 | 25 | # Find the 4 orientations required for 26 | # the general and special cases 27 | o1 = orientation(p1, q1, p2) 28 | o2 = orientation(p1, q1, q2) 29 | o3 = orientation(p2, q2, p1) 30 | o4 = orientation(p2, q2, q1) 31 | 32 | if (o1 != o2) and (o3 != o4): 33 | return True 34 | if (o1 == 0) and on_segment(p1, p2, q1): 35 | return True 36 | if (o2 == 0) and on_segment(p1, q2, q1): 37 | return True 38 | if (o3 == 0) and on_segment(p2, p1, q2): 39 | return True 40 | if (o4 == 0) and on_segment(p2, q1, q2): 41 | return True 42 | 43 | return False -------------------------------------------------------------------------------- /PathPlanning/VoronoiRoadMap/dijkstra_search.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Dijkstra Search library 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import matplotlib.pyplot as plt 10 | import math 11 | import numpy as np 12 | 13 | 14 | class DijkstraSearch: 15 | class Node: 16 | """ 17 | Node class for dijkstra search 18 | """ 19 | 20 | def __init__(self, x, y, cost=None, parent=None, edge_ids=None): 21 | self.x = x 22 | self.y = y 23 | self.cost = cost 24 | self.parent = parent 25 | self.edge_ids = edge_ids 26 | 27 | def __str__(self): 28 | return str(self.x) + "," + str(self.y) + "," + str( 29 | self.cost) + "," + str(self.parent) 30 | 31 | def __init__(self, show_animation): 32 | self.show_animation = show_animation 33 | 34 | def search(self, sx, sy, gx, gy, node_x, node_y, edge_ids_list): 35 | """ 36 | Search shortest path 37 | 38 | sx: start x positions [m] 39 | sy: start y positions [m] 40 | gx: goal x position [m] 41 | gx: goal x position [m] 42 | node_x: node x position 43 | node_y: node y position 44 | edge_ids_list: edge_list each item includes a list of edge ids 45 | """ 46 | 47 | start_node = self.Node(sx, sy, 0.0, -1) 48 | goal_node = self.Node(gx, gy, 0.0, -1) 49 | current_node = None 50 | 51 | open_set, close_set = dict(), dict() 52 | open_set[self.find_id(node_x, node_y, start_node)] = start_node 53 | 54 | while True: 55 | if self.has_node_in_set(close_set, goal_node): 56 | print("goal is found!") 57 | goal_node.parent = current_node.parent 58 | goal_node.cost = current_node.cost 59 | break 60 | elif not open_set: 61 | print("Cannot find path") 62 | break 63 | 64 | current_id = min(open_set, key=lambda o: open_set[o].cost) 65 | current_node = open_set[current_id] 66 | 67 | # show graph 68 | if self.show_animation and len( 69 | close_set.keys()) % 2 == 0: # pragma: no cover 70 | plt.plot(current_node.x, current_node.y, "xg") 71 | # for stopping simulation with the esc key. 72 | plt.gcf().canvas.mpl_connect( 73 | 'key_release_event', 74 | lambda event: [exit(0) if event.key == 'escape' else None]) 75 | plt.pause(0.1) 76 | 77 | # Remove the item from the open set 78 | del open_set[current_id] 79 | # Add it to the closed set 80 | close_set[current_id] = current_node 81 | 82 | # expand search grid based on motion model 83 | for i in range(len(edge_ids_list[current_id])): 84 | n_id = edge_ids_list[current_id][i] 85 | dx = node_x[n_id] - current_node.x 86 | dy = node_y[n_id] - current_node.y 87 | d = math.hypot(dx, dy) 88 | node = self.Node(node_x[n_id], node_y[n_id], 89 | current_node.cost + d, current_id) 90 | 91 | if n_id in close_set: 92 | continue 93 | # Otherwise if it is already in the open set 94 | if n_id in open_set: 95 | if open_set[n_id].cost > node.cost: 96 | open_set[n_id] = node 97 | else: 98 | open_set[n_id] = node 99 | 100 | # generate final course 101 | rx, ry = self.generate_final_path(close_set, goal_node) 102 | 103 | return rx, ry 104 | 105 | @staticmethod 106 | def generate_final_path(close_set, goal_node): 107 | rx, ry = [goal_node.x], [goal_node.y] 108 | parent = goal_node.parent 109 | while parent != -1: 110 | n = close_set[parent] 111 | rx.append(n.x) 112 | ry.append(n.y) 113 | parent = n.parent 114 | rx, ry = rx[::-1], ry[::-1] # reverse it 115 | return rx, ry 116 | 117 | def has_node_in_set(self, target_set, node): 118 | for key in target_set: 119 | if self.is_same_node(target_set[key], node): 120 | return True 121 | return False 122 | 123 | def find_id(self, node_x_list, node_y_list, target_node): 124 | for i, _ in enumerate(node_x_list): 125 | if self.is_same_node_with_xy(node_x_list[i], node_y_list[i], 126 | target_node): 127 | return i 128 | 129 | @staticmethod 130 | def is_same_node_with_xy(node_x, node_y, node_b): 131 | dist = np.hypot(node_x - node_b.x, 132 | node_y - node_b.y) 133 | return dist <= 0.1 134 | 135 | @staticmethod 136 | def is_same_node(node_a, node_b): 137 | dist = np.hypot(node_a.x - node_b.x, 138 | node_b.y - node_b.y) 139 | return dist <= 0.1 140 | -------------------------------------------------------------------------------- /PathPlanning/VoronoiRoadMap/kdtree.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Kd tree Search library 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import scipy.spatial 10 | 11 | 12 | class KDTree: 13 | """ 14 | Nearest neighbor search class with KDTree 15 | """ 16 | 17 | def __init__(self, data): 18 | # store kd-tree 19 | self.tree = scipy.spatial.cKDTree(data) 20 | 21 | def search(self, inp, k=1): 22 | """ 23 | Search NN 24 | 25 | inp: input data, single frame or multi frame 26 | 27 | """ 28 | 29 | if len(inp.shape) >= 2: # multi input 30 | index = [] 31 | dist = [] 32 | 33 | for i in inp.T: 34 | idist, iindex = self.tree.query(i, k=k) 35 | index.append(iindex) 36 | dist.append(idist) 37 | 38 | return index, dist 39 | 40 | dist, index = self.tree.query(inp, k=k) 41 | return index, dist 42 | 43 | def search_in_distance(self, inp, r): 44 | """ 45 | find points with in a distance r 46 | """ 47 | 48 | index = self.tree.query_ball_point(inp, r) 49 | return index 50 | -------------------------------------------------------------------------------- /PathPlanning/VoronoiRoadMap/voronoi_road_map.py: -------------------------------------------------------------------------------- 1 | """ 2 | 3 | Voronoi Road Map Planner 4 | 5 | author: Atsushi Sakai (@Atsushi_twi) 6 | 7 | """ 8 | 9 | import math 10 | import numpy as np 11 | import scipy.spatial 12 | import matplotlib.pyplot as plt 13 | from dijkstra_search import DijkstraSearch 14 | from kdtree import KDTree 15 | 16 | show_animation = True 17 | 18 | 19 | class VoronoiRoadMapPlanner: 20 | 21 | def __init__(self): 22 | # parameter 23 | self.N_KNN = 10 # number of edge from one sampled point 24 | self.MAX_EDGE_LEN = 30.0 # [m] Maximum edge length 25 | 26 | def planning(self, sx, sy, gx, gy, ox, oy, robot_radius): 27 | obstacle_tree = KDTree(np.vstack((ox, oy)).T) 28 | 29 | sample_x, sample_y = self.voronoi_sampling(sx, sy, gx, gy, ox, oy) 30 | if show_animation: # pragma: no cover 31 | plt.plot(sample_x, sample_y, ".b") 32 | 33 | road_map_info = self.generate_road_map_info( 34 | sample_x, sample_y, robot_radius, obstacle_tree) 35 | 36 | rx, ry = DijkstraSearch(show_animation).search(sx, sy, gx, gy, 37 | sample_x, sample_y, 38 | road_map_info) 39 | return rx, ry 40 | 41 | def is_collision(self, sx, sy, gx, gy, rr, obstacle_kd_tree): 42 | x = sx 43 | y = sy 44 | dx = gx - sx 45 | dy = gy - sy 46 | yaw = math.atan2(gy - sy, gx - sx) 47 | d = math.hypot(dx, dy) 48 | 49 | if d >= self.MAX_EDGE_LEN: 50 | return True 51 | 52 | D = rr 53 | n_step = round(d / D) 54 | 55 | for i in range(n_step): 56 | ids, dist = obstacle_kd_tree.search(np.array([x, y]).reshape(2, 1)) 57 | if dist[0] <= rr: 58 | return True # collision 59 | x += D * math.cos(yaw) 60 | y += D * math.sin(yaw) 61 | 62 | # goal point check 63 | ids, dist = obstacle_kd_tree.search(np.array([gx, gy]).reshape(2, 1)) 64 | if dist[0] <= rr: 65 | return True # collision 66 | 67 | return False # OK 68 | 69 | def generate_road_map_info(self, node_x, node_y, rr, obstacle_tree): 70 | """ 71 | Road map generation 72 | 73 | node_x: [m] x positions of sampled points 74 | node_y: [m] y positions of sampled points 75 | rr: Robot Radius[m] 76 | obstacle_tree: KDTree object of obstacles 77 | """ 78 | 79 | road_map = [] 80 | n_sample = len(node_x) 81 | node_tree = KDTree(np.vstack((node_x, node_y)).T) 82 | 83 | for (i, ix, iy) in zip(range(n_sample), node_x, node_y): 84 | 85 | index, dists = node_tree.search( 86 | np.array([ix, iy]).reshape(2, 1), k=n_sample) 87 | 88 | inds = index[0] 89 | edge_id = [] 90 | 91 | for ii in range(1, len(inds)): 92 | nx = node_x[inds[ii]] 93 | ny = node_y[inds[ii]] 94 | 95 | if not self.is_collision(ix, iy, nx, ny, rr, obstacle_tree): 96 | edge_id.append(inds[ii]) 97 | 98 | if len(edge_id) >= self.N_KNN: 99 | break 100 | 101 | road_map.append(edge_id) 102 | 103 | # plot_road_map(road_map, sample_x, sample_y) 104 | 105 | return road_map 106 | 107 | @staticmethod 108 | def plot_road_map(road_map, sample_x, sample_y): # pragma: no cover 109 | 110 | for i, _ in enumerate(road_map): 111 | for ii in range(len(road_map[i])): 112 | ind = road_map[i][ii] 113 | 114 | plt.plot([sample_x[i], sample_x[ind]], 115 | [sample_y[i], sample_y[ind]], "-k") 116 | 117 | @staticmethod 118 | def voronoi_sampling(sx, sy, gx, gy, ox, oy): 119 | oxy = np.vstack((ox, oy)).T 120 | 121 | # generate voronoi point 122 | vor = scipy.spatial.Voronoi(oxy) 123 | sample_x = [ix for [ix, _] in vor.vertices] 124 | sample_y = [iy for [_, iy] in vor.vertices] 125 | 126 | sample_x.append(sx) 127 | sample_y.append(sy) 128 | sample_x.append(gx) 129 | sample_y.append(gy) 130 | 131 | return sample_x, sample_y 132 | 133 | 134 | def main(): 135 | print(__file__ + " start!!") 136 | 137 | # start and goal position 138 | sx = 10.0 # [m] 139 | sy = 10.0 # [m] 140 | gx = 50.0 # [m] 141 | gy = 50.0 # [m] 142 | robot_size = 5.0 # [m] 143 | 144 | ox = [] 145 | oy = [] 146 | 147 | for i in range(60): 148 | ox.append(i) 149 | oy.append(0.0) 150 | for i in range(60): 151 | ox.append(60.0) 152 | oy.append(i) 153 | for i in range(61): 154 | ox.append(i) 155 | oy.append(60.0) 156 | for i in range(61): 157 | ox.append(0.0) 158 | oy.append(i) 159 | for i in range(40): 160 | ox.append(20.0) 161 | oy.append(i) 162 | for i in range(40): 163 | ox.append(40.0) 164 | oy.append(60.0 - i) 165 | 166 | if show_animation: # pragma: no cover 167 | plt.plot(ox, oy, ".k") 168 | plt.plot(sx, sy, "^r") 169 | plt.plot(gx, gy, "^c") 170 | plt.grid(True) 171 | plt.axis("equal") 172 | 173 | rx, ry = VoronoiRoadMapPlanner().planning(sx, sy, gx, gy, ox, oy, 174 | robot_size) 175 | 176 | assert rx, 'Cannot found path' 177 | 178 | if show_animation: # pragma: no cover 179 | plt.plot(rx, ry, "-r") 180 | plt.pause(0.1) 181 | plt.show() 182 | 183 | 184 | if __name__ == '__main__': 185 | main() 186 | -------------------------------------------------------------------------------- /PathPlanning/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathPlanning/__init__.py -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/cgmres_nmpc/Figure_1.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/cgmres_nmpc/Figure_2.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/cgmres_nmpc/Figure_3.png -------------------------------------------------------------------------------- /PathTracking/cgmres_nmpc/Figure_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/cgmres_nmpc/Figure_4.png -------------------------------------------------------------------------------- /PathTracking/lqr_steer_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/lqr_steer_control/__init__.py -------------------------------------------------------------------------------- /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.hypot(x_diff, y_diff) 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.hypot(x_diff, y_diff) 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 | # for stopping simulation with the esc key. 97 | plt.gcf().canvas.mpl_connect('key_release_event', 98 | lambda event: [exit(0) if event.key == 'escape' else None]) 99 | 100 | plt.xlim(0, 20) 101 | plt.ylim(0, 20) 102 | 103 | plt.pause(dt) 104 | 105 | 106 | def transformation_matrix(x, y, theta): 107 | return np.array([ 108 | [np.cos(theta), -np.sin(theta), x], 109 | [np.sin(theta), np.cos(theta), y], 110 | [0, 0, 1] 111 | ]) 112 | 113 | 114 | def main(): 115 | 116 | for i in range(5): 117 | x_start = 20 * random() 118 | y_start = 20 * random() 119 | theta_start = 2 * np.pi * random() - np.pi 120 | x_goal = 20 * random() 121 | y_goal = 20 * random() 122 | theta_goal = 2 * np.pi * random() - np.pi 123 | print("Initial x: %.2f m\nInitial y: %.2f m\nInitial theta: %.2f rad\n" % 124 | (x_start, y_start, theta_start)) 125 | print("Goal x: %.2f m\nGoal y: %.2f m\nGoal theta: %.2f rad\n" % 126 | (x_goal, y_goal, theta_goal)) 127 | move_to_pose(x_start, y_start, theta_start, x_goal, y_goal, theta_goal) 128 | 129 | 130 | if __name__ == '__main__': 131 | main() 132 | -------------------------------------------------------------------------------- /PathTracking/rear_wheel_feedback/Figure_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/PathTracking/rear_wheel_feedback/Figure_2.png -------------------------------------------------------------------------------- /SLAM/EKFSLAM/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/SLAM/EKFSLAM/animation.png -------------------------------------------------------------------------------- /SLAM/FastSLAM1/animation.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/SLAM/FastSLAM1/animation.png -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/LaTeX/.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore LaTeX generated files 2 | graphSLAM_formulation.* 3 | !graphSLAM_formulation.tex 4 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/LaTeX/graphSLAM.bib: -------------------------------------------------------------------------------- 1 | @article{blanco2010tutorial, 2 | title={A tutorial on ${SE}(3)$ transformation parameterizations and on-manifold optimization}, 3 | author={Blanco, Jose-Luis}, 4 | journal={University of Malaga, Tech. Rep}, 5 | volume={3}, 6 | year={2010}, 7 | publisher={Citeseer} 8 | } 9 | 10 | @article{grisetti2010tutorial, 11 | title={A tutorial on graph-based {SLAM}}, 12 | author={Grisetti, Giorgio and Kummerle, Rainer and Stachniss, Cyrill and Burgard, Wolfram}, 13 | journal={IEEE Intelligent Transportation Systems Magazine}, 14 | volume={2}, 15 | number={4}, 16 | pages={31--43}, 17 | year={2010}, 18 | publisher={IEEE} 19 | } 20 | 21 | @article{thrun2006graph, 22 | title={The graph {SLAM} algorithm with applications to large-scale mapping of urban structures}, 23 | author={Thrun, Sebastian and Montemerlo, Michael}, 24 | journal={The International Journal of Robotics Research}, 25 | volume={25}, 26 | number={5-6}, 27 | pages={403--429}, 28 | year={2006}, 29 | publisher={SAGE Publications} 30 | } 31 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/data/README.rst: -------------------------------------------------------------------------------- 1 | Acknowledgments and References 2 | ============================== 3 | 4 | Thanks to Luca Larlone for allowing inclusion of the `Intel dataset `_ in this repo. 5 | 6 | 1. Carlone, L. and Censi, A., 2014. `From angular manifolds to the integer lattice: Guaranteed orientation estimation with application to pose graph optimization `_. IEEE Transactions on Robotics, 30(2), pp.475-492. 7 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphSLAM_formulation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/SLAM/GraphBasedSLAM/graphSLAM_formulation.pdf -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Graph SLAM solver in Python. 8 | 9 | """ 10 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/edge/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/load.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Functions for loading graphs. 8 | 9 | """ 10 | 11 | 12 | import logging 13 | 14 | import numpy as np 15 | 16 | from .edge.edge_odometry import EdgeOdometry 17 | from .graph import Graph 18 | from .pose.se2 import PoseSE2 19 | from .util import upper_triangular_matrix_to_full_matrix 20 | from .vertex import Vertex 21 | 22 | 23 | _LOGGER = logging.getLogger(__name__) 24 | 25 | 26 | def load_g2o_se2(infile): 27 | """Load an :math:`SE(2)` graph from a .g2o file. 28 | 29 | Parameters 30 | ---------- 31 | infile : str 32 | The path to the .g2o file 33 | 34 | Returns 35 | ------- 36 | Graph 37 | The loaded graph 38 | 39 | """ 40 | edges = [] 41 | vertices = [] 42 | 43 | with open(infile) as f: 44 | for line in f.readlines(): 45 | if line.startswith("VERTEX_SE2"): 46 | numbers = line[10:].split() 47 | arr = np.array([float(number) for number in numbers[1:]], dtype=np.float64) 48 | p = PoseSE2(arr[:2], arr[2]) 49 | v = Vertex(int(numbers[0]), p) 50 | vertices.append(v) 51 | continue 52 | 53 | if line.startswith("EDGE_SE2"): 54 | numbers = line[9:].split() 55 | arr = np.array([float(number) for number in numbers[2:]], dtype=np.float64) 56 | vertex_ids = [int(numbers[0]), int(numbers[1])] 57 | estimate = PoseSE2(arr[:2], arr[2]) 58 | information = upper_triangular_matrix_to_full_matrix(arr[3:], 3) 59 | e = EdgeOdometry(vertex_ids, information, estimate) 60 | edges.append(e) 61 | continue 62 | 63 | if line.strip(): 64 | _LOGGER.warning("Line not supported -- '%s'", line.rstrip()) 65 | 66 | return Graph(edges, vertices) 67 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/pose/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/pose/se2.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | r"""Representation of a pose in :math:`SE(2)`. 8 | 9 | """ 10 | 11 | import math 12 | 13 | import numpy as np 14 | 15 | from ..util import neg_pi_to_pi 16 | 17 | 18 | class PoseSE2(np.ndarray): 19 | r"""A representation of a pose in :math:`SE(2)`. 20 | 21 | Parameters 22 | ---------- 23 | position : np.ndarray, list 24 | The position in :math:`\mathbb{R}^2` 25 | orientation : float 26 | The angle of the pose (in radians) 27 | 28 | """ 29 | def __new__(cls, position, orientation): 30 | obj = np.array([position[0], position[1], neg_pi_to_pi(orientation)], dtype=np.float64).view(cls) 31 | return obj 32 | 33 | # pylint: disable=arguments-differ 34 | def copy(self): 35 | """Return a copy of the pose. 36 | 37 | Returns 38 | ------- 39 | PoseSE2 40 | A copy of the pose 41 | 42 | """ 43 | return PoseSE2(self[:2], self[2]) 44 | 45 | def to_array(self): 46 | """Return the pose as a numpy array. 47 | 48 | Returns 49 | ------- 50 | np.ndarray 51 | The pose as a numpy array 52 | 53 | """ 54 | return np.array(self) 55 | 56 | def to_compact(self): 57 | """Return the pose as a compact numpy array. 58 | 59 | Returns 60 | ------- 61 | np.ndarray 62 | The pose as a compact numpy array 63 | 64 | """ 65 | return np.array(self) 66 | 67 | def to_matrix(self): 68 | """Return the pose as an :math:`SE(2)` matrix. 69 | 70 | Returns 71 | ------- 72 | np.ndarray 73 | The pose as an :math:`SE(2)` matrix 74 | 75 | """ 76 | return np.array([[np.cos(self[2]), -np.sin(self[2]), self[0]], [np.sin(self[2]), np.cos(self[2]), self[1]], [0., 0., 1.]], dtype=np.float64) 77 | 78 | @classmethod 79 | def from_matrix(cls, matrix): 80 | """Return the pose as an :math:`SE(2)` matrix. 81 | 82 | Parameters 83 | ---------- 84 | matrix : np.ndarray 85 | The :math:`SE(2)` matrix that will be converted to a `PoseSE2` instance 86 | 87 | Returns 88 | ------- 89 | PoseSE2 90 | The matrix as a `PoseSE2` object 91 | 92 | """ 93 | return cls([matrix[0, 2], matrix[1, 2]], math.atan2(matrix[1, 0], matrix[0, 0])) 94 | 95 | # ======================================================================= # 96 | # # 97 | # Properties # 98 | # # 99 | # ======================================================================= # 100 | @property 101 | def position(self): 102 | """Return the pose's position. 103 | 104 | Returns 105 | ------- 106 | np.ndarray 107 | The position portion of the pose 108 | 109 | """ 110 | return np.array(self[:2]) 111 | 112 | @property 113 | def orientation(self): 114 | """Return the pose's orientation. 115 | 116 | Returns 117 | ------- 118 | float 119 | The angle of the pose 120 | 121 | """ 122 | return self[2] 123 | 124 | @property 125 | def inverse(self): 126 | """Return the pose's inverse. 127 | 128 | Returns 129 | ------- 130 | PoseSE2 131 | The pose's inverse 132 | 133 | """ 134 | return PoseSE2([-self[0] * np.cos(self[2]) - self[1] * np.sin(self[2]), self[0] * np.sin(self[2]) - self[1] * np.cos([self[2]])], -self[2]) 135 | 136 | # ======================================================================= # 137 | # # 138 | # Magic Methods # 139 | # # 140 | # ======================================================================= # 141 | def __add__(self, other): 142 | r"""Add poses (i.e., pose composition): :math:`p_1 \oplus p_2`. 143 | 144 | Parameters 145 | ---------- 146 | other : PoseSE2 147 | The other pose 148 | 149 | Returns 150 | ------- 151 | PoseSE2 152 | The result of pose composition 153 | 154 | """ 155 | return PoseSE2([self[0] + other[0] * np.cos(self[2]) - other[1] * np.sin(self[2]), self[1] + other[0] * np.sin(self[2]) + other[1] * np.cos(self[2])], neg_pi_to_pi(self[2] + other[2])) 156 | 157 | def __sub__(self, other): 158 | r"""Subtract poses (i.e., inverse pose composition): :math:`p_1 \ominus p_2`. 159 | 160 | Parameters 161 | ---------- 162 | other : PoseSE2 163 | The other pose 164 | 165 | Returns 166 | ------- 167 | PoseSE2 168 | The result of inverse pose composition 169 | 170 | """ 171 | return PoseSE2([(self[0] - other[0]) * np.cos(other[2]) + (self[1] - other[1]) * np.sin(other[2]), (other[0] - self[0]) * np.sin(other[2]) + (self[1] - other[1]) * np.cos(other[2])], neg_pi_to_pi(self[2] - other[2])) 172 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/util.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """Utility functions used throughout the package. 8 | 9 | """ 10 | 11 | import numpy as np 12 | 13 | 14 | TWO_PI = 2 * np.pi 15 | 16 | 17 | def neg_pi_to_pi(angle): 18 | r"""Normalize ``angle`` to be in :math:`[-\pi, \pi)`. 19 | 20 | Parameters 21 | ---------- 22 | angle : float 23 | An angle (in radians) 24 | 25 | Returns 26 | ------- 27 | float 28 | The angle normalized to :math:`[-\pi, \pi)` 29 | 30 | """ 31 | return (angle + np.pi) % (TWO_PI) - np.pi 32 | 33 | 34 | def solve_for_edge_dimensionality(n): 35 | r"""Solve for the dimensionality of an edge. 36 | 37 | In a .g2o file, an edge is specified as `` ``, where only the upper triangular portion of the matrix is provided. 38 | 39 | This solves the problem: 40 | 41 | .. math:: 42 | 43 | d + \frac{d (d + 1)}{2} = n 44 | 45 | Returns 46 | ------- 47 | int 48 | The dimensionality of the edge 49 | 50 | """ 51 | return int(round(np.sqrt(2 * n + 2.25) - 1.5)) 52 | 53 | 54 | def upper_triangular_matrix_to_full_matrix(arr, n): 55 | """Given an upper triangular matrix, return the full matrix. 56 | 57 | Parameters 58 | ---------- 59 | arr : np.ndarray 60 | The upper triangular portion of the matrix 61 | n : int 62 | The size of the matrix 63 | 64 | Returns 65 | ------- 66 | mat : np.ndarray 67 | The full matrix 68 | 69 | """ 70 | triu0 = np.triu_indices(n, 0) 71 | triu1 = np.triu_indices(n, 1) 72 | tril1 = np.tril_indices(n, -1) 73 | 74 | mat = np.zeros((n, n), dtype=np.float64) 75 | mat[triu0] = arr 76 | mat[tril1] = mat[triu1] 77 | 78 | return mat 79 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/graphslam/vertex.py: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2020 Jeff Irion and contributors 2 | # 3 | # This file originated from the `graphslam` package: 4 | # 5 | # https://github.com/JeffLIrion/python-graphslam 6 | 7 | """A ``Vertex`` class. 8 | 9 | """ 10 | 11 | import matplotlib.pyplot as plt 12 | 13 | 14 | # pylint: disable=too-few-public-methods 15 | class Vertex: 16 | """A class for representing a vertex in Graph SLAM. 17 | 18 | Parameters 19 | ---------- 20 | vertex_id : int 21 | The vertex's unique ID 22 | pose : graphslam.pose.se2.PoseSE2 23 | The pose associated with the vertex 24 | vertex_index : int, None 25 | The vertex's index in the graph's ``vertices`` list 26 | 27 | Attributes 28 | ---------- 29 | id : int 30 | The vertex's unique ID 31 | index : int, None 32 | The vertex's index in the graph's ``vertices`` list 33 | pose : graphslam.pose.se2.PoseSE2 34 | The pose associated with the vertex 35 | 36 | """ 37 | def __init__(self, vertex_id, pose, vertex_index=None): 38 | self.id = vertex_id 39 | self.pose = pose 40 | self.index = vertex_index 41 | 42 | def to_g2o(self): 43 | """Export the vertex to the .g2o format. 44 | 45 | Returns 46 | ------- 47 | str 48 | The vertex in .g2o format 49 | 50 | """ 51 | return "VERTEX_SE2 {} {} {} {}\n".format(self.id, self.pose[0], self.pose[1], self.pose[2]) 52 | 53 | def plot(self, color='r', marker='o', markersize=3): 54 | """Plot the vertex. 55 | 56 | Parameters 57 | ---------- 58 | color : str 59 | The color that will be used to plot the vertex 60 | marker : str 61 | The marker that will be used to plot the vertex 62 | markersize : int 63 | The size of the plotted vertex 64 | 65 | """ 66 | x, y = self.pose.position 67 | plt.plot(x, y, color=color, marker=marker, markersize=markersize) 68 | -------------------------------------------------------------------------------- /SLAM/GraphBasedSLAM/images/Graph_SLAM_optimization.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/SLAM/GraphBasedSLAM/images/Graph_SLAM_optimization.gif -------------------------------------------------------------------------------- /SLAM/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/SLAM/__init__.py -------------------------------------------------------------------------------- /SLAM/iterative_closest_point/iterative_closest_point.py: -------------------------------------------------------------------------------- 1 | """ 2 | Iterative Closest Point (ICP) SLAM example 3 | author: Atsushi Sakai (@Atsushi_twi), Göktuğ Karakaşlı 4 | """ 5 | 6 | import math 7 | 8 | import matplotlib.pyplot as plt 9 | import numpy as np 10 | 11 | # ICP parameters 12 | EPS = 0.0001 13 | MAX_ITER = 100 14 | 15 | show_animation = True 16 | 17 | 18 | def icp_matching(previous_points, current_points): 19 | """ 20 | Iterative Closest Point matching 21 | - input 22 | previous_points: 2D points in the previous frame 23 | current_points: 2D points in the current frame 24 | - output 25 | R: Rotation matrix 26 | T: Translation vector 27 | """ 28 | H = None # homogeneous transformation matrix 29 | 30 | dError = 1000.0 31 | preError = 1000.0 32 | count = 0 33 | 34 | while dError >= EPS: 35 | count += 1 36 | 37 | if show_animation: # pragma: no cover 38 | plt.cla() 39 | # for stopping simulation with the esc key. 40 | plt.gcf().canvas.mpl_connect('key_release_event', 41 | lambda event: [exit(0) if event.key == 'escape' else None]) 42 | plt.plot(previous_points[0, :], previous_points[1, :], ".r") 43 | plt.plot(current_points[0, :], current_points[1, :], ".b") 44 | plt.plot(0.0, 0.0, "xr") 45 | plt.axis("equal") 46 | plt.pause(0.1) 47 | 48 | indexes, error = nearest_neighbor_association(previous_points, current_points) 49 | Rt, Tt = svd_motion_estimation(previous_points[:, indexes], current_points) 50 | 51 | # update current points 52 | current_points = (Rt @ current_points) + Tt[:, np.newaxis] 53 | 54 | H = update_homogeneous_matrix(H, Rt, Tt) 55 | 56 | dError = abs(preError - error) 57 | preError = error 58 | print("Residual:", error) 59 | 60 | if dError <= EPS: 61 | print("Converge", error, dError, count) 62 | break 63 | elif MAX_ITER <= count: 64 | print("Not Converge...", error, dError, count) 65 | break 66 | 67 | R = np.array(H[0:2, 0:2]) 68 | T = np.array(H[0:2, 2]) 69 | 70 | return R, T 71 | 72 | 73 | def update_homogeneous_matrix(Hin, R, T): 74 | 75 | H = np.zeros((3, 3)) 76 | 77 | H[0, 0] = R[0, 0] 78 | H[1, 0] = R[1, 0] 79 | H[0, 1] = R[0, 1] 80 | H[1, 1] = R[1, 1] 81 | H[2, 2] = 1.0 82 | 83 | H[0, 2] = T[0] 84 | H[1, 2] = T[1] 85 | 86 | if Hin is None: 87 | return H 88 | else: 89 | return Hin @ H 90 | 91 | 92 | def nearest_neighbor_association(previous_points, current_points): 93 | 94 | # calc the sum of residual errors 95 | delta_points = previous_points - current_points 96 | d = np.linalg.norm(delta_points, axis=0) 97 | error = sum(d) 98 | 99 | # calc index with nearest neighbor assosiation 100 | d = np.linalg.norm(np.repeat(current_points, previous_points.shape[1], axis=1) 101 | - np.tile(previous_points, (1, current_points.shape[1])), axis=0) 102 | indexes = np.argmin(d.reshape(current_points.shape[1], previous_points.shape[1]), axis=1) 103 | 104 | return indexes, error 105 | 106 | 107 | def svd_motion_estimation(previous_points, current_points): 108 | pm = np.mean(previous_points, axis=1) 109 | cm = np.mean(current_points, axis=1) 110 | 111 | p_shift = previous_points - pm[:, np.newaxis] 112 | c_shift = current_points - cm[:, np.newaxis] 113 | 114 | W = c_shift @ p_shift.T 115 | u, s, vh = np.linalg.svd(W) 116 | 117 | R = (u @ vh).T 118 | t = pm - (R @ cm) 119 | 120 | return R, t 121 | 122 | 123 | def main(): 124 | print(__file__ + " start!!") 125 | 126 | # simulation parameters 127 | nPoint = 1000 128 | fieldLength = 50.0 129 | motion = [0.5, 2.0, np.deg2rad(-10.0)] # movement [x[m],y[m],yaw[deg]] 130 | 131 | nsim = 3 # number of simulation 132 | 133 | for _ in range(nsim): 134 | 135 | # previous points 136 | px = (np.random.rand(nPoint) - 0.5) * fieldLength 137 | py = (np.random.rand(nPoint) - 0.5) * fieldLength 138 | previous_points = np.vstack((px, py)) 139 | 140 | # current points 141 | cx = [math.cos(motion[2]) * x - math.sin(motion[2]) * y + motion[0] 142 | for (x, y) in zip(px, py)] 143 | cy = [math.sin(motion[2]) * x + math.cos(motion[2]) * y + motion[1] 144 | for (x, y) in zip(px, py)] 145 | current_points = np.vstack((cx, cy)) 146 | 147 | R, T = icp_matching(previous_points, current_points) 148 | print("R:", R) 149 | print("T:", T) 150 | 151 | 152 | if __name__ == '__main__': 153 | main() 154 | -------------------------------------------------------------------------------- /_config.yml: -------------------------------------------------------------------------------- 1 | theme: jekyll-theme-slate 2 | show_downloads: true 3 | -------------------------------------------------------------------------------- /appveyor.yml: -------------------------------------------------------------------------------- 1 | os: Visual Studio 2019 2 | 3 | environment: 4 | global: 5 | # SDK v7.0 MSVC Express 2008's SetEnv.cmd script will fail if the 6 | # /E:ON and /V:ON options are not enabled in the batch script intepreter 7 | # See: http://stackoverflow.com/a/13751649/163740 8 | CMD_IN_ENV: "cmd /E:ON /V:ON /C .\\appveyor\\run_with_env.cmd" 9 | 10 | matrix: 11 | - MINICONDA: C:\Miniconda38-x64 12 | PYTHON_VERSION: "3.8" 13 | 14 | init: 15 | - "ECHO %MINICONDA% %PYTHON_VERSION% %PYTHON_ARCH%" 16 | 17 | install: 18 | # If there is a newer build queued for the same PR, cancel this one. 19 | # The AppVeyor 'rollout builds' option is supposed to serve the same 20 | # purpose but it is problematic because it tends to cancel builds pushed 21 | # directly to master instead of just PR builds (or the converse). 22 | # credits: JuliaLang developers. 23 | - ps: if ($env:APPVEYOR_PULL_REQUEST_NUMBER -and $env:APPVEYOR_BUILD_NUMBER -ne ((Invoke-RestMethod ` 24 | https://ci.appveyor.com/api/projects/$env:APPVEYOR_ACCOUNT_NAME/$env:APPVEYOR_PROJECT_SLUG/history?recordsNumber=50).builds | ` 25 | Where-Object pullRequestId -eq $env:APPVEYOR_PULL_REQUEST_NUMBER)[0].buildNumber) { ` 26 | throw "There are newer queued builds for this pull request, failing early." } 27 | - ECHO "Filesystem root:" 28 | - ps: "ls \"C:/\"" 29 | 30 | # Prepend newly installed Python to the PATH of this build (this cannot be 31 | # done from inside the powershell script as it would require to restart 32 | # the parent CMD process). 33 | - SET PATH=%MINICONDA%;%MINICONDA%\\Scripts;%PATH% 34 | - SET PATH=%PYTHON%;%PYTHON%\Scripts;%PYTHON%\Library\bin;%PATH% 35 | - SET PATH=%PATH%;C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\bin 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/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | SPHINXPROJ = PythonRobotics 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) -------------------------------------------------------------------------------- /docs/README.md: -------------------------------------------------------------------------------- 1 | ## Python Robotics Documentation 2 | 3 | This folder contains documentation for the Python Robotics project. 4 | 5 | 6 | ### Build the Documentation 7 | 8 | #### Install Sphinx and Theme 9 | 10 | ``` 11 | pip install sphinx sphinx-autobuild sphinx-rtd-theme 12 | ``` 13 | 14 | #### Building the Docs 15 | 16 | In the `docs/` folder: 17 | ``` 18 | make html 19 | ``` 20 | 21 | if you want to building each time a file is changed: 22 | 23 | ``` 24 | sphinx-autobuild . _build/html 25 | ``` 26 | 27 | ### 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 | -------------------------------------------------------------------------------- /docs/_static/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/_static/.gitkeep -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | set SPHINXPROJ=PythonRobotics 13 | 14 | if "%1" == "" goto help 15 | 16 | %SPHINXBUILD% >NUL 2>NUL 17 | if errorlevel 9009 ( 18 | echo. 19 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 20 | echo.installed, then set the SPHINXBUILD environment variable to point 21 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 22 | echo.may add the Sphinx directory to PATH. 23 | echo. 24 | echo.If you don't have Sphinx installed, grab it from 25 | echo.http://sphinx-doc.org/ 26 | exit /b 1 27 | ) 28 | 29 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 30 | goto end 31 | 32 | :help 33 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% 34 | 35 | :end 36 | popd 37 | -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/FastSLAM1_files/FastSLAM1_12_0.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_12_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/FastSLAM1_files/FastSLAM1_12_1.png -------------------------------------------------------------------------------- /docs/modules/FastSLAM1_files/FastSLAM1_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/FastSLAM1_files/FastSLAM1_1_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_2_files/Kalmanfilter_basics_2_5_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_14_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_16_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_19_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_21_1.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_22_0.png -------------------------------------------------------------------------------- /docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Kalmanfilter_basics_files/Kalmanfilter_basics_28_1.png -------------------------------------------------------------------------------- /docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_12_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/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/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/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/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/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/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/Planar_Two_Link_IK_files/Planar_Two_Link_IK_9_0.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/cgmres_nmpc_files/cgmres_nmpc_1_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/cgmres_nmpc_files/cgmres_nmpc_2_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/cgmres_nmpc_files/cgmres_nmpc_3_0.png -------------------------------------------------------------------------------- /docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/cgmres_nmpc_files/cgmres_nmpc_4_0.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /docs/modules/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/extended_kalman_filter_localization_files/extended_kalman_filter_localization_1_0.png -------------------------------------------------------------------------------- /docs/modules/introduction.rst: -------------------------------------------------------------------------------- 1 | 2 | Introduction 3 | ============ 4 | 5 | Ref 6 | --- 7 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /docs/modules/rrt_star_files/rrt_star_1_0.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/docs/modules/rrt_star_files/rrt_star_1_0.png -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /environment.yml: -------------------------------------------------------------------------------- 1 | name: python_robotics 2 | dependencies: 3 | - python 4 | - pip 5 | - matplotlib 6 | - scipy 7 | - numpy 8 | - pandas 9 | - coverage 10 | - pip: 11 | - cvxpy 12 | -------------------------------------------------------------------------------- /icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/icon.png -------------------------------------------------------------------------------- /mypy.ini: -------------------------------------------------------------------------------- 1 | [mypy] 2 | ignore_missing_imports = True -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pandas 3 | scipy 4 | matplotlib 5 | cvxpy 6 | -------------------------------------------------------------------------------- /rundiffstylecheck.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | echo "$(basename $0) start!" 3 | VERSION=v0.1.6 4 | wget https://github.com/AtsushiSakai/DiffSentinel/archive/${VERSION}.zip 5 | unzip ${VERSION}.zip 6 | ./DiffSentinel*/starter.sh HEAD origin/master 7 | check_result=$? 8 | rm -rf ${VERSION}.zip DiffSentinel* 9 | if [[ ${check_result} -ne 0 ]]; 10 | then 11 | echo "Error: Your changes contain pycodestyle errors." 12 | exit 1 13 | fi 14 | echo "$(basename $0) done!" 15 | exit 0 16 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /tests/.gitkeep: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LiHongbo97/PythonRobotics/3607d72b60cd500806e0f026ac8beb82850a01f9/tests/.gitkeep -------------------------------------------------------------------------------- /tests/test_LQR_planner.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from unittest import TestCase 3 | 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_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_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_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_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_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_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 | -------------------------------------------------------------------------------- /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_drone_3d_trajectory_following.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(__file__) + "/../AerialNavigation/drone_3d_trajectory_following/") 6 | 7 | import drone_3d_trajectory_following as m 8 | 9 | print(__file__) 10 | 11 | 12 | class Test(TestCase): 13 | 14 | def test1(self): 15 | m.show_animation = False 16 | m.main() 17 | -------------------------------------------------------------------------------- /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 | @staticmethod 11 | def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw): 12 | assert (abs(px[0] - start_x) <= 0.01) 13 | assert (abs(py[0] - start_y) <= 0.01) 14 | assert (abs(pyaw[0] - start_yaw) <= 0.01) 15 | print("x", px[-1], end_x) 16 | assert (abs(px[-1] - end_x) <= 0.01) 17 | print("y", py[-1], end_y) 18 | assert (abs(py[-1] - end_y) <= 0.01) 19 | print("yaw", pyaw[-1], end_yaw) 20 | assert (abs(pyaw[-1] - end_yaw) <= 0.01) 21 | 22 | def test1(self): 23 | start_x = 1.0 # [m] 24 | start_y = 1.0 # [m] 25 | start_yaw = np.deg2rad(45.0) # [rad] 26 | 27 | end_x = -3.0 # [m] 28 | end_y = -3.0 # [m] 29 | end_yaw = np.deg2rad(-45.0) # [rad] 30 | 31 | curvature = 1.0 32 | 33 | px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( 34 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 35 | 36 | self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw) 37 | 38 | def test2(self): 39 | dubins_path_planning.show_animation = False 40 | dubins_path_planning.main() 41 | 42 | def test3(self): 43 | N_TEST = 10 44 | 45 | for i in range(N_TEST): 46 | start_x = (np.random.rand() - 0.5) * 10.0 # [m] 47 | start_y = (np.random.rand() - 0.5) * 10.0 # [m] 48 | start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 49 | 50 | end_x = (np.random.rand() - 0.5) * 10.0 # [m] 51 | end_y = (np.random.rand() - 0.5) * 10.0 # [m] 52 | end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 53 | 54 | curvature = 1.0 / (np.random.rand() * 5.0) 55 | 56 | px, py, pyaw, mode, clen = dubins_path_planning.dubins_path_planning( 57 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 58 | 59 | self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw) 60 | -------------------------------------------------------------------------------- /tests/test_dynamic_window_approach.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(__file__) + "/../") 6 | try: 7 | from PathPlanning.DynamicWindowApproach import dynamic_window_approach as m 8 | except ImportError: 9 | raise 10 | 11 | print(__file__) 12 | 13 | 14 | class TestDynamicWindowApproach(TestCase): 15 | def test_main1(self): 16 | m.show_animation = False 17 | m.main(gx=1.0, gy=1.0) 18 | 19 | def test_main2(self): 20 | m.show_animation = False 21 | m.main(gx=1.0, gy=1.0, robot_type=m.RobotType.rectangle) 22 | 23 | 24 | if __name__ == '__main__': # pragma: no cover 25 | test = TestDynamicWindowApproach() 26 | test.test_main1() 27 | test.test_main2() 28 | -------------------------------------------------------------------------------- /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_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_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_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_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_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_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 | -------------------------------------------------------------------------------- /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_hybrid_a_star.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | import sys 3 | import os 4 | sys.path.append(os.path.dirname(__file__) + "/../") 5 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 6 | + "/../PathPlanning/HybridAStar") 7 | try: 8 | from PathPlanning.HybridAStar import hybrid_a_star as m 9 | except Exception: 10 | raise 11 | 12 | 13 | class Test(TestCase): 14 | 15 | def test1(self): 16 | m.show_animation = False 17 | m.main() 18 | 19 | 20 | if __name__ == '__main__': # pragma: no cover 21 | test = Test() 22 | test.test1() 23 | -------------------------------------------------------------------------------- /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_inverted_pendulum_mpc_control.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | 3 | import sys 4 | if 'cvxpy' in sys.modules: # pragma: no cover 5 | sys.path.append("./InvertedPendulumCart/inverted_pendulum_mpc_control/") 6 | 7 | import inverted_pendulum_mpc_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 | -------------------------------------------------------------------------------- /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_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_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_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_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_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 | -------------------------------------------------------------------------------- /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_n_joint_arm_to_point_control.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | from unittest import TestCase 4 | 5 | sys.path.append(os.path.dirname(__file__) + "/../ArmNavigation/n_joint_arm_to_point_control/") 6 | 7 | import n_joint_arm_to_point_control as m 8 | 9 | print(__file__) 10 | 11 | 12 | class Test(TestCase): 13 | 14 | def test1(self): 15 | m.show_animation = False 16 | m.animation() 17 | -------------------------------------------------------------------------------- /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_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_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_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_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_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_reeds_shepp_path_planning.py: -------------------------------------------------------------------------------- 1 | from unittest import TestCase 2 | from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as m 3 | 4 | import numpy as np 5 | 6 | 7 | class Test(TestCase): 8 | 9 | @staticmethod 10 | def check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw): 11 | assert (abs(px[0] - start_x) <= 0.01) 12 | assert (abs(py[0] - start_y) <= 0.01) 13 | assert (abs(pyaw[0] - start_yaw) <= 0.01) 14 | print("x", px[-1], end_x) 15 | assert (abs(px[-1] - end_x) <= 0.01) 16 | print("y", py[-1], end_y) 17 | assert (abs(py[-1] - end_y) <= 0.01) 18 | print("yaw", pyaw[-1], end_yaw) 19 | assert (abs(pyaw[-1] - end_yaw) <= 0.01) 20 | 21 | def test1(self): 22 | m.show_animation = False 23 | m.main() 24 | 25 | def test2(self): 26 | N_TEST = 10 27 | 28 | for i in range(N_TEST): 29 | start_x = (np.random.rand() - 0.5) * 10.0 # [m] 30 | start_y = (np.random.rand() - 0.5) * 10.0 # [m] 31 | start_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 32 | 33 | end_x = (np.random.rand() - 0.5) * 10.0 # [m] 34 | end_y = (np.random.rand() - 0.5) * 10.0 # [m] 35 | end_yaw = np.deg2rad((np.random.rand() - 0.5) * 180.0) # [rad] 36 | 37 | curvature = 1.0 / (np.random.rand() * 5.0) 38 | 39 | px, py, pyaw, mode, clen = m.reeds_shepp_path_planning( 40 | start_x, start_y, start_yaw, end_x, end_y, end_yaw, curvature) 41 | 42 | self.check_edge_condition(px, py, pyaw, start_x, start_y, start_yaw, end_x, end_y, end_yaw) 43 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | 6 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 7 | + "/../PathPlanning/RRTStarDubins/") 8 | 9 | try: 10 | import rrt_star_dubins as m 11 | except: 12 | raise 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_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_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_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 | -------------------------------------------------------------------------------- /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_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_visibility_road_map_planner.py: -------------------------------------------------------------------------------- 1 | import sys 2 | import os 3 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 4 | + "/../PathPlanning/VoronoiRoadMap/") 5 | 6 | from unittest import TestCase 7 | from PathPlanning.VoronoiRoadMap import voronoi_road_map as m 8 | 9 | 10 | print(__file__) 11 | 12 | 13 | class Test(TestCase): 14 | 15 | def test1(self): 16 | m.show_animation = False 17 | m.main() 18 | -------------------------------------------------------------------------------- /tests/test_voronoi_road_map_planner.py: -------------------------------------------------------------------------------- 1 | import os 2 | import sys 3 | 4 | sys.path.append(os.path.dirname(os.path.abspath(__file__)) 5 | + "/../PathPlanning/VisibilityRoadMap/") 6 | 7 | from unittest import TestCase 8 | from PathPlanning.VisibilityRoadMap import visibility_road_map as m 9 | 10 | print(__file__) 11 | 12 | 13 | class Test(TestCase): 14 | 15 | def test1(self): 16 | m.show_animation = False 17 | m.main() 18 | --------------------------------------------------------------------------------